/**wall following code including fine-grain orientation using 2 distance sensors **/ #define LEFT_LIMIT 7.00 #define RIGHT_LIMIT 6.50 /* last test -> shrunk each side of limit by 0.5" */ #define LEFT_NORMAL 100 #define LEFT_SLOW 50 #define RIGHT_NORMAL 100 #define RIGHT_SLOW 50 #define ORIENT_SPEED 40 #define ORIENT_THRESHOLD 0.25 #define ORIENT_DURATION 300. #define ORIENT_TIMEOUT 2500. #define LEFT_M 0 #define RIGHT_M 3 #define FRONT_SIDE_SENSOR 2 #define REAR_SIDE_SENSOR 4 #define LEFT_SPIN_TIME 1250. #define RIGHT_SPIN_TIME 1250. #define U_TURN_SPIN_TIME 2400. #define ENCODER_PORT 7 #define START_LIGHT_PORT 6 #define TICKS_FORWARD 80 #define TICKS_BACK 240 #define SERVO_PORT 3 #define SERVO_RIGHT_LIMIT 2000 #define SERVO_LEFT_LIMIT 3500 #define ORKO_SERVO_PORT 4 #define TICKS_OVER_BALL 15 #define NORTH_PORT 9 #define WEST_PORT 12 #define SOUTH_PORT 11 #define EAST_PORT 10 #define LEFT_ARM_PORT 4 #define LEFT_ARM_SPEED -100 #define RIGHT_ARM_PORT 1 #define RIGHT_ARM_SPEED 100 /**** music defines follow ****/ #define BASE_FREQ 440. #define SIXT 0.1125 #define EIGHTH 0.225 #define QUARTER 0.45 #define HALF 0.9 #define WHOLE 1.8 #define STEP 1.05946 void main() { float current_time; /* stores system time */ float start_time; /* stores the time that start machine was called */ int read_the_encoder; /* stores shaft encoder values */ int look_process; /* stores process id to kill orko_look() */ /* hold off on IR transmission until contest begins */ ir_transmit_off(); /* setup the servos before start light*/ enable_servos(); sleep(3.0); servo(SERVO_PORT, SERVO_LEFT_LIMIT); sleep(1.0); disable_servos(); printf("\nservo disabled"); sleep(3.0); /* align with start light and go when ready */ start_machine(START_LIGHT_PORT); start_time = (float) mseconds(); /* all code that follows will execute upon contest start. machine will shut off after 60 seconds */ printf("\nCONTEST HAS BEGUN"); /* begin IR transmission */ ir_transmit_on(); /* wait until opponent's beacon is detected */ printf("\nWaiting For IR"); while (digital(NORTH_PORT) == 0 && digital(WEST_PORT) == 0 && digital(SOUTH_PORT) == 0 && digital(EAST_PORT) == 0) { sleep(0.05); } printf("\nIR detected. Orienting"); /** INITIAL (COARSE) ORIENTATION **/ /* based on opponent's beacon */ if (digital(NORTH_PORT) == 1) { spin_right(); } else if (digital(WEST_PORT) == 1) { drive_forward(); } else if (digital(SOUTH_PORT) == 1) { spin_left(); } else if (digital(EAST_PORT) == 1) { u_turn(); } /** FINE-GRAIN ORIENTATION **/ /* orients using both distance sensors - ensures robot is parallel to wall before beginning to drive for first ball; this is after the robot has used a timed spin to approximate one of the four cardinal directions */ /* utilizes functions parallel(), orient_left(), orient_right() */ /* includes a timeout in case of faulty operation during competition */ current_time = (float) mseconds(); while (parallel() != 0 && (float) mseconds() < current_time + ORIENT_TIMEOUT ) { if (parallel() == 1) { orient_right(ORIENT_DURATION); } else { orient_left(ORIENT_DURATION); } } /********** WALL FOLLOWING CODE ************/ /* follows wall according to front distance sensor reading along the right side of the robot */ /* utilizes functions drive_forward(), veer_left() and veer_right() */ /* main while loop follows */ enable_encoder(ENCODER_PORT); reset_encoder(ENCODER_PORT); while(read_encoder(ENCODER_PORT) < TICKS_FORWARD) { if (offtrack() >= 1) { if (offtrack() == 2) veer_left(); else if (offtrack() == 1) veer_right(); } else drive_forward(); } /* for use in determining encoder settings */ read_the_encoder = read_encoder(ENCODER_PORT); printf("\n%d", read_the_encoder); ao(); /* release arm & capture ball */ enable_servos(); servo(SERVO_PORT, SERVO_RIGHT_LIMIT); sleep(0.5); reset_encoder(ENCODER_PORT); while(read_encoder(ENCODER_PORT) < TICKS_OVER_BALL) { drive_forward(); } ao(); sleep(0.25); /* start a separate thread calling fn orko_look() w/ default settings */ /* ahhh!!! what just happened to the arm?? who are all these ppl? where am i? why are they staring at me??? - quoth the orko */ look_process = start_process(orko_look()); /* fn orko_lights() turns on the LED motor ports */ orko_lights(); /* got the ball (hopefully!)... now go back to our side */ reset_encoder(ENCODER_PORT); while(read_encoder(ENCODER_PORT) < TICKS_BACK) { if (offtrack_reverse() >= 1) { if (offtrack_reverse() == 2) veer_right_reverse(); else if (offtrack_reverse() == 1) veer_left_reverse(); } else drive_reverse(); } read_the_encoder = read_encoder(ENCODER_PORT); printf("\n%d", read_the_encoder); ao(); /* whew... made it back! take a quick break */ sleep(0.5); /* kill the orko_look() process... DOUBLE CHECK THIS!!! */ kill_process(look_process); /* AND NOW BUST OUT!!! yeah baby... */ /* starts separate threads calling fns to sing, dance and shine!!! :) (default process settings) */ start_process(play_louie(start_time)); start_process(orko_dance()); start_process(orko_lights_blink()); } /********** Functions Called from Main() **********/ /* convert analog input from distance sensor into a measure of distance in inches */ float calc_distance (float m) { return (-3.16 + (950./(8.58+m))); } /* this function determines whether the robot is parallel to the wall */ int parallel() { float distance_front_analog = (float) analog(FRONT_SIDE_SENSOR); float distance_rear_analog = (float) analog(REAR_SIDE_SENSOR); float distance_front = calc_distance(distance_front_analog); float distance_rear = calc_distance(distance_rear_analog); float difference = distance_front - distance_rear; if (difference > ORIENT_THRESHOLD) { return 1; } else if (difference < -ORIENT_THRESHOLD) { return -1; } else { return 0; } } /* following two orient functions used to spin for a given amount of time */ void orient_right(float m) { float orient_time = (float) mseconds(); printf("\norienting right"); while ((float) mseconds() < orient_time + m) { motor(LEFT_M, ORIENT_SPEED); motor(RIGHT_M, -ORIENT_SPEED); } } void orient_left(float m) { float orient_time = (float) mseconds(); printf("\norienting left"); while ((float) mseconds() < orient_time + m) { motor(LEFT_M, -ORIENT_SPEED); motor(RIGHT_M, ORIENT_SPEED); } } /* using LEFT_LIMIT and RIGHT_LIMIT constants, this function determines whether the robot is too far or too close to the wall */ int offtrack() { float distance_analog = (float) analog(FRONT_SIDE_SENSOR); float distance = calc_distance(distance_analog); sleep(0.01); if (distance > LEFT_LIMIT) return 1; else if (distance < RIGHT_LIMIT) return 2; else return 0; } /* similar to offtrack(), but for driving / wall-following in reverse */ int offtrack_reverse() { float distance_analog = (float) analog(REAR_SIDE_SENSOR); float distance = calc_distance(distance_analog); sleep(0.01); if (distance > LEFT_LIMIT) return 1; else if (distance < RIGHT_LIMIT) return 2; else return 0; } /* if the robot is within the limits, just drive straight */ void drive_forward() { printf("\ndriving forward"); motor(LEFT_M, LEFT_NORMAL); motor(RIGHT_M, RIGHT_NORMAL); } void drive_reverse() { printf("\ndriving backwards"); motor(LEFT_M, -LEFT_NORMAL); motor(RIGHT_M, -RIGHT_NORMAL); } /* code used for the left turn */ void spin_left() { float spin_time; spin_time = (float) mseconds(); while ((float) mseconds() < spin_time + LEFT_SPIN_TIME) { motor(LEFT_M, -LEFT_NORMAL); motor(RIGHT_M, RIGHT_NORMAL); } } /* code used for the right turn */ void spin_right() { float turn_time = (float) mseconds(); while ((float) mseconds() < turn_time + RIGHT_SPIN_TIME) { motor(LEFT_M, LEFT_NORMAL); motor(RIGHT_M, -RIGHT_NORMAL); } } void u_turn() { float turn_time = (float) mseconds(); while ((float) mseconds() < turn_time + U_TURN_SPIN_TIME) { motor(LEFT_M, LEFT_NORMAL); motor(RIGHT_M, -RIGHT_NORMAL); } } /* use this function to veer out if the robot gets too close to the wall when attempting to wall follow */ void veer_left() { float veer_time = (float) mseconds(); /* first veer out from the wall */ printf("\nveering left"); while ((float) mseconds() < veer_time + 500.) { motor(LEFT_M, LEFT_SLOW); motor(RIGHT_M, RIGHT_NORMAL); } /* now that you are in position, straighten out the wheels */ veer_time = (float) mseconds(); while ((float) mseconds() < veer_time + 200.0) { motor(LEFT_M, LEFT_NORMAL); motor(RIGHT_M, RIGHT_SLOW); } /* drive a short distance in this position before next reading */ veer_time = (float) mseconds(); while ((float) mseconds() < veer_time + 200.0) { motor(LEFT_M, LEFT_NORMAL); motor(RIGHT_M, RIGHT_NORMAL); } } /* similar to veer_left(), but corrects the robot in the opposite direction if it strays too far from the wall. note that we take longer to straighen out the wheels and drive forward here due to the geometry of this configuration for the distance sensor. the optimal timing was calculated through testing. */ void veer_right() { float veer_time = (float) mseconds(); printf("\nveering right"); while ((float) mseconds() < veer_time + 500.) { motor(LEFT_M, LEFT_NORMAL); motor(RIGHT_M, RIGHT_SLOW); } veer_time = (float) mseconds(); while ((float) mseconds() < veer_time + 200.0) { motor(LEFT_M, LEFT_SLOW); motor(RIGHT_M, RIGHT_NORMAL); } veer_time = (float) mseconds(); while ((float) mseconds() < veer_time + 200.0) { motor(LEFT_M, LEFT_NORMAL); motor(RIGHT_M, RIGHT_NORMAL); } } /* these two functions, veer_left_reverse() and veer_right_reverse() are similar to their non-reverse counterparts and are used for wall-following when driving in reverse */ /* note that veer_left_reverse() has the pecularities of the geometry of veer_right() */ /* left and right in this context refer to the direction the vehicle moves with respect to its rear end; in other words left for forward is right for reverse */ void veer_right_reverse() { float veer_time = (float) mseconds(); /* first veer out from the wall */ printf("\nveering left"); while ((float) mseconds() < veer_time + 500.) { motor(LEFT_M, -LEFT_SLOW); motor(RIGHT_M, -RIGHT_NORMAL); } /* now that you are in position, straighten out the wheels */ veer_time = (float) mseconds(); while ((float) mseconds() < veer_time + 200.0) { motor(LEFT_M, -LEFT_NORMAL); motor(RIGHT_M, -RIGHT_SLOW); } /* drive a short distance in this position before next reading */ veer_time = (float) mseconds(); while ((float) mseconds() < veer_time + 200.0) { motor(LEFT_M, -LEFT_NORMAL); motor(RIGHT_M, -RIGHT_NORMAL); } } void veer_left_reverse() { float veer_time = (float) mseconds(); printf("\nveering right"); while ((float) mseconds() < veer_time + 500.) { motor(LEFT_M, -LEFT_NORMAL); motor(RIGHT_M,-RIGHT_SLOW); } veer_time = (float) mseconds(); while ((float) mseconds() < veer_time + 200.0) { motor(LEFT_M, -LEFT_SLOW); motor(RIGHT_M, -RIGHT_NORMAL); } veer_time = (float) mseconds(); while ((float) mseconds() < veer_time + 200.0) { motor(LEFT_M, -LEFT_NORMAL); motor(RIGHT_M, -RIGHT_NORMAL); } } void play_louie(float louie_time) { while ((float) mseconds() < louie_time + 57000.) { /* 1st line */ c_s (EIGHTH); c_s (EIGHTH); c_s (EIGHTH); d (EIGHTH + QUARTER); rest(QUARTER); custom (8., QUARTER); /* high e */ c_s (EIGHTH); d (EIGHTH + EIGHTH); c_s (EIGHTH); a (EIGHTH); b (EIGHTH); a (EIGHTH); b (EIGHTH); a (SIXT); a (SIXT); a (EIGHTH); rest(HALF); custom (14., QUARTER); custom (11., QUARTER); custom (8., QUARTER); d (EIGHTH); c (EIGHTH); a (EIGHTH); a (EIGHTH); /* 2nd line */ c_s (EIGHTH); c_s (EIGHTH); c_s (EIGHTH); d (EIGHTH + QUARTER); rest(QUARTER); custom (8., QUARTER); /* high e */ c_s (EIGHTH); d (SIXT); custom (8., SIXT + EIGHTH); /* high e */ a (EIGHTH); c (EIGHTH); a (EIGHTH); c_s (EIGHTH); a (SIXT); a (SIXT); a (QUARTER); rest(HALF); rest(HALF); rest(QUARTER); rest(EIGHTH); a (EIGHTH); /* 3rd line */ c (QUARTER); b (EIGHTH); a (EIGHTH); b (EIGHTH); a (QUARTER); g (EIGHTH); a (QUARTER); a (EIGHTH); a (EIGHTH + QUARTER); rest (EIGHTH); d(EIGHTH); custom(14.,EIGHTH); custom(14.,QUARTER); custom(8.,EIGHTH); /* high e */ d(QUARTER); rest(EIGHTH); c(EIGHTH + EIGHTH); e(EIGHTH); e(EIGHTH); d(EIGHTH + QUARTER); rest(EIGHTH); a (EIGHTH); /* 4th line */ c (QUARTER); b (EIGHTH); a (EIGHTH); b (EIGHTH); a (EIGHTH); a (EIGHTH); f_s (EIGHTH); a (QUARTER); a (EIGHTH); a (EIGHTH + QUARTER); rest(EIGHTH); a (EIGHTH); c (EIGHTH); c (EIGHTH); a (EIGHTH); a (EIGHTH); b (EIGHTH); a (EIGHTH); a (EIGHTH); e (EIGHTH); a (QUARTER); a (EIGHTH); a (EIGHTH + QUARTER); rest(QUARTER); /* this is slightly different than sheet music */ /* 5th line */ a (EIGHTH); a (QUARTER); c_s (EIGHTH); a (SIXT); a (SIXT); c_s (EIGHTH); a (EIGHTH + QUARTER); rest(HALF); /* 1st line repeated*/ c_s (EIGHTH); c_s (EIGHTH); c_s (EIGHTH); d (EIGHTH + QUARTER); rest(QUARTER); custom (8., QUARTER); /* high e */ c_s (EIGHTH); d (EIGHTH + EIGHTH); c_s (EIGHTH); a (EIGHTH); b (EIGHTH); a (EIGHTH); b (EIGHTH); a (SIXT); a (SIXT); a (EIGHTH); rest(HALF); custom (14., QUARTER); custom (11., QUARTER); custom (8., QUARTER); d (EIGHTH); c (EIGHTH); a (EIGHTH); a (EIGHTH); /* 2nd line repeated */ c_s (EIGHTH); c_s (EIGHTH); c_s (EIGHTH); d (EIGHTH + QUARTER); rest(QUARTER); custom (8., QUARTER); /* high e */ c_s (EIGHTH); d (SIXT); custom (8., SIXT + EIGHTH); /* high e */ a (EIGHTH); c (EIGHTH); a (EIGHTH); c_s (EIGHTH); a (SIXT); a (SIXT); a (QUARTER); rest(HALF); rest(HALF); rest(QUARTER); rest(EIGHTH); a (EIGHTH); /** end of song **/ } } void a (float m) { tone(BASE_FREQ*STEP, m); } void a_s (float m) { tone(BASE_FREQ*(STEP^2.), m); } void b (float m) { tone(BASE_FREQ*(STEP^3.), m); } void c (float m) { tone(BASE_FREQ*(STEP^4.), m); } void c_s (float m) { tone(BASE_FREQ*(STEP^5.), m); } void d (float m) { tone(BASE_FREQ*(STEP^6.), m); } void d_s (float m) { tone(BASE_FREQ*(STEP^7.), m); } void e (float m) { tone(BASE_FREQ*(STEP^-5.), m); } void f (float m) { tone(BASE_FREQ*(STEP^-4.), m); } void f_s (float m) { tone(BASE_FREQ*(STEP^-3.), m); } void g (float m) { tone(BASE_FREQ*(STEP^-2.), m); } void g_s (float m) { tone(BASE_FREQ*(STEP^-1.), m); } void rest (float m) { sleep(m); } /* method for creating custom note (in another octave, apart from that defined by BASE_FREQ) */ void custom (float p, float m) { tone(BASE_FREQ*(STEP^p), m); } void orko_dance() { while (1) { servo(ORKO_SERVO_PORT, 500); sleep (0.5); servo(ORKO_SERVO_PORT, 3500); sleep(0.5); servo(ORKO_SERVO_PORT, 1500); sleep (0.2); servo(ORKO_SERVO_PORT, 2500); sleep(0.2); servo(ORKO_SERVO_PORT, 1500); sleep (0.2); servo(ORKO_SERVO_PORT, 2500); sleep(0.2); } } void orko_look() { while(1) { servo(ORKO_SERVO_PORT, 3500); sleep(2.0); servo(ORKO_SERVO_PORT, 500); sleep (2.0); } } void orko_lights() { motor(LEFT_ARM_PORT, LEFT_ARM_SPEED); motor(RIGHT_ARM_PORT, RIGHT_ARM_SPEED); } void orko_lights_blink() { while (1) { motor(LEFT_ARM_PORT, LEFT_ARM_SPEED); sleep(0.1); motor(LEFT_ARM_PORT, 0); sleep(0.1); motor(RIGHT_ARM_PORT, RIGHT_ARM_SPEED); sleep(0.1); motor(RIGHT_ARM_PORT, 0); sleep(0.1); } }