Atsushi Furusawa
A simple line tracing program for m3pi.
Revision 0:ee556de472a9, committed 2014-12-11
- Comitter:
- fchoco
- Date:
- Thu Dec 11 08:44:55 2014 +0000
- Commit message:
- initial commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pi.lib Thu Dec 11 08:44:55 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/chris/code/m3pi/#4b7d6ea9b35b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Dec 11 08:44:55 2014 +0000
@@ -0,0 +1,68 @@
+#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;
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Dec 11 08:44:55 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file