File content as of revision 9:a01f90f88920:
/**
* Micromouse PES2
*
* Suchfahrtalg. + Schnellfahrtalg.
*
*/
/* todo:
- Regler gerade Fahrt
- gespeicherten Weg fahren
Optimieren:
- Beschleunigung
- Zentrieren
- über längere Strecke schneller fahren
- Abbiegen
*/
#include <mbed.h>
#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
#include "Motion.h"
//User Button
InterruptIn button(USER_BUTTON);
//Sensors:
AnalogIn lineSensor(PA_4);
AnalogIn distance2(PC_3);
AnalogIn distance4(PB_1);
AnalogIn distance1(PC_2);
AnalogIn distance3(PC_5);
IRSensor irSensorL (distance2);
IRSensor irSensorC (distance4);
IRSensor irSensorR (distance1);
IRSensor irSensorB (distance3);
//Motors:
DigitalOut myled(LED1);
DigitalOut enableMotorDriver(PB_2);
DigitalIn motorDriverFault(PB_14);
DigitalIn motorDriverWarning(PB_15);
PwmOut pwmRight(PA_8);
PwmOut pwmLeft(PA_10);
EncoderCounter counterRight(PB_6, PB_7);
EncoderCounter counterLeft(PA_0, PA_1);
Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC,
irSensorR, lineSensor, enableMotorDriver);
//------------------------------------------------------------------------------
volatile int start = 0;
const int MOVE = 1;
const int LEFT = 2;
const int RIGHT = 3;
const int SPEED = 4;
//Sensor tresholds [mm]
const float thresholdL = 80;
const float thresholdR = 80;
const float thresholdC = 100;
//------------------------------------------------------------------------------
//User button toggle
void press() {
start = !start;
}
//Return to last junction
void reverseToJunction(int junc[], int j, int r, int route[]) {
while (junc[j-1] < r ) {
//invert rotation
if (route[r] == LEFT) {
route[r] = RIGHT;
}else if (route[r] == RIGHT) {
route[r] = LEFT;
}
motion.runTask(route[r]);
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
}
//------------------------------------------------------------------------------
int main() {
//Init
int route[200] = {0};
volatile int r = 0;
int junction[20] = {0};
int j = 0;
short lWall;
short cWall;
short rWall;
short Ziel = 0;
//loop
while(1) {
button.fall(&press); //User button einlesen
/**
*
* Search run
*
*/
while(start == 1 && Ziel == 0) {
/**
* Entscheidung und Bewegung
*/
float distanceL = irSensorL.readL();
float distanceC = irSensorC.readC();
float distanceR = irSensorR.readR();
//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;
//Junction Check
if ((lWall + cWall + rWall) < 2) {
if (junction[j-1] != r) {
junction[j] = r;
j++;
}
printf("Kreuzung: %d Schritt: %d\n", j, r);
}
//No wall left
if (lWall == 0) {
if (route[r] == LEFT) {
route[r] = MOVE;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateL();
motion.scanMove();
motion.stop();
}else if (route[r] == MOVE) {
route[r] = RIGHT;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateL();
motion.scanMove();
motion.stop();
}else if (route[r] == RIGHT) {
// Kreuzung führt zu Sackgassen -> löschen
junction[j] = 0; /*Änderung alex*/
//if (j > 0) {j--;}
printf("Kreuzung %d Schritt %d geloscht\n", j, r);
j--;
//reverseToJunction(junction, j, r, route);
/*for (int i = r; i > junction[j]; i--) {
r = i;
//invert rotation
if (route[r] == LEFT) {
route[r] = RIGHT;
}else if (route[r] == RIGHT) {
route[r] = LEFT;
}
motion.runTask(route[r]);
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
}
printf("Loop fertig\n");
r--;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
*/
while (junction[j-1] < r ) {
//invert rotation
if (route[r] == LEFT) {
route[r] = RIGHT;
}else if (route[r] == RIGHT) {
route[r] = LEFT;
}
motion.runTask(route[r]);
printf("Schritt: %d, Befehl: %d\n", r, route[r]); //At work!!!!!
route[r] = 0;
r--;
}
printf("loop end");
}else{
printf("else");
route[r] = LEFT;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateL();
motion.scanMove();
motion.stop();
}
//No wall center
}else if (cWall == 0) {
if (route[r] == LEFT) {
route[r] = RIGHT;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.scanMove();
motion.stop();
}else if (route[r] == MOVE) {
junction[j] = 0; /*Änderung alex*/
//if (j > 0) {j--;}
printf("Kreuzung %d Schritt %d geloscht\n", j, r);
j--;
//reverseToJunction(junction[j], r, route);
/*for (int i = r; i > junction[j]; i--) {
r = i;
//invert rotation
if (route[r] == LEFT) {
route[r] = RIGHT;
}else if (route[r] == RIGHT) {
route[r] = LEFT;
}
motion.runTask(route[r]);
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
}
printf("Loop fertig\n");
r--;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
*/
while (junction[j-1] < r ) {
//invert rotation
if (route[r] == LEFT) {
route[r] = RIGHT;
}else if (route[r] == RIGHT) {
route[r] = LEFT;
}
motion.runTask(route[r]);
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
}else{
route[r] = MOVE;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.scanMove();
}
//No wall right
}else if (rWall == 0) {
route[r] = RIGHT;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateR();
motion.scanMove();
motion.stop();
//Dead end routine
}else if ((lWall + cWall + rWall) == 3) {
motion.rotate180();
printf("Sackgasse Schritt: %d\n", r);
r--;
//Return to last junction
while (junction[j-1] <= r ) {
//for (int i = r; i >= junction[j]; i--
if (junction[j-1] == r) {
switch (route[r]) {
case MOVE:
motion.runTask(route[r]);
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r--;
break;
case LEFT:
r--;
break;
case RIGHT:
r--;
break;
}
}else{
//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]);
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
}
r++;
}
if (motion.finish() == 1) {
/*while(r > 0) {
//Zum Start zurückfahren
if (route[r] == LEFT) {
route[r] = RIGHT;
}else if (route[r] == RIGHT) {
route[r] = LEFT;
}
motion.runTask(route[r]);
r--;
}
motion.runTask(route[r])
*/
Ziel = 1;
r = 0;
start = 0;
controller.counterReset();
}else{
Ziel = 0;
}
}
/**
*
* Speed run
*
*/
while ( start == 1 && Ziel == 1 ) {
/*if (route[r] == route[r+1] && route[r] == route[r+2]) {
//Auf längere Strecke schneller fahren
route[r+1] = SPEED;
}*/
float distanceL = irSensorL.readL();
float distanceC = irSensorC.readC();
float distanceR = irSensorR.readR();
motion.runTask(route[r]);
r++;
if (route[r] == 0) {
//Weg fertig
motion.stop();
start = 0;
}
}
}
}
Helvijs Kiselis
