Autonomous.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S2,     gyro,                sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     htproto,             sensorI2CCustom9V)
#pragma config(Motor,  motorA,          rightArmSpin,  tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          leftArmSpin,   tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     rightDrive,    tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     armClaw,       tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     armWrist,      tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     ballCollector, tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     leftDrive,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     armBase,       tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define GYRO S2
#include "autonoLib.c"

#include "JoystickDriver.c"

string Alliances[2] = { "Red", "Blue" };
string RampLocations[2] = { "Left", "Right" };


bool leftPressed()
{
	static int cyclesUnpressed = 0;

	if ( nNxtButtonPressed == kLeftButton && cyclesUnpressed > 2000 )
  {
    	cyclesUnpressed = 0;
		  return true;
  }
	else if ( cyclesUnpressed < 4000 ) cyclesUnpressed++;

	return false;
}

bool rightPressed()
{
	static int cyclesUnpressed = 0;

	if ( nNxtButtonPressed == kRightButton && cyclesUnpressed > 2000 )
    {
    	cyclesUnpressed = 0;
		return true;
    }
	else if ( cyclesUnpressed < 4000 ) cyclesUnpressed++;

	return false;
}

int getAllianceFromUser()
{
  int side = 0;
  if ( leftPressed() )
    side = 1;
  else if ( rightPressed() )
    side = 0;
  nxtDisplayString(0, "Alliance: %s", Alliances[side]);
  return side;
}

int getSideOfRampFromUser()
{
  int ramp = 0;
  if ( leftPressed() )
    ramp = 1;
  else if ( rightPressed() )
    ramp = 0;
  nxtDisplayString(0, "Ramp Location: %s", Alliances[side]);
  return ramp;
}

int getValueFromUser(string s = "Value", int max = 10, int min = 0)
{
	int m_val = min;
	while(1)
	{
		nxtDisplayString(0, "%s: %i", s, m_val);
		if ( rightPressed() )
			m_val++;
		else if ( leftPressed() )
			m_val--;

    if ( m_val => max ) m_val = max;
    else if ( m_val <= min ) m_val = min;

		if ( nNxtButtonPressed == kEnterButton )
			return m_val;
	}
}


task main()
{
  int alliance  = getAllianceFromUser();
  wait1Msec(800);
  int rampSide = getSideOfRampFromUser();
  wait1Msec(800);
  int delay = getValueFromUser("Delay");

  wait1Msec(delay*1000);

  initGyro();
  waitForStart();
  wait1Msec(delay*1000);
  switch ( autoID )
  {
    case 1:
      frontPark();
      break;
    case 2:
      redFarPark();
      break;
    case 3:
      bluFarPark();
      break;
  }

  while(1) continue;
}