PES2_R2D2.0
/
MicroMouse_MASTER_SIX
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
MicroMouse_MASTER_FIVE
by 
PES2_R2D2.0
main.cpp
- Committer:
- TheDarkDurzo
- Date:
- 2018-05-23
- Revision:
- 11:8c8dba56bfda
- Parent:
- 10:e8110fb94686
File content as of revision 11:8c8dba56bfda:
#include "mbed.h"
#include "IRSensor.h"
#include "EncoderCounter.h"
#include "LowpassFilter.h"
#include "Controller.h"
#include "KontrastSensor.h"
#include "Drive.h"
#include "CheckWalls.h"
#include "Turn.h"
Serial pc(USBTX,USBRX,460800);
//Initialisierung LED/Button
DigitalIn button(USER_BUTTON); //Moduswählknopf
DigitalOut myled(LED1); //Heartbeat (evt auch Anzeige für Modus, vor start
//Initialisierung IR-Sensoren
AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
AnalogIn distance1(PC_3);
AnalogIn distance2(PC_5);
AnalogIn distance3(PB_1);
IRSensor irSensor0(distance0); //rechts
IRSensor irSensor1(distance1); //vorne
IRSensor irSensor2(distance2); //links-vorne
IRSensor irSensor3(distance3); //links-hinten
//Initialisierung Kontrastsensor
AnalogIn kontrast(PC_0);
//Initialisierung Motor und Encoder
DigitalOut enableMotorDriver(PB_2);
PwmOut pwmLeft(PA_8); //Motor links
PwmOut pwmRight(PA_9); //Motor rechts
EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links
EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts
DigitalOut enable(PC_1);
Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
int state = 0;
int mode = 1;
int main()
{
//printf("--------\n\rREADY\n\r--------\n\r");
enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
enable = 1;
int wallRight = 0;
int wallFront = 0;
int wallLeft = 0;
int blackLine = 0;
int dontStop = 0;
int lab[5][10];
int path = 0;
int pathNext = 0;
int pathArr[51];
int directions[51];
int modeStart = 0;
int startx = 0;
int starty = 9;
int ausr = 1;
int c = 0;
int firstMove = 0;
//Funktionsdeklarationen
CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft, dontStop, modeStart, path); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
KontrastSensor kontrastSensor(kontrast, blackLine);
Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3, dontStop, modeStart, path, pathNext); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
while(1) { // Wiederholungsschleife
switch(state) { //Wartemodus 1. Lauf
case 0: {
while (button) {
wait(1.0f); //Die Ruhe vor dem Sturm...
}
firstMove = 1;
state = 1;
break;
}
case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
checkWalls.check(); //prueft wo Waende vorhanden sind
turn.turning();
if(firstMove == 1) {
dontStop = 1;
firstMove = 0;
/* **************************** */
/* Anpassung 16.02.18 */
//if (wallFront == 0) {
// ausr = 1;
// lab[startx][starty] = ausr;
// starty--;
//} else if (wallRight == 20) {
// ausr = 3; // Ausrichtung zu kontrollieren!!!!
// lab[startx][starty] = ausr;
// startx++;
//}
/***********************/
}
//merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth veraendert und speichert diese in ein Array
if(wallLeft == 0) {
switch(ausr) {
case 1: {
ausr = 2;
lab[startx][starty] = ausr;
startx--;
break;
}
case 2: {
ausr = 4;
lab[startx][starty] = ausr;
starty++;
break;
}
case 3: {
ausr = 1;
lab[startx][starty] = ausr;
starty--;
break;
}
case 4: {
ausr = 3;
lab[startx][starty] = ausr;
startx++;
break;
}
}
} else if(wallFront == 0) {
switch(ausr) {
case 1: {
ausr = 1;
lab[startx][starty] = ausr;
starty--;
break;
}
case 2: {
ausr = 2;
lab[startx][starty] = ausr;
startx--;
break;
}
case 3: {
ausr = 3;
lab[startx][starty] = ausr;
startx++;
break;
}
case 4: {
ausr = 4;
lab[startx][starty] = ausr;
starty++;
break;
}
}
} else if(wallRight == 0) {
switch(ausr) {
case 1: {
ausr = 3;
lab[startx][starty] = ausr;
startx++;
break;
}
case 2: {
ausr = 1;
lab[startx][starty] = ausr;
starty--;
break;
}
case 3: {
ausr = 4;
lab[startx][starty] = ausr;
starty++;
break;
}
case 4: {
ausr = 2;
lab[startx][starty] = ausr;
startx--;
break;
}
}
} else {
switch(ausr) {
case 1: {
ausr = 4;
lab[startx][starty] = ausr;
starty++;
break;
}
case 2: {
ausr = 3;
lab[startx][starty] = ausr;
startx++;
break;
}
case 3: {
ausr = 2;
lab[startx][starty] = ausr;
startx--;
break;
}
case 4: {
ausr = 1;
lab[startx][starty] = ausr;
starty--;
break;
}
}
}
drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
lab[startx][starty] = 5;
state = 2;
}
break;
}
case 2: { //Ziel erreicht => Wartemodus fuer 2. Lauf und Berechnung der schnellsten Weges
controller.setDesiredSpeedRight(0.0f);
controller.setDesiredSpeedLeft(0.0f);
startx = 0;
starty = 9;
int num;
c = 0;
//Speichert die 2-Dimensionale Karte in ein 1D-Array welche nun aus anweisungen besteht
while(lab[startx][starty] != 5) {
num = lab[startx][starty];
pathArr[c] = num;
switch(num) {
case 1: {
starty--;
break;
}
case 2: {
startx--;
break;
}
case 3: {
startx++;
break;
}
case 4: {
starty++;
break;
}
case 5: {
}
}//Switch End
myled =! myled; // LED is ON Heartbeat
wait(0.4f);
c++;
}//While Ende
pathArr[c]= 5; //setzt end bit
c=0;
//übersetzt den 1-Dimensionalen Array der Ausrichtung in einen Array nun Anweisungen zur Lösung darstellt
while(pathArr[c]!= 5) {
if(c==0) {
if(pathArr[c]==1) {
directions[c]=1;
} else {
directions[c]=3;
}
} else if(pathArr[c-1]==pathArr[c]) {
directions[c]=1;
} else {
if(pathArr[c]==1) {
if(pathArr[c-1]==3) {
directions[c]=2;
}
if(pathArr[c-1]==2) {
directions[c]=3;
}
if(pathArr[c-1]==4) {
directions[c-1]=4;
}
} else if(pathArr[c]==2) {
if(pathArr[c-1]==1) {
directions[c]=2;
}
if(pathArr[c-1]==3) {
directions[c]=4;
}
if(pathArr[c-1]==4) {
directions[c]=3;
}
} else if(pathArr[c]==3) {
if(pathArr[c-1]==1) {
directions[c]=3;
}
if(pathArr[c-1]==2) {
directions[c]=4;
}
if(pathArr[c-1]==4) {
directions[c]=2;
}
} else if(pathArr[c]==4) {
if(pathArr[c-1]==1) {
directions[c]=4;
}
if(pathArr[c-1]==2) {
directions[c]=2;
}
if(pathArr[c-1]==3) {
directions[c]=3;
}
}
}
c++;
myled =! myled; // LED is ON Heartbeat
wait(0.1f);
}
c = 0;
for(int p=0; p<20; p++) {
//printf("Anw: %d\r\n", directions[p]);
}
for(int p=0; p<9; p++) {
for(int q=0; q<4; q++) {
//printf(" %d ", lab[q][p]);
}
//printf("\r\n");
}
while (button) {
wait(1.0f);
}
blackLine = 0;
firstMove = 1;
state = 3;
myled = 0; // LED is ON Heartbeat
c = 0;
break;
}
case 3: {//2. Lauf Fahrzyklus
//uebergibt Anweisung an turn
modeStart=1;
path = directions[c];
pathNext = directions[c+1];
turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
/**************************/
if(firstMove == 1) {
dontStop = 1;
firstMove = 0;
}
/***********************/
drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
c++;
if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
state = 4;
}
break;
}
case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf
controller.setDesiredSpeedRight(0.0f);
controller.setDesiredSpeedLeft(0.0f);
while (button) {
wait(1.0f);
}
state = 5;
break;
}
default: {
state = 0;
break;
}
}
myled =! myled; // LED is ON Heartbeat
wait(0.1f);
}
}