void Acquired_Taste(){//dump
motor_off();
shake();
ao();
enable_servos();
servo_set(0, 450,4);//half way up
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);//back up
msleep(2000);
servo_set (0, 20, 2);//full up
ao();
motor(MOT_LEFT, 40);
motor(MOT_RIGHT, 30);//unjam if jammed
msleep(1000);
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);//redump
msleep(1000);
ao();
disable_servos();
msleep(2000);
enable_servos();
motor(MOT_LEFT, 60);
motor(MOT_RIGHT, 30);
servo_set (0, 700, 2);//forward and drop
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);
servo_set (0, 800, 2);
disable_servos();
}
void bringback2cube(){
//claw up
motor(Motor_Up,Motor_up_speed);
//zurück 1s
drive(1000,-Drivespeed,-Drivespeed);
//turn right 90
drive(970,Turnspeed,-Turnspeed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//back
drive(500,-Drivespeed,-Drivespeed);
//turn so little bit b4 cubes
drive(1130,Turnspeed,-Turnspeed);
//vor
drive(900,Drivespeed,Drivespeed);
//wait for claw up
claw_up();
//camera fix
camera_update();
//hide your cubes
cube_is_near();
//back
drive(1000,-Drivespeed,-Drivespeed);
//turn left more than 90 idk
drive(1330,-Turnspeed,Turnspeed);
//start to down motor
motor(Motor_Up,Motor_down_speed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//langsamer weil wackeldackel
drive(2000,Drivespeed_middle+20,Drivespeed_middle+20);
//back
drive(400,-Drivespeed,-Drivespeed);
//turn more than 90 lulz
drive(980,-Turnspeed,Turnspeed);
//light left and shit
drive(5000,Drivespeed_middle*2,(Drivespeed_middle*2)-7);
//light back and shit
drive(1000,-Drivespeed_middle*2,(-Drivespeed_middle*2)+7);
//wait for claw down
while(!digital(Sensor_Down)){
if(seconds()>start+113){
claw_open();
disable_servos();
}
}
freeze(Motor_Up);
msleep(2000);
claw_open();
disable_servos();
}
void CalibrateGate()
{
int i;
PromptFor("Disengage Gate");
enable_servos();
Gate(0);
PromptFor("Re-engage at top");
printf("Testing...");
for (i = 3; i >= 0; i--)
{
printf("%d", i);
sleep(1.0);
}
printf("\n");
for (i = 0; i < 5; i++)
{
Gate(1);
sleep(5.0);
Gate(0);
sleep(1.0);
}
disable_servos();
}
void find_and_grab(int color){
int k;
RIGHT(100);
LEFT(100);
msleep(2950);
LFREEZE();
RFREEZE();
RIGHT(-50);
LEFT(50);
msleep(700);
LFREEZE();
RFREEZE();
center_on_blob(90, RED, 0);
for(k = 0; k <= 10; k + 1){
RIGHT(100);
LEFT(100);
msleep(200);
center_on_blob(90, RED, 0);
}
/*
while(1){
RIGHT(100);
LEFT(100);
msleep(200);
center_on_blob(90, RED, 0);
if(depth_y > Camera_y){
RIGHT(50);
LEFT(-50);
msleep(710);
LFREEZE();
RFREEZE();
RIGHT(100);
LEFT(100);
msleep(800);
LFREEZE();
RFREEZE();
}
if(depth_y == OBJECT){
RIGHT(50);
LEFT(-50);
msleep(710);
LFREEZE();
RFREEZE();
RIGHT(100);
LEFT(100);
msleep(1600);
LFREEZE();
RFREEZE();
}
if(depth_y != OBEJECT){
RIGHT(50);
LEFT(-50);
msleep(710);
}
}*/
disable_servos();
}
int main()
{
camera_open();
ssp(SORT_SERVO,START);
ssp(RED_SERVO,RED_START);
ssp(GREEN_SERVO,GREEN_START);
wait_for_light(0);
start();
shut_down_in(119);
enable_servos();
msleep(5000);
reset();
right(47,0);
forward(35);
right(10,0);
backward(46);
full_sort();
other_side();
ssp(GREEN_SERVO,GREEN_DUMP);
msleep(1000);
ssp(GREEN_SERVO,GREEN_START);
msleep(250);
now();
ao();
disable_servos();
}
void push_botguy() {
set_servo_position(1, PUSH_BOTGUY_SERVO_DOWN);
msleep(2000);
// Garrett: Put the robot against the PVC.
// It may have to BACK up a little bit
// or maybe do a BIT of a turn before centering on Botguy.
//drives to edge of starting box
RIGHT(100);
LEFT(100);
msleep(400);
LFREEZE();
RFREEZE();
// Garrett: Try replacing the next step with:
// center_on_blob(80, RED, 1);
// First get Aaron's help to make center_on_blob USE
// its time parameter to be a FASTER center.
//turns to knock cube out of the way
RIGHT(-50);
LEFT(50);
msleep(2500);
// Garrett: Add code to:
// Drive to Botguy, knocking him down.
// KEEP GOING, so that you are out of the way of the Create
// and IN the way of their team's robots.
// Talk to Aaron about moving, then recentering,
// as the robot goes to Botguy and then through the gate.
disable_servos();
}
void ServoTest()
{
int pos;
enable_servos();
ServoRange();
disable_servos();
}
int main() {
check_poms();
start(); //start timer
//pom_collection();
disable_servos();
return 0;
}
int main() {
enable_servos();
collect_poms();
msleep(500);
disable_servos();
return 0;
}
void CompeteInit(int fPre)
{
enable_servos();
Gate(0);
if (fPre)
{
sleep(0.25);
disable_servos();
}
fBall = 0;
ipMotor = start_process(MotorDriver());
}
void kill() {
printf("killing in %i.", wait * 1000);
msleep(wait * 1000);
printf("killing program.\n");
disable_servos();
create_drive_straight(0);
motor(0, 0);
motor(1, 0);
motor(2, 0);
motor(3, 0);
exit(1);
}
/*This program moves two servos. Servos need to be plugged into ports 0 and 3. The program presets servo 0 to position 150. Then the program waits for the black button to be pushed. Then it enables servos, servo 0 goes to position 150 and servo 3 goes to 1900. Then servo 3 goes to 1900. Finally servo 0 goes to 1900 and servo 3 goes to 150 and the program ends. See appendix for finding your servo?s range. */
int main() {
set_servo_position(1,150); //preset port 0 to 150
printf("servo 0 at position 0\n");
printf("press black button to continue\n");
while(!black_button()) {} //wait for black button
enable_servos(); //enable the servos
sleep(1); //wait for servo to move
set_servo_position(1,1900); //move port 3 to 1900
sleep(1); //wait for servo to move
set_servo_position(1,1900); //move port 0 to 1900
set_servo_position(1,150); //move port 3 to 150
sleep(3); //wait for servos to move
disable_servos(); //power down servos
}
void wait_for_kill(void) {
while (!side_button()) {}
alloff();
motor(0, 0);
motor(1, 0);
motor(2, 0);
motor(3, 0);
alloff();
disable_servos();
create_drive_direct(0, 0);
create_stop();
create_disconnect();
exit(0);
}
void shake() {
off(REVOLVE_MOTOR);
ao();
enable_servos();
int i;
for(i=0; i<20; i++){
ssp(0,1333);
msleep(170);
ssp(0,1260);
msleep(170);
}
disable_servos();
motor(REVOLVE_MOTOR, 100);
}
/******** move the servo from 700 to 2048
*****************************************************************/
int main() {
// preset servo 1 position
set_servo_position(1,700);
printf("Press B to stop");
enable_servos(); // turn on servos
msleep(2000); // pause while it moves and user reads screen for 2 seconds
while((b_button()==0)) // while the button is not pressed
{ // move servo 1 in steps of 1348
set_servo_position(1,get_servo_position(1)+ 1348);
printf("servo at %d\n", get_servo_position(1));
msleep(200); // pause before next move
while((!a_button()) && (!b_button())) {}
}
disable_servos();
printf("Robot is done\n");
return 0;
}
void push_botguy(){
set_servo_position(1,PUSH_BOTGUY_SERVO_DOWN);
msleep(2000);
//drives to edge of starting box
RIGHT(100);
LEFT(100);
msleep(400);
LFREEZE();
RFREEZE();
//turns to knock cube out of the way
RIGHT(-50);
LEFT(50);
msleep(2500);
disable_servos();
}
int main() {
printf("In Drivepath test\n");
//light_start(0);
shut_down_in(119.5);
arm_drive();
claw_open();
bin_down();
enable_servos();
//forward(100);
forward(10);
int green = 0; //whether green has been collected
int red = 0; //whether red has been collected
start(); //start timer
int pile1Result = pom_collection();
backward(pom_collection_turn * 5);
pom_collection_turn = 0; //reset turn
if(pile1Result == 0) {
green = 1;
printf("Collecting first pile of green poms\n");
}
else if(pile1Result == 1) {
red = 1;
printf("Collecting first pile of red poms\n");
}
//backward(5);
left(15, ks/2);
off(MOT_RIGHT);
off(MOT_LEFT);
collect_poms();
collect_poms();
msleep(1000);
disable_servos();
ao();
return 0;
}
int main()
{
printf("starting connor\n");
enable_servo(1);
set_servo_position(1,100);
//disable_servo(1)
create_connect();
while (! digital(11))
{
printf("%d\n", digital(11));
create_drive_direct(-150,-150);
}
printf("starting connor\n");
set_servo_position(1,1400);
while (get_create_lbump() == 0 && get_create_rbump() == 0)
{
create_drive_direct(150, 150); // drive straight at 150mm/s
}
disable_servos();
}
void dance(){
int i=0;
enable_servos();
while(1){
if(i%6<3){
motor(MOT_LEFT, 40);
motor(MOT_RIGHT, 30);
}else{
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);
}
ssp(0,1133);
msleep(200);
ssp(0,1060);
msleep(200);
now();
i++;
}
disable_servos();
}
void _shut_down_task()
{
int i,j;
msleep(_shut_down_delay);
create_stop();
ao();
//disable_servos(); // delete this line if you want your servos to freeze but remain powered at the end
Bb_Game_Over= 1;
printf("Game over");
i= 0;
for (j= 256; j <= 1024; j+=256) {
// hog_processor();
while (i < j) {
kill_process(_shut_down_pid+(++i));
kill_process(_shut_down_pid-i);
}
ao();
disable_servos(); // delete this line if you want your servos to freeze but remain powered at the end
}
printf(".\n");
}
int main()
{
enable_servos(); //enable servos
set_servo_position(2,1000); //close claw,grab blocks
sleep(1.0); //wait
motor(1,100); //lift blocks
sleep(2.0); //wait
set_servo_position(2,2047); //open claw, drop blocks
disable_servos(); //disable servos
//variable declarations go here.
// uncomment the line below anf put 1s in the appropriate places if using floating sensors (ET and sonar)
//set_each_analog_state(0,0,0,0,0,0,0,0);// This line sets analog ports to be pullup (0) or floating (1)
// uncomment the line below to have the rest of the program wait for the starting light to turn on
//wait_for_light(port_number);//replace "port_number" with the analog port number into which the light sensor is connected
// uncomment the line below to have a function run for a specified amount of seconds
//run_for(num_secs, function_name);//replace num_secs with the amount of time to run and function_name with your function's name
}
int main(){
/*===Start===*/
servo_setup();
printf("Press any button to start");
WAIT(a_button()||b_button()||c_button());
//printf("waiting 2 seconds"); sleep(2);
/*===Gold One===*/
servo_drive_pos();
squareup(-80,1);
forward(35,100);
squareup(80,1);
get_gold();
/*===Score Gold===*/
back(17,80);
score_gold();
/*===Reposition===*/
left(90,0,100);
squareup(80,3);
/*===Botguy===*/
thread going=thread_create(arms_down_slow);
thread_start(going);
back(20,100);
back(30,100);
/*set_servo_position(ARM,ARM_UP);
msleep(500);
forward(34+5,100);
back(5);
set_servo_position(ARM,ARM_DOWN);
msleep(500);*/
/*===Tail===*/
disable_servos();
ao();
}
/* Measure ticks per second */
void CalibrateSpeed()
{
long msStart;
long msEnd;
int cForward;
int cBack;
int cTicks = 500;
int i;
int fServo;
for (fServo = 0; fServo < 2; fServo++)
{
if (fServo)
{
enable_servos();
Gate(0);
printf("Servo: ");
}
else
printf("No Servo: ");
for (i = 0; i < 2; i++)
{
msStart = mseconds();
Move(cTicks,cTicks);
msEnd = mseconds();
rgSpeed[i] = MultDiv(100, 1000, (int) (msEnd - msStart));
printf("%d ", rgSpeed[i]);
cTicks = -cTicks;
sleep(1.0);
}
if (fServo)
disable_servos();
printf("\n");
StartPress();
}
}
void main()
{
calibrate();
set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_RIGHT);
camera_open(LOW_RES);
enable_servos();
set_analog_pullup(3,0);
while(!b_button()){
claw_at_linefollowing();
do {
camera_update();
linefolowing();
} while(get_object_count(green) ==0); //linefolowing bis er was sieht
if(get_object_bbox(0,0).width>BLOB_SIZE_TAKE&&get_object_bbox(0,0).height>BLOB_SIZE_TAKE){
takepom();
}
printf("kompletdurch \n");
}
disable_servos();
stop();
}
void stop(){
ao();
camera_close();
disable_servos();
// stop all parts
}
void seeding(){
msleep(WAIT_FOR_CREATE);
//starts driving to the black line
LEFT(100);
RIGHT(100);
while (1)
{
if (analog10(LEFTSENSOR) > LEFTDARK)
{
break;
}
}
ao();
//turns on the black line
msleep(1500);
RIGHT(50);
LEFT(-50);
msleep(880);
LFREEZE();
RFREEZE();
msleep(1500);
//starts driving to end of black line
LEFT(100);
RIGHT(100);
while(1)
{
if (analog10(RIGHTSENSOR) < RIGHTLIGHT && analog10(CENTERSENSOR) < CENTERLIGHT)
{
break;
}
}
LFREEZE();
RFREEZE();
RIGHT(100);
LEFT(100);
msleep(200);
LFREEZE();
RFREEZE();
//opens servo to score position
set_servo_position(1, 2047);
msleep(500);
// turns to botguy
RIGHT(-50);
LEFT(50);
msleep(760);
LFREEZE();
RFREEZE();
center_on_blob(90, RED, 1);
//drives to botguy
RIGHT(100);
LEFT(100);
msleep(800);
LFREEZE();
RFREEZE();
/*while(1){
RIGHT(100);
LEFT(100);
msleep(200);
center_on_blob(90, RED, 0);
}*/
//close the claw around botguy
set_servo_position(1, 853);
msleep(500);
//backs up with botguy
RIGHT(-100);
LEFT(-100);
msleep(1200);
LFREEZE();
RFREEZE();
//turns to FMT
RIGHT(50);
LEFT(-50);
msleep(1750);
LFREEZE();
RFREEZE();
//drives to FMT
RIGHT(100);
LEFT(100);
msleep(3900);
LFREEZE();
RFREEZE();
disable_servos();
}
int main()
{
// INITIALIZE
int x, y, color=0, pink=1; // set up for color channel 0 (green)
int xvalue, yvalue;
int greencentered = 0;
int loop = 0;
int centerx=77; // calibrate these number and enter manually
int centery=63;
int margin=8;
int deltax = 0;
int deltay = 0;
int stepsizex;
int stepsizey;
int currpos;
int s_time;
light_start(L_SENSOR); // light start
shut_down_in(119);
camera_open(LOW_RES);
enable_servos(); // enable servos
set_servo_position(S_CATCHER,S_DOWN);
msleep(500);
set_servo_position(S_GATE,S_OPEN);
msleep(500);
// TRIBBLE PILE 1
//set_servo_position(S_GATE,S_OPEN); // open gate
msleep(5000);
set_servo_position(S_CATCHER,CATCHER_UP);
right(28,ks/2); // right
f_until_black(TOPHAT_RIGHT/*,TOPHAT_LEFT*/); // forward until right sensor sees black]
forward(2);
//printf("see black! time to close the gate\n");
set_servo_position(S_GATE,S_GAP); // close gate
//f_until_white(TOPHAT_RIGHT);
set_servo_position(CATCHERARM, CATCHER_UP);
// camera sort
s_time = curr_time();
while((curr_time()-s_time) < 14)
{
while (greencentered == 0)
{
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
loop = loop + 1;
printf("Update camera, loop %d\n", loop);
//msleep(10);
if (get_object_count(color) > 0)
{
xvalue = get_object_center(color,0).x;
yvalue = get_object_center(color,0).y;
deltax = abs(xvalue-centerx);
deltay = abs(yvalue-centery);
stepsizex = (deltax>10) ? 50: 20;
stepsizey = (deltay>10) ? 50: 20;
printf("x is %d, y is %d\n", xvalue, yvalue);
greencentered = ((xvalue >= centerx-margin) & (xvalue <= centerx+margin)) && ((yvalue >= centery-margin) & (yvalue <= centery+margin));
if(greencentered == 1)//get x, y for the biggest blob the channel sees
{
printf("Biggest blob at (%d,%d)\n",xvalue,yvalue);
msleep(300);
set_servo_position(CATCHERARM, CATCHER_MIDWAY);
msleep(300);
set_servo_position(CATCHERARM, CATCHER_DOWN);
//slow_servo(CATCHERARM,20,40,CATCHER_UP,CATCHER_DOWN);
msleep(300);
currpos=get_servo_position(CATCHERARM);
printf("%d\n",currpos);
/*if(CATCHER_MIDWAY>=currpos>CATCHER_DOWN)
{
currpos=get_servo_position(CATCHERARM);
set_servo_position(CATCHERARM, currpos-INCREMENT);
msleep(SLEEP_INCREMENT);
currpos=get_servo_position(CATCHERARM);
}
if(currpos<CATCHER_MIDWAY)
{
currpos=get_servo_position(CATCHERARM);
set_servo_position(CATCHERARM, currpos+INCREMENT);
msleep(SLEEP_INCREMENT);
currpos=get_servo_position(CATCHERARM);
}*/
set_servo_position(CATCHERARM,CATCHER_UP);
msleep(500);
}
else
{
if(xvalue <= centerx+margin) //moves right if senses value greater than 80
{
motor(MOT_RIGHT,70);
motor(MOT_LEFT,-70);
msleep(stepsizex);
motor(MOT_RIGHT,0);
motor(MOT_LEFT,0);
//msleep(10);
printf("blob is too right\n");
}
if(xvalue >= centerx-margin) // moves left if senses value less than 70
{
motor(MOT_LEFT,70);
motor(MOT_RIGHT,-70);
msleep(stepsizex);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too left\n");
}
if ((xvalue > 67 /*centerx-margin*/) && (xvalue < 87 /*centerx+margin*/))
{
if(yvalue > centery+margin) //moves forward if senses value greater than 43
{
motor(MOT_LEFT,-50);
motor(MOT_RIGHT,-50);
msleep(stepsizey);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too close\n");
}
if(yvalue < centery-margin) // moves backwar if senses value less than 38
{
motor(MOT_LEFT,50);
motor(MOT_RIGHT,50);
msleep(stepsizey);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too far\n");
}
}
}
}
}
}
set_servo_position(S_GATE,S_CLOSE);
while (analog10(2)<BLACK_SEN_THRESH)
{
motor(MOT_LEFT,-100);
}
// SCORE PILE 1
left(5,0);
//backward(50);
touch_back(TOUCH_SEN);
forward(10);
backward(10);
forward(20);
right(55,0);
forward(35);
backward(4);
right(45,0);
while (analog10(3)<BLACK_SEN_THRESH)
{
motor(MOT_LEFT, 100);
motor(MOT_RIGHT, 100);
}
msleep(10);
backward(24);
left(42,0);
backward(10);
left(50,0);
//backward(35);
touch_back(TOUCH_SEN);
forward(120.00);
// TRIBBLE PILE 2
set_servo_position(S_GATE,S_OPEN);
forward(40.00);
set_servo_position(S_GATE,S_GAP);
// camera sort
s_time = curr_time();
while((curr_time()-s_time) < 14) //timer
{
while (greencentered == 0)
{
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
loop = loop + 1;
printf("Update camera, loop %d\n", loop);
//msleep(10);
if (get_object_count(color) > 0)
{
xvalue = get_object_center(color,0).x;
yvalue = get_object_center(color,0).y;
deltax = abs(xvalue-centerx);
deltay = abs(yvalue-centery);
stepsizex = (deltax>10) ? 50: 20;
stepsizey = (deltay>10) ? 50: 20;
printf("x is %d, y is %d\n", xvalue, yvalue);
greencentered = ((xvalue >= centerx-margin) & (xvalue <= centerx+margin)) && ((yvalue >= centery-margin) & (yvalue <= centery+margin));
if(greencentered == 1)//get x, y for the biggest blob the channel sees
{
printf("Biggest blob at (%d,%d)\n",xvalue,yvalue);
msleep(300);
set_servo_position(CATCHERARM, CATCHER_MIDWAY);
msleep(300);
set_servo_position(CATCHERARM, CATCHER_DOWN);
//slow_servo(CATCHERARM,20,40,CATCHER_UP,CATCHER_DOWN);
msleep(300);
currpos=get_servo_position(CATCHERARM);
printf("%d\n",currpos);
/*if(CATCHER_MIDWAY>=currpos>CATCHER_DOWN)
{
currpos=get_servo_position(CATCHERARM);
set_servo_position(CATCHERARM, currpos-INCREMENT);
msleep(SLEEP_INCREMENT);
currpos=get_servo_position(CATCHERARM);
}
if(currpos<CATCHER_MIDWAY)
{
currpos=get_servo_position(CATCHERARM);
set_servo_position(CATCHERARM, currpos+INCREMENT);
msleep(SLEEP_INCREMENT);
currpos=get_servo_position(CATCHERARM);
}*/
set_servo_position(CATCHERARM,CATCHER_UP);
msleep(500);
}
else
{
if(xvalue <= centerx+margin) //moves right if senses value greater than 80
{
motor(MOT_RIGHT,70);
motor(MOT_LEFT,-70);
msleep(stepsizex);
motor(MOT_RIGHT,0);
motor(MOT_LEFT,0);
//msleep(10);
printf("blob is too right\n");
}
if(xvalue >= centerx-margin) // moves left if senses value less than 70
{
motor(MOT_LEFT,70);
motor(MOT_RIGHT,-70);
msleep(stepsizex);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too left\n");
}
if ((xvalue > 67 /*centerx-margin*/) && (xvalue < 87 /*centerx+margin*/))
{
if(yvalue > centery+margin) //moves forward if senses value greater than 43
{
motor(MOT_LEFT,-50);
motor(MOT_RIGHT,-50);
msleep(stepsizey);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too close\n");
}
if(yvalue < centery-margin) // moves backwar if senses value less than 38
{
motor(MOT_LEFT,50);
motor(MOT_RIGHT,50);
msleep(stepsizey);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too far\n");
}
}
}
}
}
}
set_servo_position(S_GATE,S_CLOSE);
// SCORE PILE 2
right(55,0);
forward(28);
backward(12);
right(55,0);
touch_back(TOUCH_SEN);
forward(200);
right(50,0);
touch_back(TOUCH_SEN);
forward(15);
left(50,0);
while (analog10(3)<BLACK_SEN_THRESH)
{
motor(MOT_LEFT, 100);
motor(MOT_RIGHT, 100);
}
backward(4);
right(50,0);
msleep(10);
forward(24);
right(42,0);
touch_back(TOUCH_SEN);
backward(4);
// END
ao();
disable_servos();
printf("done!");
}
void halt()
{
freeze_halt();
disable_servos();
}
void stop(){
ao();
camera_close();
disable_servos();
}
int main()
{
/* FIRST CALIBRATION: based on poms position adjust starting position
*/
printf("test 3.11, calibration value: \nclaw down : %d, \nright ang forward right: %d, \nright ang forward left : %d, \nback right angle %d ...\n", DOWN_SERVO, RIGHT_ANGLE_CLICKS_MRP_RIGHT, RIGHT_ANGLE_CLICKS_MRP_LEFT, RIGHT_ANGLE_CLICKS_BACK_MRP);
double start_time = seconds();
enable_servos();
clear_motor_position_counter(RIGHT_MOTOR);
clear_motor_position_counter(LEFT_MOTOR);
wait_for_light(1);
shut_down_in(118);
clawUp();
//**************************************
//* PART 1 : get first set of poms to lower storage area
//*
//**************************************
moveBackward(1, NO_DEBUG);
moveForwardHighSpeed(23, DEBUG);
clawDown();
//moveForward(8,NO_DEBUG);
//we have the Poms
rightAngleBwd(RIGHT, NO_DEBUG);
//bump against the upper storage area
moveBackwardHighSpeed(15, NO_DEBUG);
moveForwardHighSpeed(17, NO_DEBUG);
rightAngleBwd(RIGHT, NO_DEBUG);
//bump against upper PVC side
moveBackwardHighSpeed(18, NO_DEBUG);
//move towards dropping poms
moveForward(1.5, NO_DEBUG);
msleep(300);
rightAngleFwd(LEFT, NO_DEBUG);
msleep(300);
clawUp();
//uses IR sensor to stop the forward movement
moveForwardTilBlackLine(15, NO_DEBUG);
printf("====> elapsed time end of part 1: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 1 : get first set of poms to lower storage area
//*
//**************************************
//**************************************
//* PART 2 : get the blue cube to the corner
//*
//**************************************
moveBackwardHighSpeed(25, NO_DEBUG);
//recalibrate against top PVC next to botguy
rightAngleBwd(LEFT, NO_DEBUG);
moveBackwardHighSpeed(11, NO_DEBUG);
// SECOND CALIBRATION: based on blackline/blue position
moveForward(7.5, NO_DEBUG);
rightAngleFwd(RIGHT, NO_DEBUG);
//get the cube now
//====>>>>> may have to be adjust the day of competition based on position of 2nd
//====>>>>> set of poms
moveForwardHighSpeed(15, NO_DEBUG);
//====>>>>>
clawAlmostDownCube();
twentyTwoAngleFwd(LEFT, NO_DEBUG);
clawUp();
//====>>>>> calibration to get to the corner 22.5 fwd, 22.5 more forward, aiming at bottom left hand corner
moveForwardHighSpeed(2.5, NO_DEBUG);
twentyTwoAngleFwd(LEFT, NO_DEBUG);
//clawAlmostDownCube();
moveForwardHighSpeed(30, NO_DEBUG);
//====>>>>>
//moveForwardHighSpeed(2, NO_DEBUG);
//====>>>>>
moveBackwardHighSpeed(9, NO_DEBUG);
fortyFiveAngleFwd(LEFT, NO_DEBUG);
printf("====> elapsed time end of part 2: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 2 : get the blue cube in the corner
//*
//**************************************
//**************************************
//* PART 3 : prepare to get second set of poms
//*
//**************************************
//recalibrate against side PVC
rightAngleBwd(RIGHT, NO_DEBUG);
moveBackwardHighSpeed(25, NO_DEBUG);
moveForward(5, NO_DEBUG);
//calibrating with top PVC
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(23,NO_DEBUG);
// THIRD CALIBRATION: based on second set of Poms
//below is the number of inches from the side PVC
//
moveForwardHighSpeed(10, NO_DEBUG);
rightAngleBwd(RIGHT, NO_DEBUG);
//recalibrate with side pvc
moveBackwardHighSpeed(5,NO_DEBUG);
printf("====> elapsed time end of part 3: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 3 : prepare to get second set of poms
//*
//**************************************
//**************************************
//* PART 4 : bring second set of poms home
//*
//**************************************
//grabs the poms, bring down claw and go home
moveForwardHighSpeed(12.5,NO_DEBUG);
clawDown();
moveForwardHighSpeed(59.5, NO_DEBUG);
//here is the code to recalibrate one more time... but do we have enough time?
/*//recalibrate against top PVC
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(10, NO_DEBUG);
//move towards dropping poms
moveForwardHighSpeed(10, NO_DEBUG);
//msleep(500);
printf("==> moving righ angle bwd\n");
rightAngleFwd(LEFT, NO_DEBUG);
*/
//recalibrate against top PVC
//moveForwardHighSpeed(41,NO_DEBUG);
//last calibration before dropping second set of poms
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(14, NO_DEBUG);
//move towards dropping poms
moveForwardSlow(1.5, NO_DEBUG);
msleep(300);
rightAngleFwd(LEFT, NO_DEBUG);
msleep(300);
clawUp();
moveForwardTilBlackLine(15, DEBUG);
//**************************************
//* END OF PART 4 : bring second set of poms home
//*
//**************************************
printf("====> elapsed time: %f\n", (seconds() - start_time));
ao();
disable_servos();
return 0;
}
