File content as of revision 29:0419f4873807:
/*----------------------------------------------------------------------------*
* Micromouse PES2
*
* PES2 2018, TEAM 3
*
* Code entwickelt von:
*
*
*-----------------------------------------------------------------------------*/
#include <mbed.h>
#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
#include "Motion.h"
//User Button
InterruptIn button(USER_BUTTON);
//Sensors:
AnalogIn lineSensor(PC_5);
AnalogIn distance2(PC_3);
AnalogIn distance4(PB_1);
AnalogIn distance1(PC_2);
IRSensor irSensorL (distance2);
IRSensor irSensorC (distance4);
IRSensor irSensorR (distance1);
Timer t1;
//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 HALF_MOVE = 4;
const int TURN_LEFT = 5;
const int TURN_RIGHT = 6;
const int EMPTY = 7;
//Sensor tresholds [mm]
const float thresholdL = 80;
const float thresholdR = 80;
const float thresholdC = 150;
//------------------------------------------------------------------------------
//User button toggle
void press() {
start = !start;
}
//------------------------------------------------------------------------------
int main() {
//Init
int route[200] = {0};
int r = 0;
int junction[20] = {0};
int j = 0;
short lWall;
short cWall;
short rWall;
short Ziel = 0;
//infinite loop
while(1) {
button.fall(&press); //User button einlesen
/*-----------------------------------------------------------------------------*
*
* Search run
*
*-----------------------------------------------------------------------------*/
while(start == 1 && Ziel == 0) {
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] != r) {
if (junction[j] > 0) {
j += 1;
junction[j] = r;
}else{
junction[j] = r;
}
}
// 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();
}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();
}else if (route[r] == RIGHT) {
// Kreuzung führt zu Sackgassen -> löschen
junction[j] = 0;
// printf("Kreuzung %d Schritt %d geloscht\n", j, r);
j -= 1;
while (junction[j] < r ) {
//invert rotation
if (route[r] == LEFT) {
route[r] = RIGHT;
}else if (route[r] == RIGHT) {
route[r] = LEFT;
}
motion.runTask(route, r, true, junction[j]);
// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
motion.stop();
// printf("Loop stop\n");
}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();
}
//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();
}else if (route[r] == MOVE) {
junction[j] = 0;
// printf("Kreuzung %d Schritt %d geloscht\n", j, r);
j -= 1;
while (junction[j] < r ) {
//invert rotation
if (route[r] == LEFT) {
route[r] = RIGHT;
}else if (route[r] == RIGHT) {
route[r] = LEFT;
}
motion.runTask(route, r, true, junction[j]);
// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
motion.stop();
// printf("Loop stop\n");
}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();
//Dead end routine
}else if ((lWall + cWall + rWall) == 3) {
motion.rotate180();
// printf("Sackgasse Schritt: %d\n", r);
r--;
t1.reset();
t1.start();
while (t1 < 0.5f) {}
t1.stop();
//Return to last junction
while (junction[j] <= r ) {
if (junction[j] == r) {
switch (route[r]) {
case MOVE:
motion.runTask(route, r, true, junction[j]);
// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r--;
break;
case LEFT:
r--;
break;
case RIGHT:
r--;
break;
}
motion.stop();
t1.reset();
t1.start();
while (t1 < 0.5f) {}
t1.stop();
}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, true, junction[j]);
// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
}
r++;
}
//finish line check
if (motion.finish() == 1) {
Ziel = 1;
r = 1;
//Convert rotations to smooth turns
while (route[r] != 0) {
//Convert pattern MOVE - ROTATE - MOVE
if (route[r-1] == MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) {
route[r-1] = HALF_MOVE;
route[r+1] = HALF_MOVE;
if (route[r] == LEFT) route[r] = TURN_LEFT;
else route[r] = TURN_RIGHT;
//Convert consequent ROTATION to a smooth TURN
}else if (route[r-1] == HALF_MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) {
route[r-1] = EMPTY;
route[r+1] = HALF_MOVE;
if (route[r] == LEFT) route[r] = TURN_LEFT;
else route[r] = TURN_RIGHT;
}
r++;
}
r = 0;
start = 0;
controller.counterReset();
myled = 1;
}else{
Ziel = 0;
}
}
/*-------------------------------------------------------------------------*
*
* Speed run
*
*-------------------------------------------------------------------------*/
while ( start == 1 && Ziel == 1 ) {
motion.runTask(route,r,false,junction[j]);
r++;
if (route[r] == 0) {
//Weg fertig
motion.stop();
start = 0;
myled = 0;
}
}
}
}
Helvijs Kiselis
