Final version of Polulu M2 SETI
Dependencies: FileSystem_POPS m3PI_TP_POPS_II2015v0 m3pi mbed
Fork of m3PI_TP_POPS_II2015v0 by
Diff: main3.cpp
- Revision:
- 5:bc86a4fb4784
- Parent:
- 4:cdd8e61cd8c7
- Child:
- 6:9690e497319b
--- a/main3.cpp Thu Feb 22 08:21:12 2018 +0000 +++ b/main3.cpp Thu Feb 22 08:58:20 2018 +0000 @@ -49,6 +49,7 @@ void current_state() { unsigned char i; sensors=0; + m3pi.calibrated_sensors(tabsensor); for(i=0; i<5; i++) { sensors = (sensors << 1) | seuil(tabsensor[i]); } @@ -56,7 +57,7 @@ void step() { m3pi.forward(0.4*speed); - wait(0.2); + wait(0.22); // m3pi.stop(); } @@ -69,33 +70,42 @@ */ char PIDf(char commande) { if(commande==1) { + m3pi.cls(); + m3pi.print("PID", 4); + m3pi.cls(); char result; - m3pi.calibrated_sensors(tabsensor); current_state(); switch(sensors) { case 0x00: // Deadend + // Back + m3pi.cls(); + m3pi.print("Case 0", 7); m3pi.stop(); - // Back result = 2; break; case 0x1C: case 0x18: case 0x1E: // Forward/Left or Left Only // m3pi.stop(); step(); - m3pi.calibrated_sensors(tabsensor); current_state(); if ((sensors == 0x00) || (sensors == 0x10)) { // Turn Left + m3pi.cls(); + m3pi.print("Left", 5); result = 3; } else { // Forward + m3pi.cls(); + m3pi.print("Fowar", 5); result = 1; } break; case 0x07: case 0x03: case 0x0F: // Forward/Right or Right Only -> RF // m3pi.stop(); + m3pi.cls(); + m3pi.print("Right", 6); step(); result = 4; break; @@ -103,13 +113,17 @@ // 'T' or Intersection or End -> LR or RFL // m3pi.stop(); step(); - m3pi.calibrated_sensors(tabsensor); current_state(); + m3pi.print("End", 4); if (sensors == 0x1F) { // End + m3pi.cls(); + m3pi.print("The End", 8); result = 0; } else { // Turn Right + m3pi.cls(); + m3pi.print("Inter", 6); result = 4; } /* @@ -170,11 +184,12 @@ char turn(char command) { if(command > 1 && command < 5) { char result; - m3pi.calibrated_sensors(tabsensor); current_state(); switch(command) { case 2: // Turn Back + //m3pi.cls(); + //m3pi.print("Back", 5); if(sensors != 0x01) { m3pi.right(speed); result = 2; @@ -187,6 +202,8 @@ break; case 3: // Turn Left + //m3pi.cls(); + //m3pi.print("Left", 5); if(sensors != 0x10) { m3pi.left(speed); result = 3; @@ -199,6 +216,8 @@ break; case 4: // Turn Right + //m3pi.cls(); + //m3pi.print("Right", 6); if(sensors != 0x01) { m3pi.right(speed); result = 4;