PES2_R2D2.0
READY TO RUMBLE
Dependencies: mbed
Fork of
Micromouse_alpha_copy_copy
by 
PES2_R2D2.0
Revision 7:5ef09519a6e9, committed 2018-05-07
- Comitter:
- ruesipat
- Date:
- Mon May 07 18:52:04 2018 +0000
- Parent:
- 6:a09d2ee3b82e
- Commit message:
- VERSION 7.5.18
Changed in this revision
--- a/Drive.cpp Wed Apr 25 12:49:48 2018 +0000
+++ b/Drive.cpp Mon May 07 18:52:04 2018 +0000
@@ -7,11 +7,11 @@
using namespace std;
-const float Drive::FRONTDISTANCE = 62.0f; //Abstand Sensor zur VorderWand //DONT TOUCH
-const float Drive::DRIVINGSPEED = 50.0f;//Fahrgeschwindigkeit Drehzahl in [rpm]
-const int Drive::DRIVINGCOUNTS = 1773; //Entspricht Strecke von 20cm //DONT TOUCH
+const float Drive::FRONTDISTANCE = 70.0f; //Abstand Sensor zur VorderWand //DONT TOUCH //62.0f //55.0 110.0
+const float Drive::DRIVINGSPEED = 130.0f;//Fahrgeschwindigkeit Drehzahl in [rpm] //130.0f
+const int Drive::DRIVINGCOUNTS = 1390; //Entspricht Strecke von 20cm //DONT TOUCH /1773 //1800 //1350 /1370
-Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3):
+Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop):
kontrastSensor(kontrastSensor),
counterLeft(counterLeft),
counterRight(counterRight),
@@ -19,7 +19,8 @@
irSensor0(irSensor0),
irSensor1(irSensor1),
irSensor2(irSensor2),
- irSensor3(irSensor3)
+ irSensor3(irSensor3),
+ dontStop(dontStop)
{
@@ -32,7 +33,7 @@
{
controller.reset();
-
+
int countsRight = counterRight.read(); //EncoderCounts auslesen
int countsLeft = counterLeft.read(); //0 - 32767
@@ -43,11 +44,23 @@
int countsRight0 = countsRight; //ReferenzCounts setzten
int countsLeft0 = countsLeft;
- float parallelDif = 0;
- float rightLeftDif = 0;
+ float parallelDif = 0.0f;
+ float rightLeftDif = 0.0f;
+ float leftDif = 0.0f;
+ float rightDif = 0.0f;
- float correction = 0;
- float slowdown = 0;
+ float speedCorrection = 0.0f;
+ int countCorrection = 0;
+
+
+ float softStart = DRIVINGSPEED;
+ float slowdown = 0.0f;
+
+
+ if(dontStop == 2){ //geradeaus
+ softStart =0.0f;
+ }
+
int drive;
@@ -63,13 +76,13 @@
//printf("Los gehts\n");
- while(((countsRight <= countsRight0 + DRIVINGCOUNTS) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS)) && (drive == 1) ) {
+ while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) {
kontrastSensor.check();
countsRight = counterRight.read();
countsLeft = counterLeft.read();
- controller.setDesiredSpeedRight(DRIVINGSPEED - correction - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an
- controller.setDesiredSpeedLeft(-DRIVINGSPEED - correction + slowdown);
+ controller.setDesiredSpeedRight(-softStart + DRIVINGSPEED - speedCorrection - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an //slowdown
+ controller.setDesiredSpeedLeft(softStart - DRIVINGSPEED - speedCorrection + slowdown); //slowdown
//printf("CountsRight%d\n\r", countsRight);
//printf(" CountsLeft%d\n\r", countsLeft);
@@ -78,28 +91,31 @@
//printf("correction: %.0f\n\r", correction);
+
//Bereit fuer neuen Durchgang
- correction = 0;
- slowdown = 0;
+ //speedCorrection = 0.0f;
+ //leftDif = 0.0f;
+ //rightDif = 0.0f;
+ //slowdown = 0.0f;
- //Ermittlung der Differenz Hinten-Vorne
- if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
-
+ //Ermittlung der Differenz Hinten-Vorne (PARALLEL)
+ if((irSensor3.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
+
parallelDif = irSensor3.read()-irSensor2.read(); //differenz hinten vorne bestimmen
//printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
//printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
//printf(" parallelDif: %.0f \n\r", parallelDif);
-
+
} else { //ist wand eine wand nicht vorhanden => keine korrektur
-
+
parallelDif = 0;
}
- //Ermittlung der Differenz Rechts-Links
- if((irSensor0.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
-
+ //Ermittlung der Differenz Rechts-Links (LINKS UND RECHTS WAND VORHANDEN)
+ if((irSensor0.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
+
rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
//printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
//printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
@@ -109,33 +125,96 @@
rightLeftDif = 0;
+ //Ermittlung der Differenz links auf ReferenzWert
+ if(irSensor2.read() < 120.0f) { //irSensor2 => sensorLeftFornt
+
+ leftDif = 60.0f - irSensor2.read(); //differenz links rechts bestimmen
+ //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
+ //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
+ //printf(" leftDif: %.0f \n\r", leftDif);
+
+ } else { //ist wand eine wand nicht vorhanden => keine korrektur
+
+ leftDif = 0;
+
+ }
+
+
+ //Ermittlung der Differenz rechts auf Referenzwert
+ if(irSensor0.read() < 120.0f) { //irSensor0 => sensorRight
+
+ rightDif = irSensor0.read() - 60.0f; //differenz links rechts bestimmen
+ //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
+ //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
+ //printf(" rightDif: %.0f \n\r", rightDif);
+
+ } else { //ist wand eine wand nicht vorhanden => keine korrektur
+
+ rightDif = 0;
+
+ }
+
+
+
+
}
//Berechung Korrektur
- correction = ((0.1f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH
+ speedCorrection = ((0.35f * rightLeftDif) + (0.7f * parallelDif) + (0.7f * leftDif) + (0.7f * rightDif)); //DONT TOUCH 0.2 0.5
+
+
+
+ //Anfahrrampe damit die Raeder nicht durchdrehen
+ softStart = softStart - 3.5f;
+ if (softStart <= 0.0f) {
+ softStart = 0.0f;
+ }
+ //printf(" softStart: %.0f\n\r", softStart);
+
+
+
- //Kontrolle ob vorne Wand...Verlangsamen und Anhalten
-
+ //Kontrolle ob vorne Wand...wenn vorhanden nach vorderer Wand ausrichten und anhalten ansonst nur nach counts anhalten
if(irSensor1.read() < 150.0f) { //slow down
- slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand
-
- if ( slowdown > DRIVINGSPEED) {
+ countCorrection = 5000;
+ //slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand
+ slowdown = slowdown + 5.0f;
+ if (slowdown >= DRIVINGSPEED - 20.0f) {
+ slowdown = DRIVINGSPEED - 20.0f;
+ }
+ if (irSensor1.read() < FRONTDISTANCE) {
drive = 0;
}
+
+ } else if(((DRIVINGCOUNTS - countsRight) < 300 || (countsLeft + DRIVINGCOUNTS) < 300) && (irSensor2.read() > 120.0f)) { //Anhaltrampe wenn nach counts gefahren
+ slowdown = slowdown + 5.0f;
+ if (slowdown >= DRIVINGSPEED - 20.0f) {
+ slowdown = DRIVINGSPEED - 20.0f;
+ }
+
+ } else{
+ slowdown = 0.0f;
}
+
+
+ //if(dontStop == 2){ //geradeaus
+ // slowdown = 0.0f;
+ //}
+
+ wait(0.01f);
}//Ende Whileschleife Drive...
- controller.setDesiredSpeedRight(0.5f); //0.0f
- controller.setDesiredSpeedLeft(0.5f); //0.0f
+ //controller.setDesiredSpeedRight(0.5f); //0.5f
+ //controller.setDesiredSpeedLeft(-0.5f); //-0.5f
}
--- a/Drive.h Wed Apr 25 12:49:48 2018 +0000
+++ b/Drive.h Mon May 07 18:52:04 2018 +0000
@@ -15,7 +15,7 @@
public:
- Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3);
+ Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop);
virtual ~Drive();
void driving();
private:
@@ -38,6 +38,8 @@
IRSensor& irSensor2;
IRSensor& irSensor3;
+ int& dontStop;
+
};
--- a/KontrastSensor.cpp Wed Apr 25 12:49:48 2018 +0000
+++ b/KontrastSensor.cpp Mon May 07 18:52:04 2018 +0000
@@ -18,10 +18,10 @@
{
- float k = kontrast*3300; //sodass in Range bis 3300mV
- printf("%.0f\n\r", k);
+ float k = kontrast*3300.0f; //sodass in Range bis 3300mV
+ //printf("%.0f\n\r", k);
- if(k >1300){
+ if(k >2500.0f){
blackLine = 1;
}
//else{//Auschalten im Betrieb (nur zur Kontrolle)
--- a/Turn.cpp Wed Apr 25 12:49:48 2018 +0000
+++ b/Turn.cpp Mon May 07 18:52:04 2018 +0000
@@ -3,16 +3,17 @@
using namespace std;
-const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
-const int Turn::TURNINGCOUNTS = 868; //Entspricht Drehung um 90Grad //DONT TOUCH
+const float Turn::TURNINGSPEED = 70.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
+const int Turn::TURNINGCOUNTS = 950; //Entspricht Drehung um 90Grad //DONT TOUCH //940//1070
-Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft):
+Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop):
counterLeft(counterLeft),
counterRight(counterRight),
controller(controller),
wallRight(wallRight),
wallFront(wallFront),
- wallLeft(wallLeft)
+ wallLeft(wallLeft),
+ dontStop(dontStop)
{}
Turn::~Turn() {}
@@ -25,6 +26,13 @@
int countsLeft = counterLeft.read();
int countsLeft0 = countsLeft;
+ //vor dem abbiegen halten
+ controller.setDesiredSpeedRight(0.5f);
+ controller.setDesiredSpeedLeft(-0.5f);
+ //wait(0.1f);
+
+
+
//Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
if (wallLeft == 0){ //Nach Links Drehen
@@ -40,12 +48,16 @@
//printf("%d\n\r", countsLeft);
}
controller.setDesiredSpeedRight(0.5f);
- controller.setDesiredSpeedLeft(0.5f);
+ controller.setDesiredSpeedLeft(-0.5f);
+
+ dontStop = 1;
}else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
//printf("Vorne ist frei\n");
+ dontStop = 2;
+
}else if (wallRight == 0) { //Nach Rechts Drehen
//printf("Rechts ist frei\n");
@@ -59,14 +71,16 @@
//printf("%d\n", countsLeft);
}
controller.setDesiredSpeedRight(0.5f);
- controller.setDesiredSpeedLeft(0.5f);
+ controller.setDesiredSpeedLeft(-0.5f);
+
+ dontStop = 3;
}else{ //Alle Wege versperrt-> Wenden
//printf("Alles versperrt...zurueck\n");
- while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){
+ while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
controller.setDesiredSpeedRight(TURNINGSPEED);
controller.setDesiredSpeedLeft(TURNINGSPEED);
countsRight = counterRight.read();
@@ -75,6 +89,8 @@
//printf("%d\n", countsLeft);
}
controller.setDesiredSpeedRight(0.5f); //0.0f
- controller.setDesiredSpeedLeft(0.5f); //0.0f
+ controller.setDesiredSpeedLeft(-0.5f); //0.0f
+
+ dontStop = 4;
}
}
--- a/Turn.h Wed Apr 25 12:49:48 2018 +0000
+++ b/Turn.h Mon May 07 18:52:04 2018 +0000
@@ -12,7 +12,7 @@
public:
- Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft);
+ Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop);
virtual ~Turn();
void turning();
@@ -28,6 +28,7 @@
int& wallRight;
int& wallFront;
int& wallLeft;
+ int& dontStop;
};
#endif /* TURN_H_ */
\ No newline at end of file
--- a/main.cpp Wed Apr 25 12:49:48 2018 +0000
+++ b/main.cpp Mon May 07 18:52:04 2018 +0000
@@ -9,8 +9,6 @@
#include "CheckWalls.h"
#include "Turn.h"
-
-
//Initialisierung LEDs Blinker/Surri/Button
DigitalIn button(USER_BUTTON); //Moduswählknopf
DigitalOut myled(LED1); //Heartbeat (evt auch Anzeige für Modus, vor start
@@ -27,8 +25,6 @@
PwmOut led5(PB_10);
PwmOut led6(PB_9);
-
-
//Initialisierung IR-Sensoren
AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
AnalogIn distance1(PC_3);
@@ -39,22 +35,9 @@
IRSensor irSensor2(distance2); //links-vorne
IRSensor irSensor3(distance3); //links-hinten
-
-//AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte
-//DigitalOut bit0(PH_1);
-//DigitalOut bit1(PC_2);
-//DigitalOut bit2(PC_3);
-//IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
-//IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
-//IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
-//IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
-
-
//Initialisierung Kontrastsensor
AnalogIn kontrast(PC_0);
-
-
//Initialisierung Motor und Encoder
DigitalOut enableMotorDriver(PB_2);
PwmOut pwmLeft(PA_8); //Motor links
@@ -78,13 +61,16 @@
int wallFront = 0;
int wallLeft = 0;
int blackLine = 0;
+ int dontStop = 0;
+ //int way = 220;
// int way[5][10][4] = {{0},{0},{0}}; // Abspeicherung wird in roboterkoordinaten gespeichert vierstellige Zahl => 1.Zahl gefahrene Richtung, 2.Zahl Wand rechts, 3.Zahl Wand vorne, 4.Zahl Wand links
-
+
+ //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); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
+ Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft, dontStop); //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); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
+ Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3, dontStop); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
while(1) { // Wiederholungsschleife
@@ -96,6 +82,8 @@
while (button) {
//printf("WARTE AUF 1.START...");
+ //controller.setDesiredSpeedRight(0.5f);
+ //controller.setDesiredSpeedLeft(-0.5f);
wait(1.0f); //Die Ruhe vor dem Sturm...
}
@@ -104,12 +92,9 @@
}
case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
+
checkWalls.check(); //prueft wo Waende vorhanden sind
- //wallRight = 1;
- //wallFront = 1;
- //wallLeft = 1;
-
turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
@@ -133,15 +118,12 @@
// way[0][0][3] = 0;
// }
//Abspeicherung des entschiedenen Wegs und Anpassung des
-
-
-// printf("%d", way[0][0][0]);
+ //dontStop = way%10;
+ //way = way/10;
+
drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
-
-
- //blackLine = 1;
if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
state = 2;
@@ -165,11 +147,16 @@
//pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden
- checkWalls.check(); //prueft wo Waende vorhanden sind
+ //checkWalls.check(); //prueft wo Waende vorhanden sind
+ //
+ //integer fuer kuerzesten weg...1 rechts 2 gerade aus 3 links
+ // wallRight = 0;
+ // wallFront = 0;
+ // wallLeft = 0;
turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
-
+ //dontStop = 2;
drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden