Revision 5:93d3efe46493, committed 2018-05-09

Comitter:

baumgant

Date:

Wed May 09 13:33:59 2018 +0000

Parent:

4:3c6d2c035243

Commit message:

PES2;

Changed in this revision

--- a/Classes/Controller.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Controller.cpp	Wed May 09 13:33:59 2018 +0000
@@ -3,14 +3,17 @@
 using namespace std;
 
 const float Controller::PERIOD = 0.001f; // Periode von 1 ms
-const float Controller::COUNTS_PER_TURN = 1260.0f; // Encoder-Aufloesung
+const float Controller::COUNTS_PER_TURN = 1560.0f;//1200.0f; // Encoder-Aufloesung
 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
-const float Controller::KN = 18.75f; // Drehzahlkonstante in [rpm/V]
-const float Controller::KP = 0.02f; // Regler-Parameter
+const float Controller::KN = 15.0f;//40.0f; // Drehzahlkonstante in [rpm/V]
+const float Controller::KP = 0.25f; // KP Regler-Parameter
+const float Controller::KI = 1.0f; // KI Regler-Parameter
+const float Controller::I_MAX = 1000.0f; // KI Regler-Parameter Saettigung
 const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V]
 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle
 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale Duty-Cycle
 
+
 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
                         EncoderCounter& counterLeft, EncoderCounter& counterRight) :
                         pwmLeft(pwmLeft), pwmRight(pwmRight),
@@ -49,11 +52,6 @@
     ticker.detach(); // Stoppt den periodischen Task
 }
 
-
-void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
-{
-    this->desiredSpeedLeft = desiredSpeedLeft;
-}
 void Controller::resetCounter() 
 {
     ticker.detach();
@@ -61,7 +59,12 @@
     counterRight.reset();
     previousValueCounterLeft = counterLeft.read();
     previousValueCounterRight = counterRight.read();
-    ticker.attach(callback(this, &Controller::run), PERIOD);  
+    ticker.attach(callback(this, &Controller::run), PERIOD); 
+}
+
+void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
+{
+    this->desiredSpeedLeft = desiredSpeedLeft;
 }
 
 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
@@ -69,6 +72,36 @@
     this->desiredSpeedRight = desiredSpeedRight;
 }
 
+float Controller::getSpeedLeft()
+{
+    return actualSpeedLeft;
+}
+
+float Controller::getSpeedRight()
+{
+    return actualSpeedRight;
+}
+
+float Controller::getIntegralLeft()
+{
+    return KI*iSumLeft*PERIOD;
+}
+
+float Controller::getIntegralRight()
+{
+    return KI*iSumRight*PERIOD;
+}
+
+float Controller::getProportionalLeft()
+{
+    return KP*(desiredSpeedLeft-actualSpeedLeft);
+}
+
+float Controller::getProportionalRight()
+{
+    return KP*(desiredSpeedRight-actualSpeedRight);
+}
+
 void Controller::run() {
 
     // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
@@ -87,20 +120,33 @@
     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
                        /COUNTS_PER_TURN/PERIOD*60.0f);
 
-    // Berechnen der Motorspannungen Uout
+
+    //Berechnung I - Anteil
+
     
-    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
-    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)
-                         +desiredSpeedRight/KN;
+    iSumLeft += (desiredSpeedLeft-actualSpeedLeft); 
+    if (iSumLeft > I_MAX) iSumLeft = I_MAX;  //Max Saettigung I - Anteil       
+    if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil
+
+    iSumRight += (desiredSpeedRight-actualSpeedRight); 
+    if (iSumRight > I_MAX) iSumRight = I_MAX;  //Max Saettigung I - Anteil       
+    if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil
+       
+    // Berechnen der Motorspannungen Uout
+       
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
+                        +desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
+                         +desiredSpeedRight/KN;                                   
                          
     // Berechnen, Limitieren und Setzen der Duty-Cycle
     
-    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
+    float dutyCycleLeft = 0.5f-0.5f*voltageLeft/MAX_VOLTAGE;
     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
     pwmLeft = dutyCycleLeft;
     
