jesse / mecanum_route.c

#include <kipr/wombat.h> #include <stdlib.h> #define BLACK_VALUE 3700 int stop_at_line(int min_ticks){ if(analog(1)>BLACK_VALUE && gmpc(0)> min_ticks){ printf("reach end of ramp\n"); return 1; } printf("keep going analog_1=%d, gmpc_0=%d\n",analog(1), gmpc(0)); return 0; } void follow_the_line(){ if (analog(0)<BLACK_VALUE){ motor(0,90); motor(1,15); motor(2,90); motor(3,15); } else{ motor(0,15); motor(1,90); motor(2,15); motor(3,90); } } int down_ramp() { printf("down ramp\n"); motor(0,100); motor(1,100); motor(2,100);// square up motor(3,100); msleep(2200); cmpc(0); while (stop_at_line(9000) ==0){ follow_the_line(); msleep(30); } return 0; } int up_ramp(){ printf("up ramp/n"); motor(0,100); motor(1,100); motor(2,100); motor(3,100); msleep(5500); cmpc(0); while (stop_at_line(6000) ==0){ follow_the_line(); msleep(30); } return 0; } void mechanum_drive_forward_tick(int distance){ cmpc(0); int direction; if(distance > 0){ direction = 1; } else {direction = -1;} motor(0,70*direction); motor(1,70*direction); motor(2,70*direction); motor(3,70*direction); int target_pos = distance; while(abs(gmpc(0)) < abs(target_pos)){ printf("zzz...\n\n"); msleep(10); } ao(); } void section_1() { /* * Task: Drive from upper floor start box to the lower start box while pushing poms, then drops the poms * turns right and drives into the middle area and strafes sideways until lined up with the doors * * Start Position: upper start box facing ramp * End Position: facing doors, backed against the side of the upper storage * */ down_ramp(); ao(); msleep(500); motor(0,100); motor(1,100); motor(2,100); motor(3,100); msleep(1000); ao(); msleep(500); motor(0,-100); motor(1,-100); motor(2,100); motor(3,100); msleep(1160); ao(); msleep(500); motor(0,-100); motor(1,-100); motor(2,-100); motor(3,-100); msleep(1000); ao(); msleep(500); // End section_1 } void section_2(){ /* Task: drive forward, open the doors, grab botguy, backup Start position: back against the wall facing the doors End position: back against the wall facing the doors */ printf("ROUTE#I\n\n"); cmpc(0); while (stop_at_line(50) ==0){ motor(0,100); motor(1,100); motor(2,100); motor(3,100); msleep(30); } ao(); msleep(900); //code for moving left motor(0,70); motor(1,-70); motor(2,70); motor(3,-70); msleep(8000); mechanum_drive_forward_tick(2000); } void section_3() { /* Task: strafe left until it's in front of the start box, backs up and squares up against the outer wall, then turns right and backs up to square up against outer wall while facing the ramp Start Position: back against the upper storage wall facing the doors End Position: back against outer wall facing the ramp */ motor(0,-100); motor(1,-100); motor(2,-100); motor(3,-100); msleep(700); printf("go backwards\n"); set_servo_position(3,2047);//put down arm msleep(1000); printf("arm down\n"); set_servo_position(0,2047);//close claw //pick up cone msleep(1000); printf("claw closed\n"); set_servo_position(0,900);//open claw msleep(1000); printf("claw open\n"); motor(0,-100); motor(1,100); motor(2,-100); motor(3,100); msleep(4600); printf("go sideways\n"); motor(0,-100); motor(1,-100); motor(2,-100); motor(3,-100); msleep(3300); printf("go backwards\n"); motor(0,-100); motor(1,-100); motor(2,100); motor(3,100); msleep(1300); printf("turn\n"); motor(0,-100); motor(1,-100); motor(2,-100); motor(3,-100); msleep(1600); printf("go backwards\n"); } void section_4() { /* Task: drives up the ramp to drop botguy in the upper storage startbox Start Position: back against outer wall facing the ramp End Position: inside upper storage start box with botguy on the ground */ up_ramp(); } int main(){ printf("Starting Section 1\n"); section_1(); printf("Starting Section 2\n"); section_2(); printf("Starting Section 3\n"); section_3(); printf("Starting Section 4\n"); }