main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 
00004 m3pi m3pi;
00005 
00006 enum State {RUN, ROTATE};
00007 
00008 int main() {
00009     float speed = 0.2;
00010     float correction = 0.05;   
00011     int threshold = 500;
00012     float turn30 = 0.2;
00013 
00014     int sens[5]; //left, sub-l, center, sub-r, right
00015     State state = RUN;
00016         
00017     m3pi.cls();
00018     m3pi.locate(0,0);
00019     m3pi.printf("Line");
00020     m3pi.locate(0,1);
00021     m3pi.printf("  Tracer");
00022 
00023     wait(2.0);
00024     
00025     m3pi.sensor_auto_calibrate();
00026 
00027     
00028     while(1) {
00029         m3pi.putc(0x87); //get values of calibrated sensors (0-1000)
00030         for(int i=0; i < 5; ++i) {
00031             sens[i] = m3pi.getc();
00032             sens[i] += m3pi.getc() << 8;
00033         }
00034 
00035         switch(state) {
00036         case RUN:
00037             /* Turn left (over 30deg.) */
00038             if(sens[0] > threshold ||
00039                sens[2] <= threshold && sens[1] <= threshold && sens[3] <= threshold) {
00040                 m3pi.stop();
00041                 wait(0.1);
00042                 m3pi.left(speed);
00043                 wait(turn30);
00044                 m3pi.stop();
00045             
00046                 state = ROTATE;
00047             }
00048             /* Go forward */
00049             else {
00050                 if(sens[2] > threshold) {  // Go straight
00051                   m3pi.forward(speed);
00052                 } else if(sens[1] > threshold) { //Correction (left)
00053                     m3pi.left_motor(speed - correction);
00054                 } else { //Correction (right)
00055                     m3pi.right_motor(speed - correction);
00056                 }
00057             }
00058             break;
00059         case ROTATE: // Rotate clockwise and find a road
00060             if(sens[2] > threshold) {
00061                 m3pi.stop();
00062                 state = RUN;
00063             }
00064             m3pi.right(speed);
00065             break;
00066         }
00067     }
00068 }