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;
}
}
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();
}
}
}
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);
}
}
}
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
}
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;
}
}
}
}
