Atsushi Furusawa / Mbed 2 deprecated m3pi_line_tracer

A simple line tracing program for m3pi.

Dependencies:   m3pi mbed

main.cpp@0:ee556de472a9, 2014-12-11 (annotated)

Committer:
fchoco
Date:
Thu Dec 11 08:44:55 2014 +0000
Revision:
0:ee556de472a9
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fchoco 0:ee556de472a9 1 #include "mbed.h"
fchoco 0:ee556de472a9 2 #include "m3pi.h"
fchoco 0:ee556de472a9 3
fchoco 0:ee556de472a9 4 m3pi m3pi;
fchoco 0:ee556de472a9 5
fchoco 0:ee556de472a9 6 enum State {RUN, ROTATE};
fchoco 0:ee556de472a9 7
fchoco 0:ee556de472a9 8 int main() {
fchoco 0:ee556de472a9 9 float speed = 0.2;
fchoco 0:ee556de472a9 10 float correction = 0.05;
fchoco 0:ee556de472a9 11 int threshold = 500;
fchoco 0:ee556de472a9 12 float turn30 = 0.2;
fchoco 0:ee556de472a9 13
fchoco 0:ee556de472a9 14 int sens[5]; //left, sub-l, center, sub-r, right
fchoco 0:ee556de472a9 15 State state = RUN;
fchoco 0:ee556de472a9 16
fchoco 0:ee556de472a9 17 m3pi.cls();
fchoco 0:ee556de472a9 18 m3pi.locate(0,0);
fchoco 0:ee556de472a9 19 m3pi.printf("Line");
fchoco 0:ee556de472a9 20 m3pi.locate(0,1);
fchoco 0:ee556de472a9 21 m3pi.printf(" Tracer");
fchoco 0:ee556de472a9 22
fchoco 0:ee556de472a9 23 wait(2.0);
fchoco 0:ee556de472a9 24
fchoco 0:ee556de472a9 25 m3pi.sensor_auto_calibrate();
fchoco 0:ee556de472a9 26
fchoco 0:ee556de472a9 27
fchoco 0:ee556de472a9 28 while(1) {
fchoco 0:ee556de472a9 29 m3pi.putc(0x87); //get values of calibrated sensors (0-1000)
fchoco 0:ee556de472a9 30 for(int i=0; i < 5; ++i) {
fchoco 0:ee556de472a9 31 sens[i] = m3pi.getc();
fchoco 0:ee556de472a9 32 sens[i] += m3pi.getc() << 8;
fchoco 0:ee556de472a9 33 }
fchoco 0:ee556de472a9 34
fchoco 0:ee556de472a9 35 switch(state) {
fchoco 0:ee556de472a9 36 case RUN:
fchoco 0:ee556de472a9 37 /* Turn left (over 30deg.) */
fchoco 0:ee556de472a9 38 if(sens[0] > threshold ||
fchoco 0:ee556de472a9 39 sens[2] <= threshold && sens[1] <= threshold && sens[3] <= threshold) {
fchoco 0:ee556de472a9 40 m3pi.stop();
fchoco 0:ee556de472a9 41 wait(0.1);
fchoco 0:ee556de472a9 42 m3pi.left(speed);
fchoco 0:ee556de472a9 43 wait(turn30);
fchoco 0:ee556de472a9 44 m3pi.stop();
fchoco 0:ee556de472a9 45
fchoco 0:ee556de472a9 46 state = ROTATE;
fchoco 0:ee556de472a9 47 }
fchoco 0:ee556de472a9 48 /* Go forward */
fchoco 0:ee556de472a9 49 else {
fchoco 0:ee556de472a9 50 if(sens[2] > threshold) { // Go straight
fchoco 0:ee556de472a9 51 m3pi.forward(speed);
fchoco 0:ee556de472a9 52 } else if(sens[1] > threshold) { //Correction (left)
fchoco 0:ee556de472a9 53 m3pi.left_motor(speed - correction);
fchoco 0:ee556de472a9 54 } else { //Correction (right)
fchoco 0:ee556de472a9 55 m3pi.right_motor(speed - correction);
fchoco 0:ee556de472a9 56 }
fchoco 0:ee556de472a9 57 }
fchoco 0:ee556de472a9 58 break;
fchoco 0:ee556de472a9 59 case ROTATE: // Rotate clockwise and find a road
fchoco 0:ee556de472a9 60 if(sens[2] > threshold) {
fchoco 0:ee556de472a9 61 m3pi.stop();
fchoco 0:ee556de472a9 62 state = RUN;
fchoco 0:ee556de472a9 63 }
fchoco 0:ee556de472a9 64 m3pi.right(speed);
fchoco 0:ee556de472a9 65 break;
fchoco 0:ee556de472a9 66 }
fchoco 0:ee556de472a9 67 }
fchoco 0:ee556de472a9 68 }