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();
}
void runop()
{
#define D_F 0
#define D_S 2
#define D_T 1
//*
int n = 9;
int m = 11;
int direc[] = {D_T, D_T, D_F, D_T, D_F, D_T, D_F, D_T, D_F, D_F, D_T};
int ticks[] = {270,-270, 1377,-1080,1315,1080,-3005,1080,1315, -2630,-540};
// go get balls
for(int i=0;i<n;i++)
{
//ticks[i] *= TEAM;
if(direc[i]==D_F)
{
forward_ticks(ticks[i]);
}
//else if(direc[i]==D_S)
//{
// sideways_ticks(TEAM*ticks[i]);
//}
else if(direc[i]==D_T)
{
turn_ticks(TEAM*ticks[i]);
}
if(i==n-1)
{
intakecontrol(1, 0);
}
}
//come back
for(int i=n-1;i>=0+2;i--)
{
if(i==n-1)
{
intakecontrol(0, 0);
}
//ticks[i] *= TEAM;
if(direc[i]==D_F)
{
forward_ticks(ticks[i]*(-1));
}
//else if(direc[i]==D_S)
//{
// sideways_ticks(TEAM*ticks[i]);
//}
else if(direc[i]==D_T)
{
turn_ticks(TEAM*ticks[i]*(-1));
}
}
//into launch position
for(int i=n;i<m;i++)
{
//ticks[i] *= TEAM;
if(direc[i]==D_F)
{
forward_ticks(ticks[i]);
}
//else if(direc[i]==D_S)
//{
// sideways_ticks(TEAM*ticks[i]);
//}
else if(direc[i]==D_T)
{
turn_ticks(TEAM*ticks[i]);
}
}
//*/
flywheelmotors(WHEELSPEED);
delay(4000);
while(1)
{
printf("--------------Conveyor on--------\n\r");
intakecontrol(1, 0);
delay(1000);
printf("--------------Conveyor off-------\n\r");
intakecontrol(0, 0);
delay(1000);
}
/*
int forward = 1377;
int right = 1315;
int turn = 1080;
int left = 3005;
int forward2 = 1315;
forward_ticks(AUTO_SPEED, forward);
sideways_ticks(AUTO_SPEED*team, right*team);
turn_ticks(AUTO_SPEED*team, turn*team);
sideways_ticks(-AUTO_SPEED*team, -left*team);
forward_ticks(AUTO_SPEED, forward2);
//*/
}
