task autonomous()
{
SensorType[in8] = sensorNone;
wait1Msec(1000);
//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
SensorType[in8] = sensorGyro;
wait1Msec(2000);
if(SensorValue(limit1) == 0) //hang a cube from the back, move backwards until the limit switch is pressed
{
motor[port1] = -100;
motor[port2] = -100;
}
else if(SensorValue(limit1) == 1)
{
wait1Msec(1000); //wait for the cube to release from the robot
motor[port1] = 100;
motor[port2] = 100;
}
gyroTurn(45);//turns 45 degrees
drive_straight(sqrt(2), 50);//returns to starting position
drive_straight(1.5, 50);
//do something with intake
if(SensorValue(limit1) == 0)
{
motor[port1] = -100;
motor[port2] = -100;
}
}
task autonomous()
{
pre_auton();
int arm_in_position = 0; //arm is down; 0 for false and 1 for true
drive_straight_suck(100,FULL,5);//speed,suckspeed,inches//drive straight and inhale the red stack at the same time
while(arm_in_position != 1) {
arm_in_position = lock(low_lock_point);
}
turn(127,-90);//speed,degrees turn left
while(arm_in_position != 1) {//place tubes
arm_in_position = lock(low_descore_point);
}
drive_straight(-FULL,5);// back up
turn(FULL,180); //left or shouldn't matter, turn to face blue stack
drive_straight_suck(FULL,FULL,10);//drive to blue stack//drive straight and inhale the blue stack at the same time
turn(FULL,90);//turn right
drive_straight(FULL,7);//drive straight
turn(FULL,-45);//turn left to face tower
drive_straight(FULL,7);//drive to tower
sucker(-FULL,3);//spit tube into tower
}
int main(int argc, char** argv) {
startup_servos();
//wait_for_light(0);
scanf("%s", NULL);
printf("waiting for light\n");
while (analog(0) > 200) {}
shut_down_in(700000);
raise_scoop_level();
drive_straight(900);
drive_straight(-50);//old value 320
/*//to be replaced with tick thing
motor(kMotorPortDriveLeft, 100);
motor(kMotorPortDriveRight, 5);
msleep(2430);
motor(kMotorPortDriveLeft, 0);
motor(kMotorPortDriveRight, 0);*/
//dump();
//close_scoop_doors();
//open_scoop_doors();
//dump();
//msleep(800);
turn_ticks_in_place(50, 890);
// pile 1
open_scoop_doors();
lower_scoop_level();
drive_straight(250);
close_scoop_doors();
raise_scoop_level();
//Reverse to pile 2
drive_straight(-290);
turn_ticks(50, -700);
drive_straight(-250);
//set_servo_position(kServoPortScoopTiltLeft, kServoPositionScoopTiltHighBack);
//set_servo_position(kServoPortScoopTiltRight, 2047 - kServoPositionScoopTiltHighBack);
turn_ticks(50, 770);//this value
lower_scoop_level();
open_scoop_doors();
//pick up pile 2
drive_straight(270);
close_scoop_doors();
raise_scoop_level();
drive_straight(80);
raise_scoop_very_high();
drive_straight(170);
turn_ticks(50, 500);
drive_straight(-90);
turn_ticks_in_place(50, 650);
drive_straight(100);
//set_servo_position(kServoPortScoopTiltLeft, kServoPositionScoopTiltHigh - 300);
//set_servo_position(kServoPortScoopTiltLeft, (2047 - kServoPositionScoopTiltHigh) + 300);
// turn_ticks(50, 555);
// drive_straight(50);
//set_servo_position(kServoPortScoopTiltLeft, 1000);
//set_servo_position(kServoPortScoopTiltRight, 2047 - 1000);
msleep(200);
open_doors_wide();
dump();
msleep(500);
dump_shake();
dump();
dump_shake();
dump();
//raise_scoop();
drive_straight(-400);
return 0;
raise_scoop_level();
close_doors_wide();
raise_scoop_very_high();
turn_ticks_in_place(50, -1350);
drive_straight(100);
//drive to pile 3
raise_scoop_level();
turn_ticks_in_place(50, 700);
turn_ticks_in_place(50, -400);
lower_scoop_level();
turn_ticks_in_place(50, -300);
drive_straight(100);
//pick up pile 3
open_scoop_doors();
drive_straight(400);
close_scoop_doors();
turn_ticks_in_place(50, -100);
drive_straight(-400);
raise_scoop_level();
turn_ticks_in_place(50, -100);
drive_straight(-400);
turn_ticks_in_place(50, -200);
drive_straight(-290);
lower_scoop_level();
turn_ticks_in_place(50, 1150);
turn_ticks_in_place(50, -100);
raise_scoop_level();
// drive to/pick up pile 4
drive_straight(500);
turn_ticks_in_place(50, 400);
mid_scoop_level();
turn_ticks_in_place(50, -900);
raise_scoop_level();
turn_ticks_in_place(50, 400);
lower_scoop_level();
turn_ticks_in_place(50, 500);
turn_ticks_in_place(50, -50);
open_scoop_doors();
drive_straight(230);
close_scoop_doors();
raise_scoop_level();
// dump other load
turn_ticks_inverse(50, 500);
drive_straight(-100);
turn_ticks_in_place(50, -100);
drive_straight(-150);
raise_scoop_very_high();
turn_ticks_in_place(50, 700);
drive_straight(200);
set_servo_position(kServoPortScoopTiltLeft, 1000);
set_servo_position(kServoPortScoopTiltRight, 2047 - 1000);
msleep(200);
open_doors_wide();
dump();
msleep(500);
dump_shake();
dump();
dump_shake();
dump();
return 0;
turn_ticks_in_place(50, -475);
drive_straight(-200);
turn_ticks_in_place(50, 475);
drive_straight(400);
//pick up pile 4
lower_scoop_level();
drive_straight(500);
open_scoop_doors();
}
