Revision 6:7bbcdd07bc2d, committed 2018-05-03

Comitter:

ahlervin

Date:

Thu May 03 19:36:16 2018 +0000

Parent:

5:e715d157ced5

Commit message:

Aufgemotzter Regler noch nicht ganz fertig

Changed in this revision

--- a/Fahren.cpp	Thu Apr 26 05:58:07 2018 +0000
+++ b/Fahren.cpp	Thu May 03 19:36:16 2018 +0000
@@ -1,31 +1,56 @@
+/*Roboshark V4
+Fahren.cpp
+Erstellt: J. Blunschi
+geändert: V.Ahlers
+V.5.18
+*/
+
 #include "Fahren.h"
 #include <mbed.h>
+#include "Regler.h"
 
 
+using namespace std;
+
+const float Fahren :: PERIOD = 0.2f;
+
 //Konstruktor
-Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight):
-    controller(controller), counterLeft(counterLeft), counterRight(counterRight)
+Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler):
+    controller(controller), counterLeft(counterLeft), counterRight(counterRight), regler(regler){
     
-{}
+    SpeedR = 50;
+    SpeedL = 50;
+    ticker.attach(callback(this, &Fahren::getSpeed), PERIOD);
+}
+
 //Dekonstruktor
-Fahren::~Fahren() {}
-
+Fahren::~Fahren() {
+    ticker.detach();
+    }
+//Korrigierte Geschwindigkeit aus der Klasse Regler aufrufen
+void Fahren::getSpeed(){
+    SpeedR = regler.get_SpeedR();
+    SpeedL = regler.get_SpeedL();
+    
+    //printf("SpeedR in F = %f\n",SpeedR);
+    //printf("SpeedL in F = %f\n",SpeedL);
+}
 
-
-//Implementation der Methode geradeaus fahren
-void Fahren::geradeaus(){
+  
+//Implementation der Methode geradeaus fahren ungeregelt
+void Fahren::geradeausU(){
     
     //einlesen des aktuellen Encoder standes
     initialClicksRight = counterRight.read();   
     initialClicksLeft = counterLeft.read();
     
     //Anzahl Clicks die der Encoder zählen soll
-    wegRechts = 1767;
-    wegLinks = 1767;
+    wegRechts = 1773;
+    wegLinks = 1773;
         
     //Geschwindigkeit
-    speedRight = 25;
-    speedLeft = 25;
+    speedRight = 50;
+    speedLeft = 50;
     
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
@@ -61,8 +86,58 @@
         
     }
     
+//Implementation der Methode geradeaus fahren geregelt
+void Fahren::geradeausG(){
+    
+    //einlesen des aktuellen Encoder standes
+    initialClicksRight = counterRight.read();   
+    initialClicksLeft = counterLeft.read();
+    
+    //Anzahl Clicks die der Encoder zählen soll
+    wegRechts = 1773;
+    wegLinks = 1773;
+        
+    //Geschwindigkeit
+    //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;
+    stopLeft = false;
+    
+    //Drehrichtung der Motoren
+    controller.setDesiredSpeedRight(SpeedR);
+    controller.setDesiredSpeedLeft(-(SpeedL));
     
     
+    while(1){
+       
+       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
+       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
+        
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
+            controller.setDesiredSpeedRight(0);
+            stopRight = true;
+            }
+            
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
+        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
+            controller.setDesiredSpeedLeft(0);
+            stopLeft = true;
+            }
+     
+        if(stopRight == true && stopLeft == true){
+            return;
+            }  
+         
+        }
+        
+    }
+
 //Implementation der Methode 90 Grad Rechtsdrehen
 void Fahren::rechts90() {
         
@@ -71,8 +146,8 @@
     initialClicksLeft = counterLeft.read();
     
     //Anzahl Clicks die der Encoder zählen soll
-    wegRechts = 878;
-    wegLinks = 878;
+    wegRechts = 872;
+    wegLinks = 872;
     
     //Geschwindigkeit
     speedRight = 50;

--- a/Fahren.h	Thu Apr 26 05:58:07 2018 +0000
+++ b/Fahren.h	Thu May 03 19:36:16 2018 +0000
@@ -1,29 +1,44 @@
+/*Roboshark V4
+Fahren.h
+Erstellt: J. Blunschi
+geändert: V.Ahlers
+V.5.18
+*/
+
+
 #ifndef FAHREN_H_
 #define FAHREN_H_
 
 #include <mbed.h>
 #include "EncoderCounter.h"
 #include "Controller.h"
+#include "Regler.h"
+
 
 class Fahren{
     
     public:    
-    Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight); //Konstruktor
+    Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler); //Konstruktor
     
     virtual ~Fahren();
     
