Atsushi Furusawa
/
m3pi_line_tracer
A simple line tracing program for m3pi.
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?
User | Revision | Line number | New 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 | } |