Yasushi TAUCHI
LineMaze
Diff: main.cpp
- Revision:
- 0:85792b24326d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jun 26 03:02:06 2012 +0000
@@ -0,0 +1,374 @@
+#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++;
+ }
+}
\ No newline at end of file