-    void geradeaus();
+    void geradeausU();
+    void geradeausG();
     void rechts90();
     void rechts180();
     void links90();
     void ziel();
-    void stopp();                                              
+    void stopp();                        
     
     
     private:
     Controller& controller;
     EncoderCounter& counterLeft;
     EncoderCounter& counterRight;
+    Regler& regler;
+    Ticker ticker;
+    void getSpeed();
+
     
     //Variablen die in der Klasse Fahren verwendet werden
     double speedRight;
@@ -34,6 +49,10 @@
     short wegRechts;
     short stopRight;
     short stopLeft;
+    float SpeedR;
+    float SpeedL;
+    static const float PERIOD;
+   
     
 
 };

--- a/IRSensor.cpp	Thu Apr 26 05:58:07 2018 +0000
+++ b/IRSensor.cpp	Thu May 03 19:36:16 2018 +0000
@@ -1,7 +1,11 @@
+/*Roboshark V4
+IRSensor.cpp
+Erstellt: V. Ahlers
+geändert: V.Ahlers
+V.5.18
+Einlesen und Codieren der IR Sensoren
+*/
 
-//Implementation IR Sensoren
-// V04.18
-// V. Ahlers
 
 #include <cmath>
 #include "IRSensor.h"
@@ -23,16 +27,16 @@
     const float IRSensor :: PF3 = 0.0024f;
     const float IRSensor :: PF4 = -1.3299f;
     const float IRSensor :: PF5 = 351.1557f;
-    const int IRSensor :: minIrR = 100;   //Noch definieren
+    const int IRSensor :: minIrR = 100;         // minimal Distanzen
     const int IRSensor :: minIrL = 100;
     const int IRSensor :: minIrF = 100;
-    const float IRSensor :: Period = 0.2;
+    const float IRSensor :: Period = 0.2;       // Ticker Periode
   
     
 
 
 
-// IR Sensor Distanz in mm
+
 IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F, AnalogIn& LineSensor) : 
 IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F), LineSensor(LineSensor){
     ticker.attach(callback(this, &IRSensor::codeB), Period);
@@ -47,7 +51,7 @@
     }
 
 
-
+// IR Sensor Distanz in mm
 float IRSensor::readR() {
     
         measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
@@ -75,7 +79,7 @@
         return disF;    
 }
 