-    float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
+    float dutyCycleRight = 0.5f-0.5f*voltageRight/MAX_VOLTAGE;
     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
     pwmRight = dutyCycleRight;

--- a/Classes/Controller.h	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Controller.h	Wed May 09 13:33:59 2018 +0000
@@ -16,16 +16,23 @@
     virtual ~Controller();
     void setDesiredSpeedLeft(float desiredSpeedLeft);
     void setDesiredSpeedRight(float desiredSpeedRight);
-    static const float COUNTS_PER_TURN;
     void resetCounter();
+    float getSpeedLeft();
+    float getSpeedRight();
+    float getIntegralLeft();
+    float getIntegralRight();
+    float getProportionalLeft();
+    float getProportionalRight();
 
 private:
 
     static const float PERIOD;
-    
+    static const float COUNTS_PER_TURN;
     static const float LOWPASS_FILTER_FREQUENCY;
     static const float KN;
     static const float KP;
+    static const float KI;
+    static const float I_MAX;    
     static const float MAX_VOLTAGE;
     static const float MIN_DUTY_CYCLE;
     static const float MAX_DUTY_CYCLE;
@@ -42,6 +49,8 @@
     float              desiredSpeedRight;
     float              actualSpeedLeft;
     float              actualSpeedRight;
+    float              iSumLeft;
+    float              iSumRight;
     Ticker             ticker;
     
     void               run();
@@ -54,5 +63,3 @@
 
 
 
-
-

--- a/Classes/IRSensorGF.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/IRSensorGF.cpp	Wed May 09 13:33:59 2018 +0000
@@ -30,12 +30,12 @@
 
     int w = 10/(distance)*exp(1.4481);
 
-    if (w < 130) {
+    if (w < 150) {
         d=1;
     } else {
         d=0;
     }
 
-    return d;
+    return w;
 
 }
\ No newline at end of file

--- a/Classes/IRSensorGF.h	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/IRSensorGF.h	Wed May 09 13:33:59 2018 +0000
@@ -4,17 +4,18 @@
 #include <cstdlib>
 #include <mbed.h>
 
