Atsushi Furusawa
/
m3pi_line_tracer
A simple line tracing program for m3pi.
main.cpp
- Committer:
- fchoco
- Date:
- 2014-12-11
- Revision:
- 0:ee556de472a9
File content as of revision 0:ee556de472a9:
#include "mbed.h" #include "m3pi.h" m3pi m3pi; enum State {RUN, ROTATE}; int main() { float speed = 0.2; float correction = 0.05; int threshold = 500; float turn30 = 0.2; int sens[5]; //left, sub-l, center, sub-r, right State state = RUN; m3pi.cls(); m3pi.locate(0,0); m3pi.printf("Line"); m3pi.locate(0,1); m3pi.printf(" Tracer"); wait(2.0); m3pi.sensor_auto_calibrate(); while(1) { m3pi.putc(0x87); //get values of calibrated sensors (0-1000) for(int i=0; i < 5; ++i) { sens[i] = m3pi.getc(); sens[i] += m3pi.getc() << 8; } switch(state) { case RUN: /* Turn left (over 30deg.) */ if(sens[0] > threshold || sens[2] <= threshold && sens[1] <= threshold && sens[3] <= threshold) { m3pi.stop(); wait(0.1); m3pi.left(speed); wait(turn30); m3pi.stop(); state = ROTATE; } /* Go forward */ else { if(sens[2] > threshold) { // Go straight m3pi.forward(speed); } else if(sens[1] > threshold) { //Correction (left) m3pi.left_motor(speed - correction); } else { //Correction (right) m3pi.right_motor(speed - correction); } } break; case ROTATE: // Rotate clockwise and find a road if(sens[2] > threshold) { m3pi.stop(); state = RUN; } m3pi.right(speed); break; } } }