void autonFiveRight(){ //autonFiveLEft grabs the five stack, starts on left tile.
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeLeft(127, .3 * clickspermeters);
drive( .1 * clickspermeters, FORWARD, 70);
wrist(-127, 1100);
//AArm(300 , LOWER, 60);
wait1Msec(400);
drive( .6 * clickspermeters, FORWARD, 80);
wait1Msec(20);
driveBackward(100, 100);
StartTask(five);
wrist(127, 600);
wait1Msec(900);
wrist(10, 20);
aArm(1500, RAISE, 120);
drive( .4 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot2] > 1900){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 1000);
wait1Msec(500);
driveBackward(0, 1000);
}
// TODO: use correct sensor port (change to irBack)
task main() {
calibrateGyro();
{ // Drive to the IR basket, dump the block, drive back & square up against the wall
int timeForward;
ClearTimer(T1);
driveBackward(DRIVE_SPEED);
while (true) {
if (time1[T1] > SEARCH_TIME)
break;
else if (SensorValue[irFront] == 2)
break;
}
timeForward = time1[T1];
// Keep moving to adjust for the first 2 / last 2 baskets
if (timeForward < MIDDLE_TIME) {
// Back up a little
driveStop();
wait1Msec(500);
driveForward(FIRST_HALF_DELAY, DRIVE_SPEED);
timeForward -= FIRST_HALF_DELAY;
} else {
// Back up a little
driveStop();
wait1Msec(500);
driveForward(SECOND_HALF_BACKUP_TIME, DRIVE_SPEED);
timeForward -= SECOND_HALF_BACKUP_TIME;
}
driveStop();
flipper_flip();
driveForward(timeForward + 1500, DRIVE_SPEED);
motor[rightDrive] = DRIVE_SPEED;
wait1Msec(500);
driveStop();
}
{ // Drive next to the ramp & turn on to it
motor[leftDrive] = -DRIVE_SPEED;
wait1Msec(1050);
motor[rightDrive] = -DRIVE_SPEED;
wait1Msec(1300);
driveStop();
turnRightEuler(TURN_90_EULER, DRIVE_SPEED);
PlaySound(soundBeepBeep);
driveBackward(1500, DRIVE_SPEED);
}
}
// autonomous plays are named as follows auton[Score-Result][starting tile]
void autonFiveLeft(){ //autonFiveLEft grabs the five stack, starts on left tile.
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeRight(127, .3 * clickspermeters);
drive( .1 * clickspermeters, FORWARD, 70);
wait1Msec(20);
wrist(-127, 1100);
//AArm(300 , LOWER, 60);
wait1Msec(400);
drive( .7 * clickspermeters, FORWARD, 80);
StartTask(five);
wrist(127, 600);
wait1Msec(650);
driveBackward(100, 150);
wrist(10, 20);
driveBackward(100, 300);
//aArm(2100, RAISE, 120);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeLeft(100, clickspermeters);
turnLeft(60, 600);
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 300);
wait1Msec(500);
driveBackward(0, 20);
}
void avoidObstacle()
{
driveBackward();
wait1Msec(1000);
if (touchL != 0)
{
lastHitDir = -1;
rightDeg(45);
}
else if (touchR != 0)
{
lastHitDir = 1;
leftDeg(45);
}
driveForward();
wait1Msec(2000);
}
task main() {
servoPrep();
Joystick_WaitForStart();
disableDiagnosticsDisplay();
while(true) {
encoderPrep();
startTrackers();
driveForward(310.0);
turnRight(85.0);
if(irDetected == true) {
irPos2 = true;
turnRight(40.0);
irDetected = false;
driveBackward(25.0);
if(irDetected == true) {
driveBackward(15.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(105.0);
turnRight(85.0);
driveBackward(300.0);
}
else {
irPos1 = true;
driveBackward(110.0);
turnRight(46.0);
driveBackward(135.0);
turnRight(3.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(130.0);
turnRight(82.0);
driveBackward(300.0);
}
}
else {
driveForward(115.0);
// ir loop
if(irDetected == true) {
irPos3 = true;
driveBackward(15.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(120.0);
turnRight(85.0);
driveBackward(300.0, 60);
/*turnRight(43.0);
driveBackward(250.0);
motor[rightWheel] = -20;
motor[leftWheel] = -20;
wait1Msec(200);
motor[rightWheel] = 0;
motor[leftWheel] = 0;
dropClamp();
driveForward(10.0);
raiseLift(700.0);
dropBallGoal();
resetDropGoal();*/
break;
}
else {
driveBackward(220.0);
turnRight(85.0);
driveBackward(100.0);
}
}
break;
}
}
int main(int argc, char** argv) {
shutDownAfter(119);
msleep(1000); //Wait for other robot to finish. Should be 30s (3000). Is only 1000 for testing
raise_botguy_to(BOTGUY_CLAW_UP);
enable_servos();
printf("camera open response: %i\n", camera_open());
printf("driving to pos.\n");
driveStraight();
msleep(3700);
stop();
turnInPlaceCCW();
msleep(700);
stop();
driveStraight();
msleep(1900); //was 1500
stop();
turnInPlaceCW();
msleep(580);
stop();
msleep(1000);
raise_botguy_to(BOTGUY_CLAW_DOWN);
driveStraight();
msleep(1100);
stop();
close_botguy_claw();
hold_botguy_claw_closed();
//preformApproachBotguy(false, 0);
driveBackward();
msleep(700);
raise_botguy_to(BOTGUY_CLAW_MID_UP);
msleep(900);
raise_botguy_to(BOTGUY_CLAW_FULL_UP);
turnInPlaceCCW();
msleep(600);
stop();
driveBackward();
msleep(1000);
stop();
preformApproachCube(false, 1);
stop();
return 0;
turnInPlaceCW();
msleep(700);
driveStraight();
msleep(3000);
turnInPlaceCCW();
msleep(700);
driveStraight();
msleep(1700);
stop();
msleep(500);
raise_botguy_to(BOTGUY_CLAW_MID_UP);
return (EXIT_SUCCESS);
}
void autonSkills(){ //autonFiveLEft grabs the five stack, starts on left tile.
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeRight(127, .3 * clickspermeters);
drive( .1 * clickspermeters, FORWARD, 70);
wait1Msec(20);
wrist(-127, 1100);
//AArm(300 , LOWER, 60);
wait1Msec(400);
drive( .7 * clickspermeters, FORWARD, 80);
StartTask(five);
wrist(127, 600);
wait1Msec(600);
driveBackward(100, 550);
wait1Msec(20);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeLeft(100, clickspermeters);
wait1Msec(20);
turnLeft(60, 600);
wait1Msec(20);
drive( .43 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 400);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
turnLeft(60, 100);
wait1Msec(20);
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] <2800){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(100);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(100);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -35;
motor[leftWrist] = -35;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(2000);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 850);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
strafeRight(127, .9 * clickspermeters);
turnRight(100, 200);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
drive( .4 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] <2700){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(300);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
strafeLeft(127, .5 * clickspermeters);
driveBackward(127 , 700);
wait1Msec(5000);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
drive( .25 * clickspermeters, FORWARD, 70);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
drive( .7 * clickspermeters, FORWARD, 70);
wait1Msec(20)
while(SensorValue[WristPot1] <2700){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(100);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 900);
wait1Msec(10000);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
strafeLeft(127, clickspermeters * .5);
drive( 1.15 * clickspermeters, FORWARD, 70);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
}