-class IRSensorGF {
-    
+class IRSensorGF
+{
+
 public:
     IRSensorGF(AnalogIn& distance);
-    
+
     virtual ~IRSensorGF();
     int read();
-    
+
 private:
     AnalogIn& distance;
 
 };
 
-#endif /* IR_SENSORGF_H_ */ 
\ No newline at end of file
+#endif /* IR_SENSORGF_H_ */
\ No newline at end of file

--- a/Classes/IRSensorK.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/IRSensorK.cpp	Wed May 09 13:33:59 2018 +0000
@@ -21,11 +21,13 @@
 {
     int d;
 
+
+
     if (3300*(distance)> 400) { //3cm+
         d=1;
     } else {
         d=0;
-    }
+    } 
 
     return d;
 

--- a/Classes/LHR.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/LHR.cpp	Wed May 09 13:33:59 2018 +0000
@@ -20,18 +20,21 @@
 
 int LHR::moving()
 {
-    if(1/*Sensor6.read() == 0*/) {                  //Ziel erreicht
-        
-        if(Sensor4.read() == 0) {                   
-            D=1;                                    //links Drehen
-        } else if (Sensor1.read() == 0) {
-            D=2;                                    //gerade 
+    if(Sensor6.read() == 0) {                  //Ziel erreicht
+
+        if(1/*Sensor4.read() == 0*/) {
+            D=1;
+            //wait(0.1f);                                    //links Drehen
+        } else if (Sensor1.read() > 150 ) {
+            D=2;
+            //wait(0.1f);                                    //gerade
         } else if (Sensor5.read() == 0) {
-            D=3;                                    
-        } else{
+            D=3;
+            //wait(0.1f);
+        } else {
             D=4;
         }
-        
+
     }
     return D;
 }

--- a/Classes/Motion.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Motion.cpp	Wed May 09 13:33:59 2018 +0000
@@ -7,11 +7,17 @@
 #include "IRSensorK.h"
 #include "IRSensorZ.h"
 
-// Left + = Rückwärts Left - = Vorwärts
-// Right + = Vorwärts Right - = Rückwärts
+// Left + = Vorwärts Left - = Vorwärts
+// Right + = Rückwärts Right - = Rückwärts
+
+
 
 using namespace std;
 
+const float Motion::SPEED = 150;
+const float Motion::DREHEN90 = 1607;
+const float Motion::GERADE = 3280;
+
 Motion::Motion(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, Spurhaltung& spurhaltung, IRSensorGF& Sensor1, IRSensorG& Sensor2, IRSensorG& Sensor3, IRSensorK& Sensor4, IRSensorK& Sensor5, IRSensorZ& Sensor6):
     counterLeft(counterLeft), counterRight(counterRight), controller(controller), spurhaltung(spurhaltung), Sensor1(Sensor1), Sensor2(Sensor2), Sensor3(Sensor3), Sensor4(Sensor4), Sensor5(Sensor5), Sensor6(Sensor6)
 {
@@ -27,9 +33,9 @@
     controller.resetCounter();
     int lastCountRight = counterRight.read();
 
-    while(counterRight.read()-lastCountRight>(-1.19*1260)) {
-        controller.setDesiredSpeedLeft(150.0f);             //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(150.0f);
+    while(counterRight.read()-lastCountRight > -DREHEN90) {
+        controller.setDesiredSpeedLeft(-SPEED);             //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(-SPEED);
     }
     controller.setDesiredSpeedLeft(0.0f);                   //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
@@ -43,14 +49,23 @@
 
     controller.resetCounter();
     int lastCountLeft = counterLeft.read();
-    
-    if (Sensor1.read() == 0) {
-        while(counterLeft.read()-lastCountLeft>(-2.45*1260)) {
-            spurhaltung.speedl();
-            spurhaltung.speedr();
-            controller.setDesiredSpeedLeft(-spurhaltung.speedr());   //Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(spurhaltung.speedl());
+
+    while((counterLeft.read()-lastCountLeft < GERADE) && (Sensor1.read() > 120 )) { // 1.806
+        controller.setDesiredSpeedLeft(spurhaltung.speedl());   //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(-spurhaltung.speedr());
+        printf("");
+        //printf("SpurhaltungLinks: %d\r\n", spurhaltung.speedl());
+        //printf("SpurhaltungRechts: %d\r\n", spurhaltung.speedr());
+        //printf("Sensor2:%d\r\n", Sensor2.read());
+        //printf("Sensor1:%d\r\n\r\n", Sensor1.read());
+    }
+    if((Sensor1.read() > 120) && (Sensor1.read() < 250)) {
+        while(Sensor1.read() > 120) {
+            controller.setDesiredSpeedLeft(spurhaltung.speedl());   //Drehzahl in [rpm]
+            controller.setDesiredSpeedRight(-spurhaltung.speedr());
+            printf("");
         }
+
     }
     controller.setDesiredSpeedLeft(0.0f);                       //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
@@ -63,9 +78,9 @@
     controller.resetCounter();
     int lastCountLeft = counterLeft.read();
 
-    while(counterLeft.read()-lastCountLeft<(1.19*1260)) {
-        controller.setDesiredSpeedLeft(-150.0f);            //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(-150.0f);
+    while(counterLeft.read()-lastCountLeft < DREHEN90) {
+        controller.setDesiredSpeedLeft(SPEED);            //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(SPEED);
     }
     controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
@@ -79,7 +94,7 @@
     controller.resetCounter();
     int lastCountRight = counterRight.read();
 
-    while(counterRight.read()-lastCountRight>(-2.38*1260)) {
+    while(counterRight.read()-lastCountRight < 2*1607 + 150) {
         controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
         controller.setDesiredSpeedRight(150.0f);
     }
@@ -95,25 +110,31 @@
         case 1:
             // Left
             drehenl90();
+            wait(0.05f);
             // Forward
             gerade();
+            wait(0.05f);
             break;
 
         case 2:
             // Forward
             gerade();
+            wait(0.05f);
             break;
 
         case 3:
             // Right
             drehenr90();
+            wait(0.05f);
             // Forward
             gerade();
+            wait(0.05f);
             break;
 
         case 4:
             // 180 Drehen
             drehen180();
+            wait(0.05f);
             break;
 
         default:

--- a/Classes/Motion.h	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Motion.h	Wed May 09 13:33:59 2018 +0000
@@ -29,6 +29,9 @@
     
     EncoderCounter& counterLeft;
     EncoderCounter& counterRight;
+    static const float SPEED;
+    static const float DREHEN90;
+    static const float GERADE;
     Controller& controller;
     Spurhaltung& spurhaltung;
     IRSensorGF& Sensor1;

--- a/Classes/Spurhaltung.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Spurhaltung.cpp	Wed May 09 13:33:59 2018 +0000
@@ -3,6 +3,15 @@
 
 using namespace std;
 
+const int Spurhaltung::DISTANCE = 100;
+const float Spurhaltung::SPEED = 100;
+const int Spurhaltung::DIFF = 1;
+const int Spurhaltung::DIFF2 = 120;
+const int Spurhaltung::DIFF3 = 180;
+const int Spurhaltung::OFFSET = 30;
+const int Spurhaltung::TOLERANCE1 = 8;
+const int Spurhaltung::TOLERANCE2 = -8;
+
 Spurhaltung::Spurhaltung(IRSensorG& Sensor2, IRSensorG& Sensor3):
     Sensor2(Sensor2), Sensor3(Sensor3)
 
@@ -17,25 +26,90 @@
 
 int Spurhaltung::speedl()
 {
-    if(abs(Sensor3.read()-Sensor2.read())<100) {
-        
-        int sl = Sensor2.read() * 1.825;
-        return sl;
-    } else {
-        int sl = 150;
-        return sl;
+    // Wand links und rechts
+    if(abs(Sensor2.read()+OFFSET-Sensor3.read())<DIFF3) {
+        int sl = Sensor3.read() * 1.825 - 30;
+        printf("");
+        if(Sensor3.read() > (DISTANCE + 50)) {
+            sl = SPEED;
+            return sl;
+        } else {
+            return sl;
+        }
     }
-}
+    // nur Wand links
+    else if ((Sensor2.read() < DIFF2) and (Sensor3.read() > DIFF3)) {
+        if ((Sensor2.read() - DISTANCE + OFFSET) > TOLERANCE1) {
+            int sl = 80 - DIFF;
+            //printf("Fridolin\r\n");
+            return sl;
+        } else if ((Sensor2.read() - DISTANCE + OFFSET) < TOLERANCE2) {
+            int sl = 80 + DIFF;
+            return sl;
+        } else {
+            sl = SPEED;
+            return sl;
+        }
+    }
+
+    // nur Wand rechts
+    else if ((Sensor3.read()<DIFF2) and (Sensor2.read()>DIFF3)) {
+        if ((Sensor3.read() - DISTANCE) > TOLERANCE1) {
+            sl = 80 + DIFF;
+            //printf("Fridolin\r\n");
+            return sl;
+        } else if ((Sensor3.read() - DISTANCE) < TOLERANCE2) {
+            int sl = 80 - DIFF;
+            return sl;
+        } else {
+            sl = SPEED;
+            return sl;
+        }
+        }
+    }
+
 
 //------------------------------------------------------------------------------
 
 int Spurhaltung::speedr()
 {
-    if(abs(Sensor2.read()-Sensor3.read())<100) {
-        int sr = Sensor3.read() * 1.825;
-        return sr;
-    } else {
-        int sr = 150;
-        return sr;
+    // Wand links und rechts
+    if(abs(Sensor3.read()-Sensor2.read()+OFFSET)<DIFF3) {
+        int sr = (Sensor2.read()) * 1.825;
+        printf("");
+        if(Sensor2.read() > (DISTANCE - OFFSET + 50)) {
+            sr = SPEED;
+            return sr;
+        } else {
+            return sr;
+        }
     }
-}
\ No newline at end of file
+    // nur Wand links
+    else if ((Sensor2.read() < DIFF2) and (Sensor3.read() > DIFF3)) {
+        if ((Sensor2.read() - DISTANCE + OFFSET) > TOLERANCE1) {
+            int sr = 80 + DIFF;
+            //printf("Karolina\r\n");
+            return sr;
+        } else if ((Sensor2.read() - DISTANCE + OFFSET) < TOLERANCE2) {
+            int sr = 80 - DIFF;
+            return sr;
+        } else {
+            sr = SPEED;
+            return sr;
+        }
+    }
+    // nur Wand rechts
+    else if((Sensor3.read() < DIFF2) and (Sensor2.read() > DIFF3)) {
+        if ((Sensor3.read() - DISTANCE) > TOLERANCE1) {
+            int sr = 80 - DIFF;
+            //printf("Karolina\r\n");
+            return sr;
+        } else if ((Sensor3.read() - DISTANCE) < TOLERANCE2) {
+            int sr = 80 + DIFF;
+            return sr;
+        } else {
+            sr = SPEED;
+            return sr;
+        }
+    }
+}

--- a/Classes/Spurhaltung.h	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Spurhaltung.h	Wed May 09 13:33:59 2018 +0000
@@ -22,6 +22,14 @@
     IRSensorG& Sensor3;
     int sr;
     int sl;
+    static const int DISTANCE;
+    static const float SPEED;
+    static const int DIFF;
+    static const int DIFF2;
+    static const int DIFF3;
+    static const int OFFSET;
+    static const int TOLERANCE1;
+    static const int TOLERANCE2;
 
 };
 

--- a/main.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/main.cpp	Wed May 09 13:33:59 2018 +0000
@@ -23,8 +23,6 @@
 AnalogIn sensorValue4(PB_1);        // Seitlicher Sensor links
 AnalogIn sensorValue5(PA_4);        // Seitlicher Sensor rechts
 AnalogIn sensorValue6(PB_0);        // Helligkeitssensor
-//DigitalOut myled(LED1);           // LED 1
-//InterruptIn button(USER_BUTTON);  // Startknopf
 DigitalIn mybutton(USER_BUTTON);
 DigitalOut myled(LED1);
 DigitalOut power_5v(PC_4);          // 5V auf Sensoren, geschalten
@@ -35,15 +33,14 @@
 DigitalIn motorDriverFault(PB_14);
 DigitalIn motorDriverWarning(PB_15);
 
-PwmOut pwmRight(PA_8);
-PwmOut pwmLeft(PA_9);
+PwmOut pwmRight(PA_9);
+PwmOut pwmLeft(PA_8);
 
 EncoderCounter counterLeft(PB_6, PB_7);
 EncoderCounter counterRight(PA_6, PC_7);
 
 //------------------------------------------------------------------------------
 
-Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
 
 // Grosse Sensoren aufrufen
 IRSensorGF Sensor1(sensorValue1);
@@ -59,6 +56,7 @@
 
 int main()
 {
+    Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
     power_5v = 1; // Einschalten 5V Speisung der Sensoren
     enable = 1;
     enableMotorDriver = 1; //Schaltet den Leistungstreiber ein
@@ -73,8 +71,14 @@
             while (1) {
                 D = LHR.moving();
                 motion.switching(D);
-                wait(0.2);
+                /*printf("Sensor1:%d\r\n", Sensor1.read());
+                printf("Sensor2:%d\r\n", Sensor2.read());
+                printf("Sensor3:%d\r\n", Sensor3.read());
+                printf("Sensor4:%d\r\n", Sensor4.read());
+                printf("Sensor5:%d\r\n", Sensor5.read());
+                printf("\r\n");*/
+                wait(0.05f);
             }
         }
     }
-    }
+}