Diff: main.cpp
- Revision:
- 1:2b5f79285a3e
- Parent:
- 0:8491169be8fc
- Child:
- 2:f898adf2d817
--- a/main.cpp Mon Apr 16 10:27:10 2018 +0000
+++ b/main.cpp Thu Apr 19 06:19:43 2018 +0000
@@ -1,5 +1,24 @@
+/**
+ * Micromouse PES2
+ *
+ * Suchfahrtalg. + Schnellfahrtalg.
+ *
+ */
+
+ /* todo:
+
+ - Regler gerade Fahrt
+ > Mitte Werte L/R
+ - Suchfahrt testen
+ > Tresholds bestimmen
+ - gespeicherten Weg fahren
+ - Problem: Links beschleunigt schneller
+ - Ziellinie
+ - Beschleunigung
+ - über längere Strecke schneller fahren
+
+*/
#include <mbed.h>
-
#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
@@ -38,21 +57,23 @@
Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
- Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, enableMotorDriver);
+ Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC,
+ irSensorR, lineSensor, enableMotorDriver);
//------------------------------------------------------------------------------
volatile int start = 0;
- short countsLeft = 0;
- short countsRight = 0;
- short countsLeftOld = 0;
- short countsRightOld = 0;
- //den jeweiligen Test auf 1 setzen
- int testSensor = 0;
- int testMotion = 0;
- int testRotation = 0;
- int testCounts = 0;
- int testClass = 0;
+
+ const int MOVE = 1;
+ const int LEFT = 2;
+ const int RIGHT = 3;
+ const int SPEED = 4;
+ const int STOP = 5;
+
+ //Sensor tresholds [mm]
+ const float thresholdL = 70;
+ const float thresholdR = 70;
+ const float thresholdC = 100;
//------------------------------------------------------------------------------
@@ -60,173 +81,220 @@
void press() {
start = !start;
}
+
+//Return to last junction
+void reverseToJunction(int& junc, int& r, int route[]) {
+
+ while (junc < r ) {
+
+ //invert rotation
+ if (route[r] == LEFT) {
+
+ route[r] = RIGHT;
+
+ }else if (route[r] == RIGHT) {
+
+ route[r] = LEFT;
+ }
+
+ motion.runTask(route[r]);
+ route[r] = 0;
+ r--;
+ }
+}
//------------------------------------------------------------------------------
int main() {
+//Init
-/**
- Micromouse Test
-*/
+ int route[50] = {0};
+ int r = 0;
+
+ int junction[5] = {0};
+ int j = 0;
+ short lWall;
+ short cWall;
+ short rWall;
+ short Ziel = 0;
+
+//loop
while(1) {
button.fall(&press); //User button einlesen
- while(0) {
- myled = 1;
- printf("Hai\n");
- if (start == 0) break;
- }
+ /**
+ *
+ * Search run
+ *
+ */
+
+ while(start == 1 && Ziel == 0) {
-
-
- /*
- Sensoren Test
- */
- while(1) {
-
-
+ /**
+ * Entscheidung und Bewegung
+ */
+
+
float distanceL = irSensorL.readL();
float distanceC = irSensorC.readC();
float distanceR = irSensorR.readR();
- float distanceB = irSensorB.readC();
- printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
-
- //wait(0.5f);
- }
-
- /*
- Motoren Test: Gerade Bewegung
- */
- while(start == 1 && testMotion == 1) {
- controller.setDesiredSpeedLeft(20.0f); //Drehzahl in [rpm]
- controller.setDesiredSpeedRight(-20.0f);
- enableMotorDriver = 1; //Schaltet den Leistungstreiber ein
-
- countsLeft = counterLeft.read();
- countsRight = counterRight.read();
+ //Wall check
+ if (distanceL < thresholdL) lWall = 1;
+ else lWall = 0;
+ if (distanceC < thresholdC) cWall = 1;
+ else cWall = 0;
+ if (distanceR < thresholdR) rWall = 1;
+ else rWall = 0;
- printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
-
- if (start == 0) {
+ //Junction Check
+ if ((lWall + cWall + rWall) < 2) {
- motion.stop();
+ if (junction[j] > 0) {j++;}
+ junction[j] = r;
}
- }
-
- /*
- Motoren Test: 180° Rotation
- */
-
- while(start == 1 && testRotation == 1) {
+ //No wall left
+ if (lWall == 0) {
+ if (route[r] == LEFT) {
+
+ route[r] = MOVE;
+ r++;
+
+ motion.rotateL();
+ motion.scanMove();
+ motion.stop();
+
+ }else if (route[r] == MOVE) {
+
+ route[r] = RIGHT;
+ r++;
+ route[r] = MOVE;
+ r++;
+
+ motion.rotateL();
+ motion.scanMove();
+ motion.stop();
+
+ }else if (route[r] == RIGHT) {
+ // Kreuzung führt zu Sackgassen -> löschen
+ junction[j] = 0;
+ if (j > 0) {j--;}
+ reverseToJunction(junction[j], r, route);
+
+ }else{
+
+ route[r] = LEFT;
+ r++;
+ route[r] = MOVE;
+ r++;
+
+ motion.rotateL();
+ motion.scanMove();
+ motion.stop();
+ }
+ //No wall center
+ }else if (cWall == 0) {
- while((countsLeft - countsLeftOld) > -1614 || (countsRight - countsRightOld) > -1614) {
+ if (route[r] == LEFT) {
+
+ route[r] = RIGHT;
+ r++;
+ route[r] = MOVE;
+ r++;
- countsLeft = counterLeft.read();
- countsRight = counterRight.read();
-
- controller.setDesiredSpeedLeft(-50.0f);
- controller.setDesiredSpeedRight(-50.0f);
- enableMotorDriver = 1;
+ motion.scanMove();
+ motion.stop();
+
+ }else if (route[r] == MOVE) {
+
+ junction[j] = 0;
+ if (j > 0) {j--;}
+ reverseToJunction(junction[j], r, route);
+
+ }else{
+
+ route[r] = MOVE;
+ r++;
+
+ motion.scanMove();
+ motion.stop();
+ }
+ //No wall right
+ }else if (rWall == 0) {
+
+ route[r] = RIGHT;
+ r++;
+ route[r] = MOVE;
+ r++;
+
+ motion.rotateR();
+ motion.scanMove();
+ motion.stop();
+
+ //Dead end routine
+ }else if ((lWall + cWall + rWall) == 3) {
+
+ motion.rotate180();
+ r--;
+ //Return to last junction
+ while (junction[j] < r ) {
+
+ //invert rotation
+ if (route[r] == LEFT) {
+
+ route[r] = RIGHT;
+
+ }else if (route[r] == RIGHT) {
+
+ route[r] = LEFT;
+ }
+ //Run tasks in declining order
+ motion.runTask(route[r]);
+ route[r] = 0;
+ r--;
+ }
}
-
- motion.stop();
- enableMotorDriver = 0;
- printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
-
- if (start == 0) {
- countsLeftOld = countsLeft - 100;
- countsRightOld = countsRight - 100;
-
- break;
+
+ if (motion.finish() == 1) {
+ Ziel = 1;
+ r = 0;
+ start = 0;
+ }else{
+ Ziel = 0;
}
- }
-/* Benötigte Ticks für 180* Rotation am Ort:
-
- mm/count = 0.122
- Dr = 124mm
- Rr = 60mm
- 180°=> 1597 counts
- 90°=> 798 counts
-
-
- RotationsDistanz Rd= DistanzRäder*pi/2 = 194.8
-
- UmfangRad = Drad*pi = 188.5
-
-
- Counts = Rd/mm pro Count
+ }
- => Counts pro Drehung rausfinden */
- while(start == 1 && testCounts == 1) {
-
- while(countsLeft < 5000 || countsRight > -5000) {
-
- countsLeft = counterLeft.read();
- countsRight = counterRight.read();
+ /**
+ *
+ * Speed run
+ *
+ */
+
+ while ( start == 1 && Ziel == 1 ) {
- controller.setDesiredSpeedLeft(20.0f);
- controller.setDesiredSpeedRight(-20.0f);
- enableMotorDriver = 1;
+ if (route[r] == route[r+1] && route[r] == route[r+2]) {
+ //Auf längere Strecke schneller fahren
+ route[r+1] = SPEED;
}
+ motion.runTask(route[r]);
+ r++;
- controller.setDesiredSpeedLeft(0.0f);
- controller.setDesiredSpeedRight(0.0f);
- enableMotorDriver = 0;
-
+ if (route[r] == 0) {
+ //Weg fertig
+ start = 0;
+ }
}
- //Class test
- if(start == 1 && testClass == 1) {
- motion.rotate180();
- counterLeft.reset();
- counterRight.reset();
- start = 0;
- }
-
- //Sensor calib
- if(start == 1) {
- int i;
- myled = !myled;
- for (i=0;i<5;i++) {
- float distanceL = irSensorL.readL();
- float distanceC = irSensorC.readC();
- float distanceR = irSensorR.readR();
- float distanceB = irSensorB.readC();
- printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
- }
- printf("------------------------\n");
- start = 0;
-
- /* while((countsLeft - countsLeftOld) > -82 || (countsRight - countsRightOld) < 82) {
-
- controller.setDesiredSpeedLeft(-10.0f); //Drehzahl in [rpm]
- controller.setDesiredSpeedRight(10.0f);
- enableMotorDriver = 1; //Schaltet den Leistungstreiber ein
-
- countsLeft = counterLeft.read();
- countsRight = counterRight.read();
- }
-
- controller.setDesiredSpeedLeft(0.0f);
- controller.setDesiredSpeedRight(0.0f);
- enableMotorDriver = 0;
-
- float distanceL = irSensorB.read();
- printf("Hinten: %f\n", distanceL);
-
- countsLeftOld = countsLeft;
- countsRightOld = countsRight;
- start = 0;*/
-
- }
+
+
+
}
}
Helvijs Kiselis
