예제 #1
0
파일: DropOff.cpp 프로젝트: G-3/ENPH-253-1
    void DropOff::extension(){

        driveMotors(0,0);
        //Initialize timestamp
        if (extensionTimestamp == 0){
            extensionTimestamp = millis();
            extendArm(arm,200);
        }

        if (millis() - extensionTimestamp > EXTENSION_DELAY){
            extensionTimestamp = 0;
            currentPhase = RELEASE;
        }
    }
예제 #2
0
파일: DropOff.cpp 프로젝트: G-3/ENPH-253-1
    void DropOff::retraction(){
        //Initialize timestamp
        if (retractionTimestamp == 0){
            retractionTimestamp = millis();
            extendArm(arm,0);
        }

        if (millis() - retractionTimestamp > RETRACTION_DELAY){
            retractionTimestamp = 0;
            if (getPassengerPickup(CL)||getPassengerPickup(CR)){
                //if there is still another animal try again
                currentPhase = SETUP;
            }
            else{
                //Call back to event handler informing that drop-off is done
                EHandler::finishDropOff();
            }
        }
    }
예제 #3
0
task main()
{
 int x = SensorValue[pot1];
 int y = SensorValue[pot2];

 while(true)
  {
  	motor[motor2] = vexRT[Ch3];
		limMotor(motor3, lim1, 0);

		BtnX = 0;

   //assign values to buttons to change value of BtnX
   if (vexRT[Btn5U] == 1){
     BtnX = BtnX + 1;}

   if (vexRT[Btn5D] == 1){
     BtnX = BtnX + 2;}

   if (vexRT[Btn7U] == 1){
     BtnX = BtnX + 4;}

   if (vexRT[Btn7L] == 1){
     BtnX = BtnX + 8;}

   if (vexRT[Btn7D] == 1){
     BtnX = BtnX + 16;}

   if (vexRT[Btn7R] == 1){
     BtnX = BtnX + 32;}

   if (vexRT[Btn6U] == 1){
     BtnX = BtnX + 64;}

   if (vexRT[Btn6D] == 1){
     BtnX = BtnX + 128;}

   if (vexRT[Btn8U] == 1){
     BtnX = BtnX + 256;}

   if (vexRT[Btn8L] == 1){
     BtnX = BtnX + 512;}

   if (vexRT[Btn8D] == 1){
     BtnX = BtnX + 1024;}

   if (vexRT[Btn8R] == 1){
     BtnX = BtnX + 2048;}

   if ((vexRT[Btn7D] == 0)&&(vexRT[Btn7U] == 0)&&(vexRT[Btn7L] == 0)&&(vexRT[Btn7R] == 0)
     &&(vexRT[Btn8D] == 0)&&(vexRT[Btn8U] == 0)&&(vexRT[Btn8L] == 0)&&(vexRT[Btn8R] == 0)
     &&(vexRT[Btn5U] == 0)&&(vexRT[Btn5D] == 0)&&(vexRT[Btn6U] == 0)&&(vexRT[Btn6D] == 0)){
     BtnX = 0;}

   //twoServos (servo1, servo2, BtnX);
   clawOpenCloseCenter(servo1, servo2, servo3, servo4, 1, BtnX);
   extendArm(motor1, 63, BtnX);
   autonomousFcns(BtnX, x);

   //joystick ch1 controls servo4
    if((vexRT[Ch1] <= -40) && (vexRT[Ch1] > -80))
		{
			stepServo(servo4, -1);
		}
		else if((vexRT[Ch1] <= -80) && (vexRT[Ch1] > -110))
		{
			stepServo(servo4, -2);
		}
		else if(vexRT[Ch1] <= -110)
		{
			stepServo(servo4, -3);
		}

		if((vexRT[Ch1] >= 40) && (vexRT[Ch1] < 80))
		{
			stepServo(servo4, 1);
		}
		else if((vexRT[Ch1] >= 80) && (vexRT[Ch1] < 110))
		{
			stepServo(servo4, 2);
		}
		else if(vexRT[Ch1] >= 110)
		{
			stepServo(servo4, 3);
		}

		//joystick ch2 controls servo3
    if((vexRT[Ch2] <= -40) && (vexRT[Ch2] > -80))
		{
			stepServo(servo3, -1);
		}
		else if((vexRT[Ch2] <= -80) && (vexRT[Ch2] > -110))
		{
			stepServo(servo3, -2);
		}
		else if(vexRT[Ch2] <= -110)
		{
			stepServo(servo3, -3);
		}

		if((vexRT[Ch2] >= 40) && (vexRT[Ch2] < 80))
		{
			stepServo(servo3, 1);
		}
		else if((vexRT[Ch2] >= 80) && (vexRT[Ch2] < 110))
		{
			stepServo(servo3, 2);
		}
		else if(vexRT[Ch2] >= 110)
		{
			stepServo(servo3, 3);
		}
	}
}
예제 #4
0
task main()
{
 int x = SensorValue[pot1];

 while(true)
  {
  	//prevents servos and elbow from drooping over time
  	if(servoCount == 2)
  		{
  			servoValue1 = motor[servo1];
 				servoValue2 = motor[servo2];
 				servoValue3 = motor[servo3];
 				servoValue4 = motor[servo4];
 				tempPot2 = SensorValue[pot2];
  		}

  	if(servoCount == servoCountMax)
  		{
  			idleServo (servo1, servoValue1, 2);
  			idleServo (servo2, servoValue2, 2);
  			idleServo (servo3, servoValue3, 2);
  			idleServo (servo4, servoValue4, 2);
  			idleElbow(3, tempPot2);
  			servoCount = 0;
  		}

  	 servoCount++;

  	motor[motor4] = vexRT[Ch4]; //base motor controlled by joystick ch4
  	motor[motor1] = -vexRT[Ch3]; //elbow motor controlled by joystick ch3

    //dead zones for motors
  	if(abs(vexRT[Ch3]) < 30)
  		{
  		motor[motor1] = 0;
  		}

  	if(abs(vexRT[Ch4]) < 30)
  		{
  		motor[motor4] = 0;
  		}

		limMotor(motor4, lim1, lim2);

		BtnX = 0;
   //assign values to buttons to change value of BtnX
   if (vexRT[Btn5U] == 1){
     BtnX = BtnX + 1;}

   if (vexRT[Btn5D] == 1){
     BtnX = BtnX + 2;}

   if (vexRT[Btn7U] == 1){
     BtnX = BtnX + 4;}

   if (vexRT[Btn7L] == 1){
     BtnX = BtnX + 8;}

   if (vexRT[Btn7D] == 1){
     BtnX = BtnX + 16;}

   if (vexRT[Btn7R] == 1){
     BtnX = BtnX + 32;}

   if (vexRT[Btn6U] == 1){
     BtnX = BtnX + 64;}

   if (vexRT[Btn6D] == 1){
     BtnX = BtnX + 128;}

   if (vexRT[Btn8U] == 1){
     BtnX = BtnX + 256;}

   if (vexRT[Btn8L] == 1){
     BtnX = BtnX + 512;}

   if (vexRT[Btn8D] == 1){
     BtnX = BtnX + 1024;}

   if (vexRT[Btn8R] == 1){
     BtnX = BtnX + 2048;}

   if ((vexRT[Btn7D] == 0)&&(vexRT[Btn7U] == 0)&&(vexRT[Btn7L] == 0)&&(vexRT[Btn7R] == 0)
     &&(vexRT[Btn8D] == 0)&&(vexRT[Btn8U] == 0)&&(vexRT[Btn8L] == 0)&&(vexRT[Btn8R] == 0)
     &&(vexRT[Btn5U] == 0)&&(vexRT[Btn5D] == 0)&&(vexRT[Btn6U] == 0)&&(vexRT[Btn6D] == 0)){
     BtnX = 0;}

   clawOpenCloseCenter(servo1, servo2, servo3, servo4, 1, BtnX);
   extendArm(motor3, 127, BtnX);
   autonomousFcns(BtnX, x);

   //joystick ch1 controls servo4
    if((vexRT[Ch1] <= -30) && (vexRT[Ch1] > -80))
		{
			stepServo(servo4, -1);
		}
		else if((vexRT[Ch1] <= -80) && (vexRT[Ch1] > -110))
		{
			stepServo(servo4, -2);
		}
		else if(vexRT[Ch1] <= -110)
		{
			stepServo(servo4, -3);
		}

		if((vexRT[Ch1] >= 30) && (vexRT[Ch1] < 80))
		{
			stepServo(servo4, 1);
		}
		else if((vexRT[Ch1] >= 80) && (vexRT[Ch1] < 110))
		{
			stepServo(servo4, 2);
		}
		else if(vexRT[Ch1] >= 110)
		{
			stepServo(servo4, 3);
		}

		//joystick ch2 controls servo3
    if((vexRT[Ch2] <= -40) && (vexRT[Ch2] > -80))
		{
			stepServo(servo3, -1);
		}
		if((vexRT[Ch2] <= -80) && (vexRT[Ch2] > -110))
		{
			stepServo(servo3, -2);
		}
		if(vexRT[Ch2] <= -110)
		{
			stepServo(servo3, -3);
		}

		if((vexRT[Ch2] >= 40) && (vexRT[Ch2] < 80))
		{
			stepServo(servo3, 1);
		}
		if((vexRT[Ch2] >= 80) && (vexRT[Ch2] < 110))
		{
			stepServo(servo3, 2);
		}
		if(vexRT[Ch2] >= 110)
		{
			stepServo(servo3, 3);
		}
	}
}
task main()
{
	initializeRobot();

	int routineCounter = 0;

	while(nNxtButtonPressed != ORANGE_BUTTON){
		switch((int)nNxtButtonPressed){
		case LEFT_BUTTON:
			while(nNxtButtonPressed == LEFT_BUTTON){}	//empty while to delay until button is released
			routineCounter ++;
			break;
		case RIGHT_BUTTON:
			while(nNxtButtonPressed == RIGHT_BUTTON){}	//empty while to delay until button is released
			routineCounter --;
			break;
		}//end switch
		switch (routineCounter){
		case OFF_RAMP:
			displayBigStringAt(LEFT_X, TOP_Y, "OFF_RAMP");
			break;
		case START_AT_PARK_ZONE:
			displayBigStringAt(LEFT_X, TOP_Y, "PARKING_ZONE");
			break;
		}
	}//end while (should end when Orange button is pressed)

	waitForStart(); // Wait for the beginning of autonomous phase.

	nMotorEncoder[rightDrive] = 0;
	nMotorEncoder[leftDrive] = 0;

	switch(routineCounter){
	case OFF_RAMP:
		while(nMotorEncoder[rightDrive] > -8500)
		{
			motor[rightDrive] = -50;
			motor[leftDrive] = -50;
		}
		motor[rightDrive] = 0;
		motor[leftDrive] = 0;

		extendArm();

		break;
		//we may want to mount the sensor at a 30 deggree angle
	case START_AT_PARK_ZONE:
		//if(SensorValue(IRSeek) == /*straight ahead (4?)*/)
		{
			//go straight forward to hit kickstand
		}
		//if(SensorValue(IRSeek) > /*straight ahead (4?) meaning the center module is tilted*/ && SensotValue(IRSeek) < /*farthest to the left meaning center is at second position*/)
		{
			//go forward a little bit then turn and go forward to hit kickstand
		}
		//if(SensorValue(IRSeek) > /*farthest left sensor value, means the center is at third position*/

		/*PSEUDOCODE for START_AT_PARK_ZONE
		sense for IR
		depending on where it is
		go straight forward to hit kickstand
		go forward a little bit then turn and go forward to hit kickstand
		go forward a little, straight, turn, forward to hit kickstand
		stop*/
		break;
	}


	while (true){}//this just keeps the program running until the end of Autonomous
}
예제 #6
0
task main()
{
 int x = SensorValue[pot1];
 int y = SensorValue[pot2];

 while(true)
  {
  	int tempSensor = SensorValue[pot2];

    int servoValue1 = motor[servo1];
    int servoValue2 = motor[servo2];
    int servoValue3 = motor[servo3];
    int servoValue4 = motor[servo4];

  	motor[motor4] = vexRT[Ch4]; //base motor controlled by joystick ch4
  	motor[motor1] = -vexRT[Ch3]; //elbow motor controlled by joystick ch3

    //dead zones for motors
  	if(abs(vexRT[Ch3]) < 15)
  		{
  		motor[motor1] = 0;
  		}

  	if(abs(vexRT[Ch4]) < 15)
  		{
  		motor[motor4] = 0;
  		}

		limMotor(motor3, lim1, 0);

		BtnX = 0;

   //assign values to buttons to change value of BtnX
   if (vexRT[Btn5U] == 1){
     BtnX = BtnX + 1;}

   if (vexRT[Btn5D] == 1){
     BtnX = BtnX + 2;}

   if (vexRT[Btn7U] == 1){
     BtnX = BtnX + 4;}

   if (vexRT[Btn7L] == 1){
     BtnX = BtnX + 8;}

   if (vexRT[Btn7D] == 1){
     BtnX = BtnX + 16;}

   if (vexRT[Btn7R] == 1){
     BtnX = BtnX + 32;}

   if (vexRT[Btn6U] == 1){
     BtnX = BtnX + 64;}

   if (vexRT[Btn6D] == 1){
     BtnX = BtnX + 128;}

   if (vexRT[Btn8U] == 1){
     BtnX = BtnX + 256;}

   if (vexRT[Btn8L] == 1){
     BtnX = BtnX + 512;}

   if (vexRT[Btn8D] == 1){
     BtnX = BtnX + 1024;}

   if (vexRT[Btn8R] == 1){
     BtnX = BtnX + 2048;}

   if ((vexRT[Btn7D] == 0)&&(vexRT[Btn7U] == 0)&&(vexRT[Btn7L] == 0)&&(vexRT[Btn7R] == 0)
     &&(vexRT[Btn8D] == 0)&&(vexRT[Btn8U] == 0)&&(vexRT[Btn8L] == 0)&&(vexRT[Btn8R] == 0)
     &&(vexRT[Btn5U] == 0)&&(vexRT[Btn5D] == 0)&&(vexRT[Btn6U] == 0)&&(vexRT[Btn6D] == 0)){
     BtnX = 0;}


   clawOpenCloseCenter(servo1, servo2, servo3, servo4, 1, BtnX);
   extendArm(motor3, 127, BtnX);
   autonomousFcns(BtnX, x);

   //joystick ch1 controls servo4
    if((vexRT[Ch1] <= -30) && (vexRT[Ch1] > -80))
		{
			stepServo(servo4, -1);
		}
		else if((vexRT[Ch1] <= -80) && (vexRT[Ch1] > -110))
		{
			stepServo(servo4, -2);
		}
		else if(vexRT[Ch1] <= -110)
		{
			stepServo(servo4, -3);
		}

		if((vexRT[Ch1] >= 30) && (vexRT[Ch1] < 80))
		{
			stepServo(servo4, 1);
		}
		else if((vexRT[Ch1] >= 80) && (vexRT[Ch1] < 110))
		{
			stepServo(servo4, 2);
		}
		else if(vexRT[Ch1] >= 110)
		{
			stepServo(servo4, 3);
		}

		//joystick ch2 controls servo3
    if((vexRT[Ch2] <= -40) && (vexRT[Ch2] > -80))
		{
			stepServo(servo3, -1);
		}
		if((vexRT[Ch2] <= -80) && (vexRT[Ch2] > -110))
		{
			stepServo(servo3, -2);
		}
		if(vexRT[Ch2] <= -110)
		{
			stepServo(servo3, -3);
		}

		if((vexRT[Ch2] >= 40) && (vexRT[Ch2] < 80))
		{
			stepServo(servo3, 1);
		}
		if((vexRT[Ch2] >= 80) && (vexRT[Ch2] < 110))
		{
			stepServo(servo3, 2);
		}
		if(vexRT[Ch2] >= 110)
		{
			stepServo(servo3, 3);
		}

		if(vexRT[Ch3] == 0 && BtnX != 256 && BtnX != 512 && BtnX != 1024 && BtnX != 2048)
			{
				while((SensorValue[pot2] - tempSensor) > 3)
			  	{
			  		motor[motor1] = -30;
			  	}
			  while((SensorValue[pot2] - tempSensor) < -3)
			  	{
			  		motor[motor1] = 30;
			  	}
			}
	}
}