PES2_R2D2.0
Version12.04.18
Dependencies: mbed
Fork of
Micromouse_alpha_copy_copy
by 
Patrick Ruesi
Revision 4:e3f388933954, committed 2018-04-12
- Comitter:
- ruesipat
- Date:
- Thu Apr 12 16:14:02 2018 +0000
- Parent:
- 3:2ec7cf8bc3fc
- Commit message:
- Version12.04.18
Changed in this revision
--- a/Drive.cpp Sun Apr 08 19:17:01 2018 +0000
+++ b/Drive.cpp Thu Apr 12 16:14:02 2018 +0000
@@ -7,18 +7,20 @@
using namespace std;
const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit
-const int Drive::DRIVINGCOUNTS = 1712; //Entspricht Strecke von 20cm
+const int Drive::DRIVINGCOUNTS = 1712000; //Entspricht Strecke von 20cm
-Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack):
+Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3):
kontrastSensor(kontrastSensor),
counterLeft(counterLeft),
counterRight(counterRight),
- controller(controller)
+ controller(controller),
+ irSensor0(irSensor0),
+ irSensor1(irSensor1),
+ irSensor2(irSensor2),
+ irSensor3(irSensor3)
+
{
- this->distanceRight = distanceRight;
- this->distanceFront = distanceFront;
- this->distanceLeftFront = distanceLeftFront;
- this->distanceLeftBack = distanceLeftBack;
+
}
Drive::~Drive() {}
@@ -31,42 +33,94 @@
int countsRight0 = countsRight; //ReferenzCounts setzten
int countsLeft = counterLeft.read();
int countsLeft0 = countsLeft;
- int totCLeft, totCRight, counter, sumCorrection;
- int corLeft = 0;
- int corRight = 0;
+ //int totCLeft, totCRight,
+ int sumCorrection =0;
+ //int sumCor = 0;
+ float corLeft = 0;
+ float corRight = 0;
+ //int counter = 0;
- printf("Los gehts\n");
+
+
+ //printf("Los gehts\n");
- while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!!
+ while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) ){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! || distanceFront > 35.0f
//kontrastSensor.check();
- controller.setDesiredSpeedRight(DRIVINGSPEED + corRight);
+ controller.setDesiredSpeedRight(DRIVINGSPEED - corRight); //Korrektur verringert Geschwindigkeit
controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft);
countsRight = counterRight.read();
countsLeft = counterLeft.read();
//printf("CountsRight%d\n\r", countsRight);
- //printf("CountsLeft%d\n\r", countsLeft);
+ //printf(" CountsLeft%d\n\r", countsLeft);
+
+ //totCLeft =+countsLeft;
+ //totCRight =+countsRight;
- totCLeft =+countsLeft;
- totCRight =+countsRight;
- counter++;
+ if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)){ // irSensor0 durch irSensor3 ersetzt
+
+// int i = 0;
+// while (i < 10){
+//
+// sumCorrection += irSensor3.read()-irSensor2.read(); //irSensor0.read() => sensorRight irSensor2.read() => sensorLeftFornt // irSensor0 durch irSensor3 ersetzt
+// //sumCor = irSensor2.read()-irSensor3.read();
+// //double AvSensDist;
+// i++;
+// }
+// sumCorrection = sumCorrection/(double)i;
+ sumCorrection = irSensor2.read()-irSensor3.read();
+ printf(" SumCorrection: %d\n\r", sumCorrection);
+ printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
+ printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
+ }else{
+ corRight=0;
+ corLeft=0;
+ }
- if(counter < 100){ //Wait Time until corretion value added
- sumCorrection = totCLeft-totCRight;
- if(sumCorrection <0){
- corLeft =+1; //Driving Speed Left Tire Addition
+ //if(counter > 10){ //Wait Time until corretion value added
+
+
+
+ if(sumCorrection < -1){ // (steht naeher an rechter Wand) Arsch zu weit links
+ //corLeft = 1 * -sumCorrection; //Driving Speed Left Tire Addition
+ corLeft = 20.0f; // Correction neu 20 statt 1
corRight=0;
- }else if(sumCorrection >0){
+ }else if(sumCorrection > 1){ // (steht naeher an linker Wand) Arsch zu weit rechts
corLeft=0;
- corRight=+1; //Driving Speed Right Tire Addition
+ corRight = 20.0f; // Correction neu 20 statt 1
+ //corRight = 1 * sumCorrection; //Driving Speed Right Tire Addition
+ }else{
+
+
+ corLeft=0;
+ corRight=0;
+
+
+ // if(sumCor < -2){
+ // corLeft=0;
+ // corRight = 1 * sumCor; //Driving Speed Right Tire Addition
+
+ // }else if(sumCor > 2){
+ // corLeft = 1 * -sumCor; //Driving Speed Left Tire Addition
+ // corRight=0;
+ // }else{
+
+ // corLeft=0;
+ // corRight=0;
+ // }
}
- counter = 0;
- totCLeft = 0;
- totCRight = 0;
- }
+
+ //counter = 0;
+ sumCorrection =0;
+ // totCLeft = 0;
+ //totCRight = 0;
+ //}
+ //counter++;
+
+
}
- controller.setDesiredSpeedRight(0.0f);
- controller.setDesiredSpeedLeft(0.0f);
+ controller.setDesiredSpeedRight(0.0f);
+ controller.setDesiredSpeedLeft(0.0f);
}
--- a/Drive.h Sun Apr 08 19:17:01 2018 +0000
+++ b/Drive.h Thu Apr 12 16:14:02 2018 +0000
@@ -15,7 +15,7 @@
public:
- Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack);
+ Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3);
virtual ~Drive();
void driving();
private:
@@ -32,10 +32,10 @@
Controller& controller;
- float distanceRight;
- float distanceFront;
- float distanceLeftFront;
- float distanceLeftBack;
+ IRSensor& irSensor0;
+ IRSensor& irSensor1;
+ IRSensor& irSensor2;
+ IRSensor& irSensor3;
--- a/Turn.cpp Sun Apr 08 19:17:01 2018 +0000
+++ b/Turn.cpp Thu Apr 12 16:14:02 2018 +0000
@@ -4,7 +4,7 @@
using namespace std;
const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit
-const int Turn::TURNINGCOUNTS = 941; //Entspricht Drehung um 90Grad
+const int Turn::TURNINGCOUNTS = 946; //Entspricht Drehung um 90Grad //941;
Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft):
counterLeft(counterLeft),
@@ -29,26 +29,26 @@
if (wallLeft == 0){ //Nach Links Drehen
- printf("Links ist frei\n");
+ //printf("Links ist frei\n");
while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
controller.setDesiredSpeedRight(TURNINGSPEED);
controller.setDesiredSpeedLeft(TURNINGSPEED);
countsRight = counterRight.read();
countsLeft = counterLeft.read();
- printf("%d\n\r", countsRight);
- printf("%d\n\r", countsLeft);
+ //printf("%d\n\r", countsRight);
+ //printf("%d\n\r", countsLeft);
}
controller.setDesiredSpeedRight(0.0f);
controller.setDesiredSpeedLeft(0.0f);
}else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
- printf("Vorne ist frei\n");
+ //printf("Vorne ist frei\n");
}else if (wallRight == 0) { //Nach Rechts Drehen
- printf("Rechts ist frei\n");
+ //printf("Rechts ist frei\n");
while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
controller.setDesiredSpeedRight(-TURNINGSPEED);
@@ -64,7 +64,7 @@
}else{ //Alle Wege versperrt-> Wenden
- printf("Alles versperrt...zurueck\n");
+ //printf("Alles versperrt...zurueck\n");
while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){
controller.setDesiredSpeedRight(TURNINGSPEED);
--- a/main.cpp Sun Apr 08 19:17:01 2018 +0000
+++ b/main.cpp Thu Apr 12 16:14:02 2018 +0000
@@ -37,7 +37,7 @@
IRSensor irSensor0(distance0); //rechts
IRSensor irSensor1(distance1); //vorne
IRSensor irSensor2(distance2); //links-vorne
-IRSensor irSensor3(distance3); //links-hinten
+IRSensor irSensor3(distance3); //links-hinten
//AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte
@@ -74,7 +74,10 @@
int main()
{
-
+ int wallRight = 0;
+ int wallFront = 0;
+ int wallLeft = 0;
+ int blackLine = 0;
enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
@@ -101,16 +104,13 @@
float distanceLeftFront = irSensor2.read();
float distanceLeftBack = irSensor3.read();
- int wallRight;
- int wallFront;
- int wallLeft;
- int blackLine;
+
CheckWalls checkWalls(distanceRight, distanceFront, distanceLeftFront, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
//Decide??
Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft); //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, distanceRight, distanceFront, distanceLeftFront, distanceLeftBack); //20cm nach vorne...
+ Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3); //20cm nach vorne...
switch(state) { //Ausrichten
@@ -127,7 +127,7 @@
//wallFront = 1;
//wallLeft = 1;
- turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
+ //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden