Beispiel #1
0
int main(void) {

	setup();

	int cycles = 2;
	int cycle = 0;
	bool direction = false;

	uint32_t distance = 100;
	uint16_t fSpeed = 60;
	uint16_t rSpeed = 35;

	while (1) {
		if (cycles < 0 || cycle < cycles) {
			if (!left_running && !right_running) {
				delay();
				direction = !direction;

				if (direction) {
//					leftWheel(direction, lSpeed, distance);
//					rightWheel(direction, rSpeed, distance);
					runCommand( FORWARD, fSpeed, distance / 2);
					delay();
					runCommand(LEFT_FWD, fSpeed, distance / 3);
					delay();
					runCommand( FORWARD, fSpeed, distance / 2);
					delay();
					runCommand(LEFT_FWD, fSpeed, distance / 3);
					delay();
					runCommand( FORWARD, fSpeed, distance / 2);
					delay();
					runCommand(LEFT_FWD, fSpeed, distance / 3);
					delay();
					runCommand( FORWARD, fSpeed, distance / 2);
					delay();
					allStop();
				} else {
//					leftWheel(direction, lSpeed / 2, distance);
//					rightWheel(direction, rSpeed / 2, distance);
					runCommand( REVERSE, rSpeed, distance);
					delay();
					runCommand(RIGHT_FWD, rSpeed, distance / 4);
					delay();
					runCommand( REVERSE, rSpeed, distance);
					delay();
					runCommand(RIGHT_FWD, rSpeed, distance / 4);
					delay();
					runCommand( REVERSE, rSpeed, distance);
					delay();
					runCommand(RIGHT_FWD, rSpeed, distance / 4);
					delay();
					runCommand( REVERSE, rSpeed, distance);
					delay();
					allStop();
				}
				cycle++;
			}
		}
	}
}
Beispiel #2
0
int main(void) {

	setup();

	int cycles = 2;
	int cycle = 0;
	bool direction = false;

	uint32_t distance = 1000;
	uint16_t fSpeed = 80;
	uint16_t rSpeed = 50;

	while (1) {
		if (cycles < 0 || cycle < cycles) {
			if (!left_running && !right_running) {
				delay();
				direction = !direction;

				if (direction) {
					runCommand( FORWARD, fSpeed, distance);
					allStop();
					delay();
				} else {
					runCommand( REVERSE, rSpeed, distance);
					allStop();
				}
				cycle++;
			}
		}
	}
}
Beispiel #3
0
task main()
{
	phase = AUTO;
	driveCycles = 6;
	humor = 80;
	bDisplayDiagnostics = false;
	initializeAutonomous();
	bDisplayDiagnostics = true;
	waitForStart();
	motor [lift] = 100;
	wait1Msec(750);
	motor [lift] = 0;
	wait1Msec(250);
	while (nMotorEncoder [frontRight] > -(driveCycles * driveEncoderCycle))
	{
		drive(0, -25, 0);
	}
	while (SensorValue [sonar] > 50)
	{
		drive(0, 0, 25);
	}
	wait1Msec(550);
	allStop();
	StartTask(failSafe);
	while (SensorValue [sonar] > 20)
	{
		drive(0, -20, 0);
	}
	StopTask(failSafe);
	wait1Msec(500);
	allStop();
	servo [leftHook] = 168;
	servo [rightHook] = 16;
	wait1Msec(50);
	score(60);
	wait1Msec(200);

	drive(0, 0, -100);
	motor [arm] = 100;
	servo [pivot] = 245;
	wait1Msec(1000);
	motor [arm] = 0;
	wait1Msec(500);
	allStop();
	wait1Msec(50);
	drive(-100, 0, 0);
	wait1Msec(1700);
	drive(0, -100, 0);
	wait1Msec(4500);
	allStop();
}
task main()
{
	waitForStart();
	init();

	intakeOn(2);

	raiseCube(4.5);

	reverse(100,.7);

	dropCube();
	wait1Msec(1000);
	cubeUp();

	forward(100,.5);

	right(100,1.5);
	reverse(100,1);


	left(100,1);
	raiseArm(1);
	reverse(100,.5);
	left(100,1.5);
	reverse(100,2);
	allStop();
}
// If there is a radio packet, read and execute it. send info packets if appropriate
void DashBot::dashPacketHandler(void){
  if(readRadioPacket()) { 
    executeRadioCommand();
  }
  debugBlinkOff();
  
  //if in gyro-assisted mode, update controller and allow it to change the motor settings
  if(mode == GYRO_MODE) {
    dashRun(power, -heading);
  }
  
  //if in automatic info packet transmission mode is enabled, send an info packet
  if(millis() - infoPacketTime > delay_between_sensor_emissions)
  {
    infoPacketTime = millis();
    //if (infoPacketTransmissionMode == 1)
    sendInfoPacket();
    //else if (infoPacketTransmissionMode == 2)
    //  sendNamePacket();
    //else if (infoPacketTransmissionMode == 3)
    //sendMessagePacket();
  }
  
  //if there's not been a packet for the last 1/2 second, call an all-stop
  if(millis() > lastPacketTime + 500)
  {
    allStop();
  }
}
Beispiel #6
0
task main()
{
	driveCycles = 6;
	humor = 80;
	bDisplayDiagnostics = false;
	initializeAutonomous();
	bDisplayDiagnostics = true;
	waitForStart();


	motor [lift] = 100;
	wait1Msec(750);
	motor [lift] = 0;
	wait1Msec(250);
	if (SensorValue [ir] == 5)
	{
		//while (nMotorEncoder [frontRight] > -(driveCycles * driveEncoderCycle))
		//{
		//	drive(0, 25, 0);
		//}
		while (SensorValue [sonar] > 45)
		{
			drive(0, 100, 0);
		}
		allStop();
		//begin precision rotate cycle
		while (SensorValue [ir] > 4)
		{
			drive(0, 0, 60);
		}
		allStop();
		ClearTimer(T2);
		while (SensorValue [ir] < 6)
		{
			drive(0, 0, -60);
		}
		int spinTime = time1[T2];
		allStop();
		ClearTimer(T2);
		while (time1[T2] < spinTime)
		{
			drive(0, 0, 60);
		}
		allStop();
	}
}
void post3()
{
  motor[motorJ]=-100;
motor[motorK]=-100;
wait10Msec(200);
allStop();

}
Beispiel #8
0
void Engines::disarm(){
  if (!_armed) return;
  
  setThrottle(0);
  allStop();
  
  _armed = false;
}
void post2()
{

motor[motorD]=-100;
motor[motorE]=100;
wait10Msec(60);
allStop();

}
Beispiel #10
0
task main() {
	StartTask(USNEW);
	StartTask(USOLD);
	StartTask(pAxies);
	MSSUMOsetShortRange(HTMSSUMO);

	while(true)

  	{
  	runSensors();

  		nxtDisplayCenteredBigTextLine(1, "X: %d", xA);
	  	nxtDisplayCenteredBigTextLine(3, "Y: %d", yA);
	  	nxtDisplayCenteredBigTextLine(5, "Z: %d", zA);
	  	zone = MSSUMOreadZone(HTMSSUMO);




	  	switch (zone)
	    	{
		      case MSSUMO_FRONT:
		      	allStop();
		      	break;
		      case MSSUMO_LEFT:
		      	turnRight();
		      	break;
		      case MSSUMO_RIGHT:
		      	turnLeft();
		      	break;
		      case MSSUMO_NONE:
		      	allGo();
		      	break;
	   	 	}
	   	if ((us_NewResult < 14 ) || ( us_OldResult < 14 ))
	   		{
	  			sonarObsticalCheck(1);
	  		}
	  	if ( xA < -25)
	  		{
		  		allStall();
					wait1Msec(500);
					allBack();
					wait1Msec(1000);
					sonarObsticalCheck(3);
	  		}
	  	if ( yA < -25)
	  		{
		  		allStall();
					wait1Msec(500);
					allBack();
					wait1Msec(1000);
					sonarObsticalCheck(3);
	  		}
  	}
}
Beispiel #11
0
bool checkConnection()
{
	nNoMessageCounterLimit = 250; //4ms per check (3 seconds after disconnect)
	if(bDisconnected == 1)
	{
		allStop();
		return 1;
	}
	return 0;
}
task raiseLiftWhile(){
  raiseLift(100);
  time1[T1]=0;
	while (nMotorEncoder[intake] < 415 && time1[T1] < 2800) //while the encoder wheel turns one revolution
  {
  	times = time1[T1];
  }
	allStop();
	wait1Msec(500);
	if(time1[T1]>2500)
		StopAllTasks();
}
task main()
{
	waitForStart();



	forward(100,1);
	init();
	intakeOn(2);
	left(100,.5);
	raiseArm(1);
	forward(100,1.8);
	allStop();
}
task newi() //////////////////////////////////////THIS IS NEW STUFFFF.
{
  //motorD drive
//motorE drive
//motorH lift
//motorI lift
//motorJ traverse
//motorK traverse
motor[motorD]=100;
motor[motorE]=100;
wait10Msec(#);
allStop();
motor[motorJ]=100;
motor[motorK]=100;
wait10Msec(#);
allStop();
motor[motorD]=100;
motor[motorE]=100;
wait10Msec(#);
allStop();
motor[motorD]=-100;
motor[motorE]=100;
allStop();
}
Beispiel #15
0
void Motors::setMotorSpeed(const byte motor, float speed)
{
    //Mapping of the controller speed to the ESC speed
    speed = (speed * 2) + MIN_MOTOR_SPEED_PWM - 6;

    //If the speed command is too high we just shut down all the motors
    //It might not be the best solution but it's at least safer for testing
    if (speed > 260)
    {
        allStop();
        Serial.print("Motor speed superior than 260. ALL MOTORS STOPPED");
        while(1);
    }

    analogWrite(motors[motor-1], speed*motorsOn);
    motor_speeds[motor-1] = speed;
}
MotorTest::MotorTest(QWidget *parent) : Page(parent)
{
    int i;
    setupUi(this);

    m_cbobData = CbobData::instance();

    QObject::connect(m_cbobData, SIGNAL(eStop()), this, SLOT(allStop()));
    QObject::connect(ui_ClearButton, SIGNAL(pressed()), this, SLOT(resetMotorCounter()));

    m_motorNumber = 0;

    for(i=0;i<4;i++){
                m_targetPower[i] = 0;
                m_targetSpeed[i] = 0;
                m_targetPosition[i] = 0;
                m_controlState[i] = 2;
                m_playState[i] = false;
            }
}
Beispiel #17
0
task main(){

	init(); //Initiate pieces
	
	WaitForStart(); //Wait for the FCS to say go
	
	StartTask(joystickOne); //Start joystick polling tasks
	StartTask(joystickTwo);
	
	while(true){ //Main execution loop
		if(DEBUGMODE){DEBUG();}
		
		if(bDisconnected){ //Stop Robot if disconnected
			allStop();
			continue;
        }
		updateSensors();
		//Place any autonomous executions during teleop here
	}
}
Beispiel #18
0
task main()
{
    initializeRobot();
    waitForStart();
    //drive(0, 100, 0);
    //wait1Msec(1000);
    while (SensorValue [sonarSensor] > blockPlacementDist)
    {
        if (SensorValue [irSensor] < 5) {
            drive(-50, 50, 0);
        }
        else if (SensorValue [irSensor] > 5) {
            drive(50, 50, 0);
        }
        else if (SensorValue [irSensor] == 5) {
            drive(0, 50, 0);
        }
    }
    allStop();
    placeBlock();
}
Beispiel #19
0
task main()
{
    init();/*
	float wait = 0.0;
	while(nNxtButtonPressed!=1){ //Choose drive time
		nxtDisplayCenteredTextLine(1,"Driving for:%fs",wait);
		if(nNxtButtonPressed==0)
			wait-=.5;
		else if(nNxtButtonPressed==2)
			wait+=.5;
		if(wait<0)
			wait=0;
	}
*/
    waitForStart();

    forward(100);
    wait1Msec(1500);
    allStop();
    wait1Msec(500);
}
Beispiel #20
0
void stopRobot(void) {
	int sign;
	float speed;

	sign = sign(control.speeds.angular_speed);
	speed = abs(control.speeds.angular_speed);
	speed -= control.max_acc * DT;
	speed = max(0, speed);
	control.speeds.angular_speed = speed;

	sign = sign(control.speeds.linear_speed);
	speed = abs(control.speeds.linear_speed);
	speed -= control.max_acc * DT;
	speed = max(0, speed);
	control.speeds.linear_speed = sign*speed;

	if (abs(wheels_spd.left) + abs(wheels_spd.right) < SPD_TO_STOP) {
		allStop();
	} else {
		applyPID();
	}
}
Beispiel #21
0
//Lets Get this thing moving!
task main() {
	// Start up the two sonar sensors and their config files
	StartTask(USNEW);
	StartTask(USOLD);
  //Start up the IDMU config
	StartTask(pAxies);
	// Set Sumo to Long Range
	MSSUMOsetShortRange(HTMSSUMO);
  while(true) {
  	zone = MSSUMOreadZone(HTMSSUMO);
    // Check front IR sensor LEFT FRONT RIGHT
    switch (zone) {
      case MSSUMO_FRONT:
      	allStop();
      	break;
      case MSSUMO_LEFT:
      	turnRight();
      	break;
      case MSSUMO_RIGHT:
      	turnLeft();
      	break;
      case MSSUMO_NONE:
      	allGo();
      	break;
    }
  	// Check left and right sonar sensors for objects
   	if ((us_NewResult < 12 ) || ( us_OldResult < 12 )){
  		sonarObsticalCheck(1);
  		} else if ( ( yA > 35 ) || ( xA > 35 ) ){
  		allStall();
			wait1Msec(500);
			allBack();
			wait1Msec(1000);
			sonarObsticalCheck(3);
		//checkLevel();
  		}
  }
}
Beispiel #22
0
YaesuRotor::YaesuRotor()
	: rotorMoveUpdateInterval(100UL), rotatingAzimuth(false), rotatingElevation(false) {

	// TODO check for saved config

	// Rotor config defaults
	config.configPresentFlag = 0xFF;
	config.azimuthaAdZeroOffset = 0;
	config.elevationAdZeroOffset = 0;
	config.bias = 1 * SCALE_FACTOR;

	// Initialise digital pins for output.
	pinMode(PIN_EL_UP, OUTPUT);
	pinMode(PIN_EL_DOWN, OUTPUT);
	pinMode(PIN_AZ_LEFT, OUTPUT);
	pinMode(PIN_AZ_RIGHT, OUTPUT);

	allStop();

	// Get current position and set target to same value to prevent unnecessary rotation on startup.
	targetAzimuth = getCurrentAzimuth();
	targetElevation = getCurrentElevation();
}
Beispiel #23
0
/**
 Constructor establishes values for all motor pins
 and sets all pins to output mode.  allStop() is
 called to prevent premature motion.
 **/
HMotor::HMotor(){
	
	rightSideMotor[0] = 7; // A side
	rightSideMotor[1] = 8; // B side
	rightSidePWM = 5;
	leftSideMotor[0] = 4; // A side
	leftSideMotor[1] = 9; // B side
	leftSidePWM = 6;
	rightSideCurrent = A2;
	leftSideCurrent = A3;
	rightSideEnable = A0;
	leftSideEnable = A1;
	
	pinMode(rightSideMotor[0], OUTPUT);
	pinMode(rightSideMotor[1], OUTPUT);
	pinMode(rightSidePWM, OUTPUT);
	pinMode(leftSideMotor[0], OUTPUT);
	pinMode(leftSideMotor[1], OUTPUT);
	pinMode(leftSidePWM, OUTPUT);
	
	allStop();
	
}
Beispiel #24
0
void _ISR _ADCInterrupt(void)
{
    int i, thresholdMet = 1, threshold = 10000;    //  May need to adjust threshold
    adcdata_t rawData[M_SLIDING_DFT_nchannels];
    
    //  TODO ADCBUF1, ADCBUF2, and ADCBUF3 may need to be swapped here
    rawData[0] = ADCBUF1;                          //  Copy ADC data
    rawData[1] = ADCBUF2;                          //  Copy ADC data
    rawData[2] = ADCBUF3;                          //  Copy ADC data
    dft_update(dft, rawData);                      //  Propagate the DFT
    
    for (i = 0; i < M_SLIDING_DFT_nchannels; i ++) //  For all channels:
        thresholdMet &= (dft->mag[i] > threshold); //  Check for threshold
    
    if (thresholdMet) {                            //  If we are hearing a ping:
        float d10 = dft_relativePhase(dft, 0, 1);  //  TDOA 0/1
        float d20 = dft_relativePhase(dft, 0, 2);  //  TDOA 0/2
        steerToward(d10, d20);                     //  Steer car
    } else
        allStop();                                 //  Otherwise, halt car
    
    IFS0bits.ADIF = 0;                             //  Clear interrupt bit
}
Beispiel #25
0
task main()
{
	initializeRobot();
	waitForStart(); // Wait for the beginning of autonomous phase.
	servo[handJoint] = 240;
	wait1Msec(100);

	black_threshold = LSvalNorm(middle_light);
	white_threshold = black_threshold + 5;

	writeDebugStreamLine("black_threshold: %d", black_threshold);
	writeDebugStreamLine("white_threshold: %d", white_threshold);

	//
	// Move to other side of field
	//

	moveStraight(50,2000);

	move(-50,50,450);

	moveStraight(50,500);

	//
	// Find the line
	//
	// Continue forward until the middle sensor detects the white line
	//
	writeDebugStreamLine("middle light sensor value going into first loop: %d" , LSvalNorm(middle_light));

	while (LSvalNorm(middle_light) < white_threshold){

		//!--Code for debugging
		middle_nrm = LSvalNorm(middle_light);
		if (middle_nrm != temp_middle_nrm) {
			writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
			temp_middle_nrm = middle_nrm;
		}
		//--!

		setAllMotorVals(10);
	}
	allStop();
	wait1Msec(100);

	//
	// Turn 90 degrees
	//
	// Move forward a little more and then turn until the middle sensor detects the white line
	//

	bool isLeft = false; // If this boolean is true, the 90 degree turn is toward the left

	moveStraight(30,350);
	//moveStraight(15,1000); // with long arm

	// Make a variable that counts the number of times through the loop. If it goes above a certain
	// threshold, assume the robot is stuck on the edge of the wood and increase the power
	int counter1 = 0;

	while (LSvalNorm(middle_light) < white_threshold){

		//!--Code for debugging
		middle_nrm = LSvalNorm(middle_light);
		if (middle_nrm != temp_middle_nrm) {
			writeDebugStreamLine("left light sensor value: %d", middle_nrm);
			temp_middle_nrm = middle_nrm;
		}
		//--!

		if (isLeft){
			if (counter1 > 500){
				setDriveMotorVals(40,-40);
			}
			else{
				setDriveMotorVals(20,-20);
			}
		}
		else{
			if (counter1 > 100){
				setDriveMotorVals(-40,40);
			}
			else{
				setDriveMotorVals(-20,20); // with long arm
			}
		}
		counter1 += 1;
	}

	//
	// Follow the line until touch sensor is triggered
	//
	//	1. Left on white, middle on white (or black), right on black: turn toward the left
	//	2. Left on black, middle on white (or black), right on white: turn toward the right
	//	3. Left on black, middle on white, right on black: move straight
	//	4. Left on black, middle on black, right on black: lost...
	//

	// Set up touch sensor variables
	int nButtonsMask;
	bool isTouch = false; // Boolean indicating if a touch sensor has been pressed.
												// If one has, this changes to true.

	// Make a variable that counts the number of times through the loop. If it goes above a certain
	// threshold, assume the robot is stuck on the edge of the wood and increase the power
	int counter2 = 0;

	// Initial check of the touch sensor values. If a robot is pushed up against the spear,
	// do not deploy it.
	nButtonsMask = SensorValue[S3];

	if (nButtonsMask & 0x01)
		isTouch = true;
	if (nButtonsMask & 0x02)
		isTouch = true;

	if (isTouch == false){
		//deploy the spear
		deploySpear(true);
	}

	while (isTouch == false){

		writeDebugStreamLine("-----------------counter2 val: %d", counter2);

		// Read light sensor values
		left_nrm = LSvalNorm(left_light);
		middle_nrm = LSvalNorm(middle_light);
		right_nrm = LSvalNorm(right_light);

		//!--Code for debugging
		//if (left_nrm != temp_left_nrm) {
			writeDebugStreamLine("left light sensor value: %d", left_nrm);
			//temp_left_nrm = left_nrm;
		//}
		//if (middle_nrm != temp_middle_nrm) {
			writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
			//temp_middle_nrm = middle_nrm;
		//}
		//if (right_nrm != temp_right_nrm) {
			writeDebugStreamLine("middle light sensor value: %d", right_nrm);
			//temp_right_nrm = right_nrm;
		//}
		//--!

		if (left_nrm < black_threshold){
			// left sensor is black

			if (middle_nrm < black_threshold){
				// middle sensor is black

				if (right_nrm > black_threshold){
					// right sensor is white or gray
					// turn right
					writeDebugStreamLine("Case 1: turned right");

					if (counter2 > 600){
						setDriveMotorVals(0,40);
					}
					else{
						setDriveMotorVals(0,30);
					}
				}
				else{
					// right sensor is black
					// lost, so just turn right a lot

					setDriveMotorVals(-30,30);
				}
			}

			else{
				// middle sensor is not black

				if (middle_nrm > white_threshold){
					// middle sensor is white
					// assume right sensor is black
					// move straight

					writeDebugStreamLine("Case 2: went straight");

					if (counter2 > 600){
						setAllMotorVals(40);
					}

					else{
						setAllMotorVals(30);
					}
				}
				else{
					// middle sensor is gray
					// assume right sensor is gray
					// turn right

					writeDebugStreamLine("Case 3: turned right");

					if (counter2 > 600){
						setDriveMotorVals(0,40);
					}
					else{
						setDriveMotorVals(0,30);
					}
				}
			}
		}
		else{
			// left sensor is not black

			if (left_nrm > white_threshold){
				// left sensor is white
				// assume middle sensor is black
				// assume the right sensor is black too
				// turn left

				writeDebugStreamLine("Case 4: turned left");

				if (counter2 > 600){
					setDriveMotorVals(40,0);
				}
				else{
					setDriveMotorVals(30,0);
				}
			}
			else{
				// left sensor is gray
				// middle sensor is gray
				// assume right sensor is black
				// turn left

				writeDebugStreamLine("Case 5: turned left");

				if (counter2 > 600){
					setDriveMotorVals(40,0);
				}
				else{
					setDriveMotorVals(30,0);
				}
			}
		}

		counter2 += 1;

		// Read touch sensor values
		nButtonsMask = SensorValue[S3];

		if (nButtonsMask & 0x01)
			isTouch = true;
		if (nButtonsMask & 0x02)
			isTouch = true;
	}
	allStop();
	wait1Msec(1000);

	//
	// More later....scoring stuff
	//
	//	1. Move backward a little
	//	2. Unfold arm
	//	3. Move forward a little
	//	4. Move arm down to score
	//	5. Back up
	//	6. Reset arm
	//	7. Move toward our rings?
	//


	// back up a little
	moveStraight(-20,800);

	// unfold arm
	fold_arm(false);

	// move forward a little
	moveStraight(20,600);

	// move arm down
	motor[shoulderJoint] = -50;
	wait1Msec(800);
	motor[shoulderJoint] = 0;

	// back up
	moveStraight(-40,500);

	// reset arm
	fold_arm(true);

	//retract spear
	//deploySpear(false);

	while (true){
		return;
	}
}
Beispiel #26
0
task main()
{
	init();
	waitForStart();
	driveSonar(1,25,80);
	allStop();
	wait1Msec(100);
	backward(20);
	wait1Msec(700);
	sticksDown();
	wait1Msec(250);
	allStop();
	wait1Msec(100);
  	raiseLift(100);
  	time1[T1]=0;
  	while (nMotorEncoder[intake] < 410 && time1[T1] < 2500) //while the encoder wheel turns one revolution
  	{
  		times = time1[T1];
  	}
	allStop();
	wait1Msec(500);
	//if(time1[T1]>2500)
	//	while(true){
	//		nxtDisplayCenteredBigTextLine(1,":(");
	//	}
	releaseBalls();
	allStop();
	wait1Msec(3000);
	retainBalls();
	allStop();
	wait1Msec(750);
  	lowerLift(80);
  	while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution
  	{
  	}
	allStop();
	wait1Msec(500);
	releaseAutoBall();
	wait1Msec(500);
	motor[intake] = 100;
	wait1Msec(1000);
	motor[intake] = 0;
	turn(0,180,100);
	allStop();
	wait1Msec(100);
	drive(1,1,75);
	sticksUp();
	drive(0,1,75);
	time1[T1]=0;
	while(SensorValue[sonarSensor]>30||time1[T1]<1200){
		left(50);
	}
	while(SensorValue[sonarSensor]<200){
		left(50);
	}
	allStop();
	wait1Msec(100);
	turn(0,25,70);
	allStop();
	wait1Msec(100);
	driveSonar(1,25,50);
	backward(30);
	wait1Msec(700);
	sticksDown();
	wait1Msec(250);
	allStop();
	raiseLift(100);
  	while (nMotorEncoder[intake] < 1000) //while the encoder wheel turns one revolution
  	{
  	}
	allStop();
	wait1Msec(500);
	releaseBalls();
	allStop();
	wait1Msec(3000);
	retainBalls();
	allStop();
	wait1Msec(750);
  	lowerLift(80);
  	while (nMotorEncoder[intake] > 740) //while the encoder wheel turns one revolution
  	{
  	}
	allStop();
	wait1Msec(500);
}
void Control_Process(void)
{
	IsLight = (ADC_LeftIR + ADC_RightIR)/2 < ADC_Thumb;
        
        if(!IsLight)
	{
		//          "0123456789abcdef"
		display_2 = "       Dark     ";
                
                if(WasLight && (ProgramState == 1 || ProgramState == 3))
                  ProgramState++;
	}
	else
        {
		display_2 = "       Light    ";
        }
        
        if((ProgramState != 1 && ProgramState != 3) && MotorTimer <= 0)
          ProgramState++;
        
        if(OldState != ProgramState)
          switch(ProgramState)
          {
            case 1: 
              allStop();
              waitMsec(500);
              MotorTimer = 10000;
              LeftMotorPower = LPOWER;
              RightMotorPower = RPOWER;
              
              //          "0123456789abcdef"
              display_1 = "Forward to Line ";
              break;
            case 2:
              allStop();
              waitMsec(500);
              LeftMotorPower = -LPOWER;
              RightMotorPower = -RPOWER;
              MotorTimer = 1000;
              
              //          "0123456789abcdef"
              display_1 = " Blind Reverse  ";
              break;
            case 3:
              allStop();
              MotorTimer = 10000;
              LeftMotorPower = -LPOWER;
              RightMotorPower = -RPOWER;
              
              //          "0123456789abcdef"
              display_1 = "Reverse to Line ";
              break;
            case 4: 
              allStop();
              //waitMsec(250);
              MotorTimer = (10000 - MotorTimer + 4000)/2;
              waitMsec(500);
              LeftMotorPower = LPOWER-10;
              RightMotorPower = RPOWER;
              
              //          "0123456789abcdef"
              display_1 = "Forward to Mid  ";
              break;
            case 5:
              allStop();
              waitMsec(500);
              MotorTimer = SPIN_TIME*3;
              LeftMotorPower = LPOWER;
              RightMotorPower = -RPOWER;
              
              //          "0123456789abcdef"
              display_1 = "   Spin CW  3x  ";
              break;
            case 6:
              allStop();
              waitMsec(500);
              MotorTimer = SPIN_TIME*2;
              LeftMotorPower = -LPOWER;
              RightMotorPower = RPOWER;
              
              //          "0123456789abcdef"
              display_1 = "   Spin CCW  2x ";
              break;
            default:
              allStop();
              MotorTimer = 0;
              
              //          "0123456789abcdef"
              display_1 = "     Default    ";
              break;
          }
        
        WasLight = IsLight;
        OldState = ProgramState;
}
task main()
{
	init();
	waitForStart();
	StartTask(keepHeading);
	StartTask(raiseLiftWhile);
	driveSonar(1,25,50);
	allStop();
	wait1Msec(100);
	backward(20);
	wait1Msec(700);
	sticksDown();
	wait1Msec(250);
	allStop();
	wait1Msec(300);
  //LIFT

	releaseBalls();
	allStop();
	wait1Msec(3000);
	retainBalls();
	allStop();
	wait1Msec(750);
  lowerLift(80);
  while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution
  {
  }
	allStop();
	wait1Msec(100);
	turn(1,19,70);
	allStop();
	wait1Msec(100);
	drive(0,2.0,100);
	allStop();
	wait1Msec(100);
	turn(0,170,100);
	allStop();
	wait1Msec(100);
	sticksUp();
	drive(1,.3,100);
	allStop();
	wait1Msec(100);
	turn(0,180,100);
	allStop();
	wait1Msec(100);
	//while(SensorValue[sonarSensor]>40){
	//	backward(100);
	//}
	//turnToGlobalHeading(-15);
	allStop();
	wait1Msec(100);
	drive(1,1.3,100);
	allStop();
	wait1Msec(100);
	turnToGlobalHeading(-78);
	allStop();
	wait1Msec(100);
	while(SensorValue[sonarSensor]>25){
		backward(30);
	}
	allStop();
	wait1Msec(100);
	while(SensorValue[sonarSensor]<200){
		left(50);
	}
	allStop();
	wait1Msec(100);
	turn(0,18,100);
	allStop();
	wait1Msec(100);
	driveSonar(1,25,50);
	backward(30);
	wait1Msec(700);
	sticksDown();
	wait1Msec(250);
	allStop();
	turn(1,17,70);
	allStop();
	wait1Msec(100);
	drive(0,2.7,100);
	allStop();
	wait1Msec(100);
	turn(0,175,100);
	allStop();
	wait1Msec(50);
	sticksUp();
	drive(1,.8,100);
}
void Control_Process(void)
{
	IsLight = (ADC_LeftIR + ADC_RightIR)/2 < ADC_Thumb;
        
	if(!IsLight)
	{
		//          "0123456789abcdef"
		display_2 = "       Dark     ";
	}
	else
	{
		display_2 = "       Light    ";
	}
        
	if(MotorTimer <= 0)
	{
		if(!IsLight)
			ProgramState = 1;
		
		if(IsLight)
			ProgramState = 2;
	}	 
        
	if(ProgramState != OldState)
		StateTransitions++;
		  
	/*if(StateTransitions == 50)
		ProgramState = 3, StateTransitions++;
		
	if(StateTransitions >= 100)
		ProgramState = 0;
	  */
	if(MotorTimer <= 0)
	{
		switch(ProgramState)
		{
			case 1: 
				//allStop();
				MotorTimer = 1;
				LeftMotorPower = LPOWER;
				RightMotorPower = -50;
              
				//          "0123456789abcdef"
				display_1 = "  Turning Right ";
				break;
            case 2:
				//allStop();
				MotorTimer = 1;
				LeftMotorPower = -50;
				RightMotorPower = RPOWER;
				              
				//          "0123456789abcdef"
				display_1 = "  Turning Left  ";
				break;
			case 3: 
				allStop();
				MotorTimer = 1000;
				LeftMotorPower = -LPOWER;
				RightMotorPower = RPOWER;
            default:
              allStop();
              //MotorTimer = 6000;              
              //          "0123456789abcdef"
              display_1 = "     Default    ";
              break;
			}
			
			
			if(!IsLight)
				ProgramState = 1;
		
			if(IsLight)
				ProgramState = 2;
             
		}
        
	WasLight = IsLight;
	OldState = ProgramState;
}
task main()
{
	init();
	wait1Msec(1000);
	sticksDown();
  raiseLift(100);
  time1[T1]=0;
  while (nMotorEncoder[intake] < 430 && time1[T1] < 2500) //while the encoder wheel turns one revolution
  {
  	times = time1[T1];
  }
	allStop();
	wait1Msec(500);
	//if(time1[T1]>2500)
	//	while(true){
	//		nxtDisplayCenteredBigTextLine(1,":(");
	//	}
	releaseBalls();
	allStop();
	wait1Msec(3000);
	retainBalls();
	allStop();
	wait1Msec(750);
  lowerLift(20);
  while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution
  {
  }
	allStop();
	wait1Msec(500);
	releaseAutoBall();
	intakeIn(100);
	wait1Msec(600);
	allStop();
	wait1Msec(500);
	sticksUp();
	wait1Msec(3000);

	while(SensorValue[sonarSensor]>25){allStop();}
	wait1Msec(2000);

	sticksDown();

	allStop();
	wait1Msec(500);
	raiseLift(100);
  while (nMotorEncoder[intake] < 1000) //while the encoder wheel turns one revolution
  {
  }
	allStop();
	wait1Msec(500);
	releaseBalls();
	allStop();
	wait1Msec(3000);
	retainBalls();
	allStop();
	wait1Msec(750);
  lowerLift(20);
  while (nMotorEncoder[intake] > 740) //while the encoder wheel turns one revolution
  {
  }
	allStop();
	wait1Msec(500);
}