-// IR Sensor Zusatand
+// IR Sensor Zusatand codieren
 int IRSensor::codeR(){
     
         if(disR < minIrR) {
@@ -100,10 +104,11 @@
         return IrF;//IrF;
 }
 
+//Line Sensor
 void IRSensor :: codeB() {
     
         double line = LineSensor.read();
-        if (line >0.3){            //Line Sensor
+        if (line >0.3){            
         finish = 1;
          }else{ finish = 0;
 }    

--- a/IRSensor.h	Thu Apr 26 05:58:07 2018 +0000
+++ b/IRSensor.h	Thu May 03 19:36:16 2018 +0000
@@ -1,6 +1,9 @@
-// Deklaration IR Sensoren
-// V04.18
-// V. Ahlers
+/*Roboshark V4
+IRSensor.h
+Erstellt: V. Ahlers
+geändert: V.Ahlers
+V.5.18
+*/
 
 
 #ifndef IRSENSOR_H_

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Regler.cpp	Thu May 03 19:36:16 2018 +0000
@@ -0,0 +1,94 @@
+/*Roboshark V5
+Regler.cpp
+Erstellt: V.Ahlers
+geändert: V.Ahlers
+V.5.18
+Regler zum geradeaus fahren
+*/
+
+#include <cmath>
+#include "Regler.h"
+
+
+using namespace std;
+
+const float Regler :: PERIOD = 0.2f;
+const int Regler :: FIXSPEED = 50;
+float faktor = 0.02;
+
+
+
+Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft):
+IrRight (IrRight), IrLeft (IrLeft) {
+        
+        SpeedR = 0;
+        SpeedL = 0;
+        ticker.attach(callback(this, &Regler::setSpeed), PERIOD);
+    }
+     
+Regler::~Regler(){
+    ticker.detach();
+    }
+    
+    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
+        
+        if((measR2 < 100)) {
+            if(measL2 > 280){
+                div3 = measL2 - 280;
+                kor3 = faktor*div3;
+                div2 = 0;
+                div1 = 0;
+                div4 = 0;
+                SpeedR = FIXSPEED;
+                SpeedL = FIXSPEED + kor3;
+            } else if (measR2 < 280){
+                div4 = 280 - measR2;
+                kor4 = faktor*div4;
+                div2 = 0;
+                div1 = 0;
+                div3 = 0;
+                SpeedR = FIXSPEED + kor4;
+                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;
+            SpeedR = FIXSPEED;
+            SpeedL = FIXSPEED + kor1;
+        } else if ((measR2 < measL2)&& meas2R > 100) {
+            div2 = measL2 - measR2;
+            kor2 = 0.02f*div2;
+            div1 = 0;
+            SpeedR = FIXSPEED + kor2;
+            SpeedL = FIXSPEED;
+        } else { 
+            SpeedR = FIXSPEED;
+            SpeedL = FIXSPEED;
+            }
+//printf("Div1 = %f\n",div1);
+//printf("Div2 = %f\n",div2);  
+//printf("SpeedR1 = %f\n",SpeedR);
+//printf("SpeedL1 = %f\n",SpeedL);        
+}
+float Regler :: get_SpeedR (){
+        SpeedR = SpeedR;
+        //printf("SpeedR2 = %f\n",SpeedR);
+        return SpeedR;
+    }
+float Regler :: get_SpeedL (){
+        SpeedL = SpeedL;
+        //printf("SpeedL2 = %f\n",SpeedL);  
+        return SpeedL;
+    }
+        
+        
+        
+            
+            
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Regler.h	Thu May 03 19:36:16 2018 +0000
@@ -0,0 +1,49 @@
+/*Roboshark V4
+Regler.h
+Erstellt: V.Ahlers
+geändert: V.Ahlers
+V.5.18
+*/
+
+#ifndef REGLER_H_
+#define REGLER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+class Regler{
+
+    public:    
+    Regler(AnalogIn& IrRight, AnalogIn& IrLeft); //Konstruktor
+    
+    virtual ~Regler();
+    
+ 
+    float get_SpeedR();
+    float get_SpeedL();
+    
+    private:
+        AnalogIn& IrRight;
+        AnalogIn& IrLeft;
+        static const int FIXSPEED;
+        static const float PERIOD;
+        float SpeedR;
+        float SpeedL;
+        float measR2;
+        float measL2;
+        float div1;
+        float div2;
+        float div3;
+        float div4;
+        float kor1;
+        float kor2;
+        float kor3;
+        float kor4;
+        float faktor;
+        
+        
+        void setSpeed();
+        Ticker ticker;
+        
+    };
+    #endif
\ No newline at end of file

--- a/StateMachine.cpp	Thu Apr 26 05:58:07 2018 +0000
+++ b/StateMachine.cpp	Thu May 03 19:36:16 2018 +0000
@@ -3,6 +3,8 @@
 //V04.18
 // V. Ahlers
 
+//Wird nicht mehr gebraucht
+
 #include <mbed.h>
 #include "StateMachine.h"
 

--- a/StateMachine.h	Thu Apr 26 05:58:07 2018 +0000
+++ b/StateMachine.h	Thu May 03 19:36:16 2018 +0000
@@ -2,6 +2,8 @@
 // V04.18
 // V. Ahlers
 
+// wird nicht mehr gebraucht
+
 
 #ifndef STATEMACHINE_H_
 #define STATEMACHINE_H_

--- a/main.cpp	Thu Apr 26 05:58:07 2018 +0000
+++ b/main.cpp	Thu May 03 19:36:16 2018 +0000
@@ -1,3 +1,11 @@
+/*Roboshark V4
+main.cpp
+Erstellt: J. Blunschi
+geändert: V.Ahlers
+V.5.18
+main zu Robishark V4
+*/
+
 
 #include <mbed.h>
 
@@ -5,7 +13,7 @@
 #include "Controller.h"
 #include "IRSensor.h"
 #include "Fahren.h"
-#include "StateMachine.h"
+#include "Regler.h"
 
 DigitalOut myled(LED1);
 
@@ -22,13 +30,6 @@
 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);
-IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
-IRSensor irSensor5(distance, bit0, bit1, bit2, 5);*/
-
 AnalogIn IrRight(PC_3);                                                         //von main Vincent kopiert
 AnalogIn IrLeft (PC_5);
 AnalogIn IrFront(PC_2);
