// Common Bot // Does everything. // // Motor Left Front = motor 0 // Motor Right Front = motor 1 // // Starting Light = analog 2 // // Line Left Front = analog 3 // Line Right Front = analog 5 // // Touch Left Back = digital 9 // Touch Right Back = digital 10 // Touch Left Front = digital 11 // Touch Right Front = digital 12 // // Camera Rotator = servo 0 // #use botball.ic #define leftmotor 0 #define rightmotor 1 #define startinglight 2 #define leftfrontline 3 #define rightfrontline 5 #define leftfronttouch 9 #define rightfronttouch 10 #define leftbacktouch 11 #define rightbacktouch 12 #define rotator 0 #define true 1 // Way points attributes // 00 way point number #define index 0 // 01 time (milliseconds) #define time 1 // 02 left motor speed (-1000 to 1000) #define speedleft 2 // 03 right motor speed (-1000 to 1000) #define speedright 3 // 04 left front touch sensor (0 don't check, 1 when touch, 2 when two touch) #define touchleftfront 4 // 05 right front touch sensor (0 don't check, 1 when touch, 2 when two touch) #define touchrightfront 5 // 06 left back touch sensor (0 don't check, 1 when touch, 2 when two touch) #define touchleftback 6 // 07 right back touch sensor (0 don't check, 1 when touch, 2 when two touch) #define touchrightback 7 // 08 left line sensor (0 don't check, 1 when black, 2 when two black) #define lineleft 8 // 09 right line sensor (0 don't check, 1 when black, 2 when two black) #define lineright 09 // 10 beyond way point (continue time in milliseconds) #define beyond 10 // 11 new heading (turn -180 to 180) #define head 11 // 12 special function (0=nothing, 1=align front, -1 align rear) // Other special functions include line following or color blob tracking #define special 12 int points[8][13] = { //(00, 11111, 22222, 33333, 4, 5, 6, 7, 8, 9, AAAA, AAAA, BB), {01, 4000, 750, 700, 1, 1, 0, 0, 0, 0, 0, 0, 0}, // Forward to wall {02, 400, -750, -700, 0, 0, 0, 0, 0, 0, 0, 88, -1}, // Backup, turn and align {03, 9000, 750, 700, 0, 0, 0, 0, 1, 1, 500, 88, -1}, // Forward to line, turn and align {04, 13000, 750, 700, 1, 1, 0, 0, 0, 0, 0, 0, 0}, // Forward to wall {05, 600, -750, -700, 0, 0, 0, 0, 0, 0, 0, 88, 0}, // Backup and turn {06, 8000, 750, 700, 1, 1, 0, 0, 0, 0, 0, 0, 0}, // Forward to wall {07, 300, -750, -700, 0, 0, 0, 0, 0, 0, 0, 88, -1}, // Backup, turn and align {08, 4000, 750, 700, 1, 1, 0, 0, 0, 0, 0, 0, 0} // Forward to wall }; int pointcurr = 0; // Initial way point number int pointmax = _array_size(points); // Max way point int cal = 0; // Calibrate start light int navigatepid = 0; // Navigate process ID int linegray = 170; // Greater is black long endtime = 0L; // Timer stop value // Motor Speed Variables int left_normal = 750; int right_normal = 700; // Main Controller =================================================== void main() { display_clear(); printf("Common Bot V2\n"); msleep(200L); beep(); msleep(200L); beep(); msleep(200L); beep(); msleep(200L); printf("Press A to Calibrate\n"); printf(" Press B to Bypass\n"); while(cal==0) { // Deal with starting light if (a_button()==1) { // Calibrate cal = 1; beep(); wait_for_light(startinglight); } if (b_button()==1) { // Bypass calibrate cal = -1; beep(); msleep(100L); beep(); msleep(500L); } } // Start shut down process shut_down_in(150.0); // Max 180 seconds // Start navigate process navigatepid = start_process(navigate()); msleep(200L); } // Navigator =================================================== void navigate() { // Initiate servos enable_servos(); set_servo_position(rotator,100); msleep(1000L); disable_servos(); stop(); // Loop through way points for (pointcurr=0; pointcurrlinegray) { if (points[pointcurr][lineleft]==2) { if (analog(rightfrontline)>linegray) break; } else { break; } } } if (points[pointcurr][lineright]!=0) { if (analog(rightfrontline)>linegray) { if (points[pointcurr][lineright]==2) { if (analog(leftfrontline)>linegray) break; } else { break; } } } } // End drive loop, way point reached // If go beyond way point if (points[pointcurr][beyond]!=0) { msleep((long)points[pointcurr][beyond]); } stop(); // Then stop driving // If new heading if (points[pointcurr][head]!=0) { heading((long)points[pointcurr][head]); } // If special function if (points[pointcurr][special]!=0) { if (points[pointcurr][special]==1) alignfront(1900L); if (points[pointcurr][special]==-1) alignrear(1900L); } stop(); } // Servos and motors off disable_servos(); stop(); beep(); msleep(200L); beep(); msleep(200L); beep(); msleep(200L); printf("Done!\n"); } // Drive Processes and Functions ============================================= // Motors On void motors(int left_speed, int right_speed) { mav(leftmotor,left_speed); mav(rightmotor,right_speed); } // Motors Off void stop() { mav(leftmotor,0); mav(rightmotor,0); msleep(400L); } // Forward void forward() { motors(left_normal, right_normal); } // Backward void backward() { motors(-left_normal, -right_normal); } // Set new heading void heading(long angle) { printf(" New Heading %l\n", angle); mrp(rightmotor,right_normal,-(angle*6L)); mrp(leftmotor,left_normal,(angle*6L)); bmd(rightmotor); bmd(leftmotor); stop(); } // Align to pipe in rear void alignrear(long maxtime) { endtime = mseconds() + maxtime; // Set end time backward(); while (mseconds()