Yasushi TAUCHI
/
m3pi_LineMaze5
LineMaze
main.cpp
- Committer:
- yueee_yt
- Date:
- 2012-06-26
- Revision:
- 0:85792b24326d
File content as of revision 0:85792b24326d:
#include "mbed.h" #include "m3pi.h" m3pi m3pi; // Minimum and maximum motor speeds #define MAX 0.3 #define MAX2 0.6 #define MIN 0 // PID terms #define P_TERM 0.6 #define I_TERM 0 #define D_TERM 20 DigitalIn sw(p21); char path[100] = ""; unsigned char path_length = 0; void follow_segment(void); void follow_segment2(void); void m3pi_play( char x[]) { m3pi.putc(0xb3); int i; m3pi.putc(strlen(x)); for (i=0; i<strlen(x); i++) { m3pi.putc(x[i]); } } void read_line(unsigned int sensors[5]) { unsigned char x[10]; int i; m3pi.putc(0x87); for (i=0; i<10; i++) { x[i]=m3pi.getc(); } sensors[0]=(x[1]*0x100+x[0])*2; sensors[1]=(x[3]*0x100+x[2])*2; sensors[2]=(x[5]*0x100+x[4])*2; sensors[3]=(x[7]*0x100+x[6])*2; sensors[4]=(x[9]*0x100+x[8])*2; } char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right) { // Make a decision about how to turn. The following code // implements a left-hand-on-the-wall strategy, where we always // turn as far to the left as possible. if (found_left) return 'L'; else if (found_straight) return 'S'; else if (found_right) return 'R'; else return 'B'; } void simplify_path() { // only simplify the path if the second-to-last turn was a 'B' if (path_length < 3 || path[path_length-2] != 'B') return; int total_angle = 0; int i; for (i=1; i<=3; i++) { switch (path[path_length-i]) { case 'R': total_angle += 90; break; case 'L': total_angle += 270; break; case 'B': total_angle += 180; break; } } // Get the angle as a number between 0 and 360 degrees. total_angle = total_angle % 360; // Replace all of those turns with a single one. switch (total_angle) { case 0: path[path_length - 3] = 'S'; break; case 90: path[path_length - 3] = 'R'; break; case 180: path[path_length - 3] = 'B'; break; case 270: path[path_length - 3] = 'L'; break; } // The path is now two steps shorter. path_length -= 2; } void turn(char dir) { float a=0.0347; switch (dir) { case 'L': // Turn left. m3pi_play("T240ec"); m3pi.left(0.2); wait(0.3125+a); break; case 'R': // Turn right. m3pi_play("T240ce"); m3pi.right(0.2); wait(0.3125+a); break; case 'B': // Turn around. m3pi_play("T240cc"); m3pi.left(0.2); wait(0.625+a); break; case 'S': // Don't do anything! break; } } void turn2(char dir) { float a=0.0347; switch (dir) { case 'L': // Turn left. m3pi_play("T240ec"); m3pi.right_motor(0.0); m3pi.left_motor(0.6); wait(0.223); break; case 'R': // Turn right. m3pi_play("T240ce"); m3pi.right_motor(0.6); m3pi.left_motor(0); wait(0.223); break; case 'B': // Turn around. m3pi_play("T240cc"); m3pi.left(0.2); wait(0.625+a); break; case 'S': // Don't do anything! m3pi.forward(0.3); wait(0.2+a); break; } } int main() { unsigned int sensors[5]; sw.mode(PullUp); m3pi.locate(0,1); m3pi.printf("LineMaze"); m3pi.leds(0); wait(2.0); unsigned char ll=0; m3pi.sensor_auto_calibrate(); while (1) { follow_segment(); m3pi.left_motor(0.2); m3pi.right_motor(0.2); wait(0.02); //m3pi.left_motor(0);m3pi.right_motor(0); unsigned char found_left=0; unsigned char found_straight=0; unsigned char found_right=0; ll=0; read_line(sensors); if (sensors[0] > 1000) { found_left = 1; ll=ll|0x04; } if (sensors[4] > 1000) { found_right = 1; ll=ll|0x01; } wait(0.186); read_line(sensors); if (sensors[1] > 1000 || sensors[2] > 1000 || sensors[3] > 1000) { found_straight = 1; ll=ll|0x02; } if (sensors[1] > 1000 && sensors[2] > 1000 && sensors[3] > 1000) { ll=ll|0x08; break; // return; } unsigned char dir = select_turn(found_left, found_straight, found_right); turn(dir); path[path_length] = dir; path_length ++; // You should check to make sure that the path_length does not // exceed the bounds of the array. We'll ignore that in this // example. // Simplify the learned path. simplify_path(); m3pi.leds(ll); } m3pi.stop(); while (1) { if (sw==0)break; } wait(1); //2nd loop int i; for (i=0; i<path_length; i++) { // SECOND MAIN LOOP BODY follow_segment2(); // Drive straight while slowing down, as before. // m3pi.left_motor(0.2); // m3pi.right_motor(0.2); // wait(0.02); // read_line(sensors); // wait(0.186); // Make a turn according to the instruction stored in // path[i]. turn2(path[i]); } // Follow the last segment up to the finish. follow_segment2(); m3pi.stop(); } void follow_segment() { unsigned int sensors[5]; float right; float left; float current_pos_of_line = 0.0; float previous_pos_of_line = 0.0; float derivative,proportional,integral = 0; float power; float speed = MAX; int k=0; while (1) { // Get the position of the line. current_pos_of_line = m3pi.line_position(); proportional = current_pos_of_line; // Compute the derivative derivative = current_pos_of_line - previous_pos_of_line; // Compute the integral integral += proportional; // Remember the last position. previous_pos_of_line = current_pos_of_line; // Compute the power power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; // Compute new speeds right = speed+power; left = speed-power; // limit checks if (right < MIN) right = MIN; else if (right > MAX) right = MAX; if (left < MIN) left = MIN; else if (left > MAX) left = MAX; // set speed m3pi.left_motor(left); m3pi.right_motor(right); //Read Sensor if (k>200) { read_line(sensors); if (sensors[1] < 650 && sensors[2] < 650 && sensors[3] < 650) { // There is no line visible ahead, and we didn't see any // intersection. Must be a dead end. m3pi.leds(0x07); return; } else if (sensors[0] > 1000 || sensors[4] > 1000) { // Found an intersection. m3pi.leds(0x0f); return; } } k++; } } void follow_segment2() { unsigned int sensors[5]; float right; float left; float current_pos_of_line = 0.0; float previous_pos_of_line = 0.0; float derivative,proportional,integral = 0; float power; float speed = MAX2; int k=0; while (1) { // Get the position of the line. current_pos_of_line = m3pi.line_position(); proportional = current_pos_of_line; // Compute the derivative derivative = current_pos_of_line - previous_pos_of_line; // Compute the integral integral += proportional; // Remember the last position. previous_pos_of_line = current_pos_of_line; // Compute the power power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; // Compute new speeds right = speed+power; left = speed-power; // limit checks if (right < MIN) right = MIN; else if (right > MAX2) right = MAX2; if (left < MIN) left = MIN; else if (left > MAX2) left = MAX2; // set speed m3pi.left_motor(left); m3pi.right_motor(right); //Read Sensor if (k>100) { read_line(sensors); if (sensors[1] < 650 && sensors[2] < 650 && sensors[3] < 650) { // There is no line visible ahead, and we didn't see any // intersection. Must be a dead end. m3pi.leds(0x07); return; } else if (sensors[0] > 1000 || sensors[4] > 1000) { // Found an intersection. m3pi.leds(0x0f); return; } } k++; } }