Revision 7:862d80e0ea2d, committed 2018-05-04

Comitter:

ahlervin

Date:

Fri May 04 16:26:59 2018 +0000

Parent:

6:7bbcdd07bc2d

Commit message:

Mit Regler 2.0

Changed in this revision

--- a/Fahren.cpp	Thu May 03 19:36:16 2018 +0000
+++ b/Fahren.cpp	Fri May 04 16:26:59 2018 +0000
@@ -1,4 +1,4 @@
-/*Roboshark V4
+/*Roboshark V6
 Fahren.cpp
 Erstellt: J. Blunschi
 geändert: V.Ahlers
@@ -12,11 +12,11 @@
 
 using namespace std;
 
-const float Fahren :: PERIOD = 0.2f;
+const float Fahren :: PERIOD = 0.8f;
 
 //Konstruktor
-Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler):
-    controller(controller), counterLeft(counterLeft), counterRight(counterRight), regler(regler){
+Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler, int reglerEin):
+    controller(controller), counterLeft(counterLeft), counterRight(counterRight), regler(regler), reglerEin(reglerEin){
     
     SpeedR = 50;
     SpeedL = 50;
@@ -27,16 +27,22 @@
 Fahren::~Fahren() {
     ticker.detach();
     }
+    
+
 //Korrigierte Geschwindigkeit aus der Klasse Regler aufrufen
 void Fahren::getSpeed(){
-    SpeedR = regler.get_SpeedR();
-    SpeedL = regler.get_SpeedL();
+    if(reglerEin == 1){
+    SpeedR = regler.getSpeedR();
+    SpeedL = regler.getSpeedL();
+    }else {SpeedR = 50;
+            SpeedL = 50;
+    }
     
     //printf("SpeedR in F = %f\n",SpeedR);
     //printf("SpeedL in F = %f\n",SpeedL);
 }
 
-  
+
 //Implementation der Methode geradeaus fahren ungeregelt
 void Fahren::geradeausU(){
     
@@ -49,16 +55,16 @@
     wegLinks = 1773;
         
     //Geschwindigkeit
-    speedRight = 50;
-    speedLeft = 50;
+    //speedRight = 50;
+    //speedLeft = 50;
     
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
     stopLeft = false;
     
     //Drehrichtung der Motoren
-    controller.setDesiredSpeedRight(speedRight);
-    controller.setDesiredSpeedLeft(-(speedLeft));
+    controller.setDesiredSpeedRight(SpeedR);
+    controller.setDesiredSpeedLeft(-(SpeedL));
     
     
     while(1){
@@ -101,8 +107,7 @@
     //speedRight = 50;//SpeedR
     //speedLeft = 50;//SpeedL
     
-    //printf("SpeedR in F2= %f\n",SpeedR);
-    //printf("SpeedL in F2= %f\n",SpeedL);
+
     
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
@@ -111,7 +116,8 @@
     //Drehrichtung der Motoren
     controller.setDesiredSpeedRight(SpeedR);
     controller.setDesiredSpeedLeft(-(SpeedL));
-    
+        printf("SpeedR in F2= %f\n",SpeedR);
+        printf("SpeedL in F2= %f\n",SpeedL);
     
     while(1){
        
@@ -146,21 +152,22 @@
     initialClicksLeft = counterLeft.read();
     
     //Anzahl Clicks die der Encoder zählen soll
-    wegRechts = 872;
-    wegLinks = 872;
+    wegRechts = 868;
+    wegLinks = 868;
     
     //Geschwindigkeit
-    speedRight = 50;
-    speedLeft = 50;
+    //speedRight = 50;
+    //speedLeft = 50;
     
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
     stopLeft = false;
     
     //Drehrichtung der Motoren
-    controller.setDesiredSpeedRight((speedRight));
-    controller.setDesiredSpeedLeft((speedLeft));
-    
+    controller.setDesiredSpeedRight((SpeedR));
+    controller.setDesiredSpeedLeft((SpeedL));
+    //printf("SpeedR in F = %f\n",SpeedR);
+    //printf("SpeedL in F = %f\n",SpeedL);
     while(1){
        
        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
@@ -196,20 +203,20 @@
     initialClicksLeft = counterLeft.read();
     
     //Anzahl Clicks die der Encoder zählen soll
-    wegRechts = 1755;
-    wegLinks = 1755;
+    wegRechts = 1752;
+    wegLinks = 1752;
     
     //Geschwindigkeit
-    speedRight = 50;
-    speedLeft = 50;
+    //speedRight = 50;
+    //speedLeft = 50;
     
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
     stopLeft = false;
     
     //Drehrichtung der Motoren
-    controller.setDesiredSpeedRight((speedRight));
-    controller.setDesiredSpeedLeft((speedLeft));
+    controller.setDesiredSpeedRight((SpeedR));
+    controller.setDesiredSpeedLeft((SpeedL));
     
     while(1){
        
@@ -249,16 +256,16 @@
     wegLinks = 878;
     
     //Geschwindigkeit
-    speedRight = 50;
-    speedLeft = 50;
+    //speedRight = 50;
+    //speedLeft = 50;
     
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
     stopLeft = false;
     
     //Drehrichtung der Motoren
-    controller.setDesiredSpeedRight((-speedRight));
-    controller.setDesiredSpeedLeft((-speedLeft));
+    controller.setDesiredSpeedRight((-SpeedR));
+    controller.setDesiredSpeedLeft((-SpeedL));
     
     while(1){
        
@@ -298,16 +305,16 @@
     wegLinks = 7050;
     
     //Geschwindigkeit
-    speedRight = 80;
-    speedLeft = 80;
+    //speedRight = 80;
+    //speedLeft = 80;
     
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
     stopLeft = false;
     
     //Drehrichtung der Motoren
-    controller.setDesiredSpeedRight((speedRight));
-    controller.setDesiredSpeedLeft((speedLeft));
+    controller.setDesiredSpeedRight((SpeedR));
+    controller.setDesiredSpeedLeft((SpeedR));
     
     while(1){
        

--- a/Fahren.h	Thu May 03 19:36:16 2018 +0000
+++ b/Fahren.h	Fri May 04 16:26:59 2018 +0000
@@ -18,7 +18,7 @@
 class Fahren{
     
     public:    
-    Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler); //Konstruktor
+    Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler, int reglerEin); //Konstruktor
     
     virtual ~Fahren();
     
@@ -52,6 +52,7 @@
     float SpeedR;
     float SpeedL;
     static const float PERIOD;
+    int reglerEin;
    
     
 

--- a/IRSensor.cpp	Thu May 03 19:36:16 2018 +0000
+++ b/IRSensor.cpp	Fri May 04 16:26:59 2018 +0000
@@ -27,9 +27,9 @@
     const float IRSensor :: PF3 = 0.0024f;
     const float IRSensor :: PF4 = -1.3299f;
     const float IRSensor :: PF5 = 351.1557f;
-    const int IRSensor :: minIrR = 100;         // minimal Distanzen
-    const int IRSensor :: minIrL = 100;
-    const int IRSensor :: minIrF = 100;
+    const int IRSensor :: minIrR = 88;         // minimal Distanzen
+    const int IRSensor :: minIrL = 88;
+    const int IRSensor :: minIrF = 79;
     const float IRSensor :: Period = 0.2;       // Ticker Periode
   
     
@@ -57,6 +57,8 @@
         measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
         measR = measR * 1000; // Change the value to be in the 0 to 1000 range
         disR = PR1*pow(measR,4)+PR2*pow(measR,3)+PR3*pow(measR,2)+PR4*measR+PR5; //disR = f(measR)
+        roundR = disR*100;
+        disR = roundR/100.0-10.0;
         
         return disR;    
 }
@@ -66,6 +68,8 @@
         measL = IrLeft.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
         measL = measL * 1000; // Change the value to be in the 0 to 1000 range
         disL = PL1*pow(measL,4)+PL2*pow(measL,3)+PL3*pow(measL,2)+PL4*measL+PL5; //disL = f(measL)
+        roundL = disL*100;
+        disL = roundL/100.0-10.0;
         
         return disL;    
 }
@@ -75,6 +79,8 @@
         measF = IrFront.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
         measF = measF * 1000; // Change the value to be in the 0 to 1000 range
         disF = PF1*pow(measF,4)+PF2*pow(measF,3)+PF3*pow(measF,2)+PF4*measF+PF5; //disF = f(measF)
+        roundF = disF*100;
+        disF = roundF/100.0-20.0;
         
         return disF;    
 }

--- a/IRSensor.h	Thu May 03 19:36:16 2018 +0000
+++ b/IRSensor.h	Fri May 04 16:26:59 2018 +0000
@@ -48,6 +48,9 @@
         float dis2R;
         float dis2L;
         float dis2F;
+        int roundL;
+        int roundR;
+        int roundF;
         static const float PR1;
         static const float PR2;
         static const float PR3;

--- a/Regler.cpp	Thu May 03 19:36:16 2018 +0000
+++ b/Regler.cpp	Fri May 04 16:26:59 2018 +0000
@@ -8,18 +8,19 @@
 
 #include <cmath>
 #include "Regler.h"
+#include "IRSensor.h"
 
 
 using namespace std;
 
 const float Regler :: PERIOD = 0.2f;
 const int Regler :: FIXSPEED = 50;
-float faktor = 0.02;
+float faktor = 0.1;
 
 
 
-Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft):
-IrRight (IrRight), IrLeft (IrLeft) {
+Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft, IRSensor& iRSensor):
+IrRight (IrRight), IrLeft (IrLeft), iRSensor (iRSensor) {
         
         SpeedR = 0;
         SpeedL = 0;
@@ -31,40 +32,79 @@
     }
     
     void Regler::setSpeed (){
-        measR2 = IrRight.read();            // Converts and read the analog input value (value from 0.0 to 1.0)
-        measR2 = measR2* 1000;              // Change the value to be in the 0 to 1000 range
-        measL2 = IrLeft.read();             // Converts and read the analog input value (value from 0.0 to 1.0)
-        measL2 = measL2 * 1000;             // Change the value to be in the 0 to 1000 range
+        measR2 = iRSensor.readR();            // Converts and read the analog input value (value from 0.0 to 1.0)
+        measL2 = iRSensor.readL();             
         
-        if((measR2 < 100)) {
-            if(measL2 > 280){
-                div3 = measL2 - 280;
+        if((measR2 > 100) && (measL2 < 100)) {                //keine Wnad rechts
+            if(measL2 > 47){
+                div3 = measL2 - 47;
                 kor3 = faktor*div3;
                 div2 = 0;
                 div1 = 0;
                 div4 = 0;
+                div5 = 0;
+                div6 = 0;
                 SpeedR = FIXSPEED;
                 SpeedL = FIXSPEED + kor3;
-            } else if (measR2 < 280){
-                div4 = 280 - measR2;
+            } else if (measL2 < 52){
+                div4 = 52 - measR2;
                 kor4 = faktor*div4;
                 div2 = 0;
                 div1 = 0;
                 div3 = 0;
-                SpeedR = FIXSPEED + kor4;
+                div5 = 0;
+                div6 = 0;
+                SpeedR = FIXSPEED;
+                SpeedL = FIXSPEED + kor4;
+                }else { 
+            SpeedR = FIXSPEED;
+            SpeedL = FIXSPEED;
+            }
+        }
+        if((measL2 > 100) &&(measR2 < 100)) {                //keine Wnad links
+            if(measR2 > 47){
+                div5 = measR2 - 47;
+                kor5 = faktor*div5;
+                div2 = 0;
+                div1 = 0;
+                div4 = 0;
+                div6 = 0;
+                div3 = 0;
+                SpeedR = FIXSPEED;
+                SpeedL = FIXSPEED + kor5;
+            } else if (measR2 < 52){
+                div6 = 52 - measR2;
+                kor6 = faktor*div4;
+                div2 = 0;
+                div1 = 0;
+                div3 = 0;
+                div4 = 0;
+                div5 = 0;
+                SpeedR = FIXSPEED + kor6;
                 SpeedL = FIXSPEED;
-                }
-    
-        if ((measR2 > measL2) && (meas2R > 100)) {              //IR Sensor werte werden verglichen und die Korrektur wird berechnet
-            div1 = measR2 - measL2;
-            kor1 = faktor*div1;
-            div2 = 0;
+            } else { 
+            SpeedR = FIXSPEED;
+            SpeedL = FIXSPEED;
+            }
+        }
+        if ((measR2 < measL2)&& (measL2 - measR2 > 3)) {              //IR Sensor werte werden verglichen und die Korrektur wird berechnet
+            div1 = measR2 - measL2;                             // An beiden seinen Wände
+            kor1 = 0.1f*div1;
+                div2 = 0;
+                div3 = 0;
+                div4 = 0;
+                div5 = 0;
+                div6 = 0;
             SpeedR = FIXSPEED;
             SpeedL = FIXSPEED + kor1;
-        } else if ((measR2 < measL2)&& meas2R > 100) {
+        } else if ((measR2 > measL2) && (measR2 - measL2 >3)) {
             div2 = measL2 - measR2;
-            kor2 = 0.02f*div2;
-            div1 = 0;
+            kor2 = 0.1f*div2;
+                div1 = 0;
+                div3 = 0;
+                div4 = 0;
+                div5 = 0;
+                div6 = 0;
             SpeedR = FIXSPEED + kor2;
             SpeedL = FIXSPEED;
         } else { 
@@ -76,17 +116,16 @@
 //printf("SpeedR1 = %f\n",SpeedR);
 //printf("SpeedL1 = %f\n",SpeedL);        
 }
-float Regler :: get_SpeedR (){
+    float Regler :: getSpeedR (){
         SpeedR = SpeedR;
         //printf("SpeedR2 = %f\n",SpeedR);
         return SpeedR;
     }
-float Regler :: get_SpeedL (){
+    float Regler :: getSpeedL (){
         SpeedL = SpeedL;
         //printf("SpeedL2 = %f\n",SpeedL);  
         return SpeedL;
-    }
-        
+    }    
         
         
             

--- a/Regler.h	Thu May 03 19:36:16 2018 +0000
+++ b/Regler.h	Fri May 04 16:26:59 2018 +0000
@@ -1,4 +1,4 @@
-/*Roboshark V4
+/*Roboshark V5
 Regler.h
 Erstellt: V.Ahlers
 geändert: V.Ahlers
@@ -10,21 +10,23 @@
 
 #include <cstdlib>
 #include <mbed.h>
+#include "IRSensor.h"
 
 class Regler{
 
     public:    
-    Regler(AnalogIn& IrRight, AnalogIn& IrLeft); //Konstruktor
+    Regler(AnalogIn& IrRight, AnalogIn& IrLeft, IRSensor& iRSensor); //Konstruktor
     
     virtual ~Regler();
     
  
-    float get_SpeedR();
-    float get_SpeedL();
+    float getSpeedR();
+    float getSpeedL();
     
     private:
         AnalogIn& IrRight;
         AnalogIn& IrLeft;
+        IRSensor& iRSensor;
         static const int FIXSPEED;
         static const float PERIOD;
         float SpeedR;
@@ -35,10 +37,14 @@
         float div2;
         float div3;
         float div4;
+        float div5;
+        float div6;
         float kor1;
         float kor2;
         float kor3;
         float kor4;
+        float kor5;
+        float kor6;
         float faktor;
         
         

--- a/main.cpp	Thu May 03 19:36:16 2018 +0000
+++ b/main.cpp	Fri May 04 16:26:59 2018 +0000
@@ -1,4 +1,4 @@
-/*Roboshark V4
+/*Roboshark V5
 main.cpp
 Erstellt: J. Blunschi
 geändert: V.Ahlers
@@ -51,6 +51,7 @@
 int temp = 0;   
 float SpeedR = 0;
 float SpeedL = 0;
+int reglerEin = 0;
                                                      //von main Vincent kopiert
 
 DigitalOut enableMotorDriver(PB_2);
@@ -67,9 +68,9 @@
 
 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
 
-Regler regler(IrRight, IrLeft);
+Regler regler(IrRight, IrLeft, iRSensor);
 
-Fahren fahren(controller, counterLeft, counterRight, regler);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
+Fahren fahren(controller, counterLeft, counterRight, regler, reglerEin);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
 
 int main()
 {
@@ -107,7 +108,7 @@
 
 
    
-  /* printf (" ende = %d\n",ende); 
+   /* printf (" ende = %d\n",ende); 
     printf (" finish = %d\n",finish);
     printf (" finishLast = %d\n",finishLast);
     printf("line = %f\n", line);
@@ -136,25 +137,33 @@
         } else {caseDrive = 0;                                                  // Folge; Stop
         }
         
-        //printf("caseDrive = %d\n",caseDrive);
+        printf("caseDrive = %d\n",caseDrive);
         //printf("ende = %d\n",ende);
 
-        wait (0.2);
+        wait (0.1);
         
 
         
         
         if(caseDrive == 1){                 // Aufrufen der verschiedenen fahr funktionen
+            reglerEin = 1;                 
             fahren.geradeausG();
+            reglerEin = 0;
         } else if (caseDrive == 2){
             fahren.rechts90();
+            reglerEin = 1;
             fahren.geradeausG();
+            reglerEin = 0;
         } else if (caseDrive == 3){
             fahren.links90();
+            reglerEin = 1;
             fahren.geradeausG();
+            reglerEin = 0;
         } else if (caseDrive == 4){
             fahren.rechts180();
+            reglerEin = 1;
             fahren.geradeausG();
+            reglerEin = 0;
         } else if (caseDrive == 0){
             fahren.stopp();
         } else if(caseDrive == 5){