@@ -47,12 +48,10 @@
 bool finish = 0;       
 bool finishLast = 0;
 int ende = 0;    
-int temp = 0;                                                        //von main Vincent kopiert
-
-IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor); 
-//IRSensor bottom (IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor);           //von main Vincent kopiert
-StateMachine stateMachine(IrR, IrL, IrF);                                       //von main Vincent kopiert
-
+int temp = 0;   
+float SpeedR = 0;
+float SpeedL = 0;
+                                                     //von main Vincent kopiert
 
 DigitalOut enableMotorDriver(PB_2);
 DigitalIn motorDriverFault(PB_14);
@@ -64,9 +63,13 @@
 EncoderCounter counterLeft(PB_6, PB_7);
 EncoderCounter counterRight(PA_6, PC_7);
 
+IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor);          //von main Vincent kopiert
+
 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
 
-Fahren fahren(controller, counterLeft, counterRight);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
+Regler regler(IrRight, IrLeft);
+
+Fahren fahren(controller, counterLeft, counterRight, regler);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
 
 int main()
 {
@@ -75,7 +78,6 @@
     //controller.setDesiredSpeedRight(-150.0f);                                 //DEFAULT VON TOBI
     enable = 1;
     enableMotorDriver = 1;                                                      // Schaltet den Leistungstreiber ein
-
     while(1) {
 
 // Test um Drehungen und geradeaus zu testen
@@ -91,7 +93,7 @@
         
         fahren.links90();
         wait(1.0f);*/
-                                                                         // IR Sensoren einlesen Programm Vincen      
+                                                                         // IR Sensoren einlesen
         float disR = iRSensor.readR(); // Distanz in mm
         float disL = iRSensor.readL();
         float disF = iRSensor.readF();
@@ -101,8 +103,7 @@
         int IrR = iRSensor.codeR();     // min Distanz unterschritten = 1
         int IrL = iRSensor.codeL();
         int IrF = iRSensor.codeF();
-        /*int caseDrive = stateMachine.drive();*/ //entscheidung welcher Drive Case
-        ende = iRSensor.get_ende();
+        ende = iRSensor.get_ende();     // Line erkennt = 1
 
 
    
@@ -111,7 +112,7 @@
     printf (" finishLast = %d\n",finishLast);
     printf("line = %f\n", line);
         
-      /*  printf ("IR Right = %d \n", IrR);
+        printf ("IR Right = %d \n", IrR);
         printf("IR Left = %d\n",IrL);
         printf("IR Front = %d\n",IrF);*/
         
@@ -124,36 +125,36 @@
         }   else  if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
         caseDrive = 1;                                                          // Folge: geradeaus fahren
         }   else  if ((IrR==1) && (IrL==0) && (IrF==1) && (ende == 0)){
-        caseDrive = 3;                                                          // Folge: 270 Grad nach rechts drehen
+        caseDrive = 3;                                                          // Folge: 90 Grad nach Links drehen
         }   else  if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
         caseDrive = 1;                                                          // Folge: geradeaus fahren
         }   else  if ((IrR==1) && (IrL==1) && (IrF==1) && (ende == 0)){
         caseDrive = 4;                                                          // Folge: 180 Grad nach rechts drehen
         } else if ((ende == 1) && (temp == 0)){
-        caseDrive = 5;
+        caseDrive = 5;                                                          // Folge: Ziel erreicht
         temp = 1;
-        } else {caseDrive = 0;
+        } else {caseDrive = 0;                                                  // Folge; Stop
         }
         
-        printf("caseDrive = %d\n",caseDrive);
-        printf("ende = %d\n",ende);
+        //printf("caseDrive = %d\n",caseDrive);
+        //printf("ende = %d\n",ende);
 
-        wait (0.5);
+        wait (0.2);
         
 
         
         
-        if(caseDrive == 1){
-            fahren.geradeaus();
+        if(caseDrive == 1){                 // Aufrufen der verschiedenen fahr funktionen
+            fahren.geradeausG();
         } else if (caseDrive == 2){
             fahren.rechts90();
-            fahren.geradeaus();
+            fahren.geradeausG();
         } else if (caseDrive == 3){
             fahren.links90();
-            fahren.geradeaus();
+            fahren.geradeausG();
         } else if (caseDrive == 4){
             fahren.rechts180();
-            fahren.geradeaus();
+            fahren.geradeausG();
         } else if (caseDrive == 0){
             fahren.stopp();
         } else if(caseDrive == 5){