main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
Helvis
Date:
Mon Apr 16 12:44:48 2018 +0000
Parent:
4:e74c06e43485
Child:
6:1f084e6f7e80
Commit message:
testtestest;

Changed in this revision

Controller.cpp Show annotated file Show diff for this revision Revisions of this file
EncoderCounter.cpp Show annotated file Show diff for this revision Revisions of this file
IRSensor.cpp Show annotated file Show diff for this revision Revisions of this file
IRSensor.h Show annotated file Show diff for this revision Revisions of this file
Motion.cpp Show annotated file Show diff for this revision Revisions of this file
Motion.h Show annotated file Show diff for this revision Revisions of this file
Path.cpp Show annotated file Show diff for this revision Revisions of this file
Path.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/Controller.cpp	Wed Apr 11 15:26:03 2018 +0000
+++ b/Controller.cpp	Mon Apr 16 12:44:48 2018 +0000
@@ -3,9 +3,9 @@
 using namespace std;
 
 const float Controller::PERIOD = 0.001f; // Periode von 1 ms
-const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung
+const float Controller::COUNTS_PER_TURN = 1560.0f; // Encoder-Aufloesung
 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
-const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V]
+const float Controller::KN = 15.0f; // Drehzahlkonstante in [rpm/V]
 const float Controller::KP = 0.25f; // KP Regler-Parameter
 const float Controller::KI = 4.0f; // KI Regler-Parameter
 const float Controller::I_MAX = 10000.0f; // KI Regler-Parameter Saettigung
--- a/EncoderCounter.cpp	Wed Apr 11 15:26:03 2018 +0000
+++ b/EncoderCounter.cpp	Mon Apr 16 12:44:48 2018 +0000
@@ -138,7 +138,7 @@
  */
 void EncoderCounter::reset() {
     
-    TIM->CNT = 0x0000;
+    TIM->CNT = 0x0000; 
 }
 
 /**
--- a/IRSensor.cpp	Wed Apr 11 15:26:03 2018 +0000
+++ b/IRSensor.cpp	Mon Apr 16 12:44:48 2018 +0000
@@ -8,11 +8,48 @@
 
 IRSensor::~IRSensor() {}
 
-float IRSensor::read() {
+float IRSensor::readL() {
+    //Links B2
+    float a1 = -4.474f*pow(10.0f,4.0f);
+    float a2 = 8.415f*pow(10.0f,4.0f);
+    float a3 = -6.279f*pow(10.0f,4.0f);
+    float a4 = 2.36f*pow(10.0f,4.0f);
+    float a5 = -4670.0f;
+    float a6 =  445.1f;
+    
+
+    float d2 = a1*pow(distance,5) + a2*pow(distance,4) + a3*pow(distance,3) + a4*pow(distance,2) + a5*distance + a6;
+    
+    return d2;
     
-    float d = -0.58f*sqrt(distance)+0.58f;
-    //float d = distance;
+}
+
+float IRSensor::readR(){
+    //Rechts B1
+    float b1 = -1.079f*pow(10.0f,4.0f);
+    float b2 = 2.404f*pow(10.0f,4.0f);
+    float b3 = -2.179f*pow(10.0f,4.0f);
+    float b4 = 1.012f*pow(10.0f,4.0f);
+    float b5 = -2519.0f;
+    float b6 = 310.8f;
+    
+    float d1 = b1*pow(distance,5) + b2*pow(distance,4) + b3*pow(distance,3) + b4*pow(distance,2) + b5*distance + b6;
     
-    return d;
+    return d1;
+}
+
+float IRSensor::readC(){
+    //Vorne B4
+    float c1 = -2.681f*pow(10.0f,4.0f);
+    float c2 = 6.007f*pow(10.0f,4.0f);
+    float c3 = -5.32f*pow(10.0f,4.0f);
+    float c4 = 2.377f*pow(10.0f,4.0f);
+    float c5 = -5686.0f;
+    float c6 = 697.8f;
     
-}
\ No newline at end of file
+    float d4 = c1*pow(distance,5) + c2*pow(distance,4) + c3*pow(distance,3) + c4*pow(distance,2) + c5*distance + c6;
+    
+    return d4;
+    }
+
+//float IRSensor::readB(){}
\ No newline at end of file
--- a/IRSensor.h	Wed Apr 11 15:26:03 2018 +0000
+++ b/IRSensor.h	Mon Apr 16 12:44:48 2018 +0000
@@ -10,7 +10,10 @@
     IRSensor (AnalogIn& distance);
                 
     virtual ~IRSensor();
-    float read();
+    float readL();
+    float readR();
+    float readC();
+    float readB();
 private:
     AnalogIn& distance;
 };
--- a/Motion.cpp	Wed Apr 11 15:26:03 2018 +0000
+++ b/Motion.cpp	Mon Apr 16 12:44:48 2018 +0000
@@ -10,71 +10,137 @@
 
 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
                 EncoderCounter& counterRight, IRSensor& irSensorL,
-                IRSensor& irSensorC, IRSensor& irSensorR) :
+                IRSensor& irSensorC, IRSensor& irSensorR, 
+                DigitalOut& enableMotorDriver) :
                 controller(controller), counterLeft(counterLeft),
                 counterRight(counterRight), irSensorL(irSensorL),
-                irSensorC(irSensorC), irSensorR(irSensorR) {
+                irSensorC(irSensorC), irSensorR(irSensorR),
+                enableMotorDriver(enableMotorDriver) {
                     
-                    countsLeft = 0;
-                    countsRight = 0;
+                    countsL = 0;
+                    countsR = 0;
+                    countsLOld = 0;
+                    countsROld = 0 ;
 }
                
 Motion::~Motion() {}
 
+/**
+ * Eine Feldbewegung druchführen
+ */
 void Motion::move() {
     
-        distanceL = irSensorL.read();
-        distanceC = irSensorC.read();
-        distanceR = irSensorR.read();
+        //distanceL = irSensorL.read();
+        //distanceC = irSensorC.read();
+        //distanceR = irSensorR.read();
+        
+        while ((countsL - countsLOld)  < 1630 || (countsR - countsROld) > -1630) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
         
-    while(countsLeft  < 200 || countsRight < 200) { 
-        //Counts für eine Feld Bewegung
-        countsLeft = counterLeft.read();
-        countsRight = counterRight.read();
-    
-        controller.setDesiredSpeedLeft(50.0f);
-        controller.setDesiredSpeedRight(-50.0f);
-    
-    }
+            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+            enableMotorDriver = 1;
+        }    
+        //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
+        
+        stop();
+        enableMotorDriver = 0;
+        countsLOld = countsL + 150;
+        countsROld = countsR - 150;
 }
-    
+
+/**
+ * 90° Rotation nach Links
+ */
 void Motion::rotateL() {
     
-    while(countsLeft  < 200 || countsRight > -200) { 
-        //Count Grenze durch Rechnung bestimmen für 90° Umfang/4
-        countsLeft = counterLeft.read();
-        countsRight = counterRight.read();
-    
-        controller.setDesiredSpeedLeft(50.0f);
-        controller.setDesiredSpeedRight(-50.0f);
-    
-    }
+        while ((countsL - countsLOld)  > -760 || (countsR - countsROld) > -760) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(-50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+            enableMotorDriver = 1;
+        }    
+        
+        stop();
+        enableMotorDriver = 0;
+        countsLOld = countsL - 100;
+        countsROld = countsR - 100;
 }
     
+/**
+ * 90° Rotation nach Rechts
+ */
 void Motion::rotateR() {
     
-    while(countsLeft  > -200 || countsRight < 200) { 
-        //Count Grenze durch Rechnung bestimmen für 90° Umfang/4
-        countsLeft = counterLeft.read();
-        countsRight = counterRight.read();
-    
-        controller.setDesiredSpeedLeft(-50.0f);
-        controller.setDesiredSpeedRight(50.0f);
-    
-    }
+        while ((countsL - countsLOld)  < 760 || (countsR - countsROld) < 760) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedRight(50.0f);
+            enableMotorDriver = 1;
+        }    
+        
+        stop();
+        enableMotorDriver = 0;
+        countsLOld = countsL + 100;
+        countsROld = countsR + 100;
 }
 
-//180° rotation
-void Motion::reverse() {
+/**
+ * Motor Stop
+ */
+void Motion::stop() {
+       
+        controller.setDesiredSpeedLeft(0.0f);
+        controller.setDesiredSpeedRight(0.0f);
+}
+/**
+ * 180° Rotation
+ */
+void Motion::rotate180() {
     
-    while(countsLeft  < 200 || countsRight > -200) { 
-        //Count Grenze durch Rechnung bestimmen für 180° Umfang/2
-        countsLeft = counterLeft.read();
-        countsRight = counterRight.read();
+        while ((countsL - countsLOld)  > -1560 || (countsR - countsROld) > -1560) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(-50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+            enableMotorDriver = 1;
+            printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
+        }    
+        
+        stop();
+        enableMotorDriver = 0;
+        countsLOld = countsL - 150;
+        countsROld = countsR - 150;
+        
+}
+
+void Motion::test() {
+        
+        while (countsL > -1560 || countsR > - 1560) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(-50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+            enableMotorDriver = 1;
+            printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
+        }
+        
+        counterLeft.reset();
+        counterRight.reset();
+        stop();
+        enableMotorDriver = 0;
+}
+
     
-        controller.setDesiredSpeedLeft(50.0f);
-        controller.setDesiredSpeedRight(-50.0f);
-    
-    }
-}
-    
--- a/Motion.h	Wed Apr 11 15:26:03 2018 +0000
+++ b/Motion.h	Mon Apr 16 12:44:48 2018 +0000
@@ -14,13 +14,16 @@
     
         Motion(Controller& controller, EncoderCounter& counterLeft,
                 EncoderCounter& counterRight, IRSensor& irSensorL,
-                IRSensor& irSensorC, IRSensor& irSensorR);
+                IRSensor& irSensorC, IRSensor& irSensorR, 
+                DigitalOut& enableMotorDriver);
         
         virtual ~Motion();
-        void    reverse();
         void    move();
         void    rotateL();
         void    rotateR();
+        void    stop();
+        void    rotate180();
+        void    test();
 
     private:
     
@@ -33,11 +36,14 @@
         IRSensor& irSensorL;
         IRSensor& irSensorC;
         IRSensor& irSensorR;
+        DigitalOut& enableMotorDriver;
         float distanceL;
         float distanceC;
         float distanceR;
-        short countsLeft;
-        short countsRight;
+        short countsL;
+        short countsR;
+        short countsLOld;
+        short countsROld;
     
     };
     
--- a/Path.cpp	Wed Apr 11 15:26:03 2018 +0000
+++ b/Path.cpp	Mon Apr 16 12:44:48 2018 +0000
@@ -3,3 +3,9 @@
 #include "Path.h"
 
 using namespace std;
+
+Path::Path() {}
+
+Path::~Path() {}
+
+void Path::reverse() {}
--- a/Path.h	Wed Apr 11 15:26:03 2018 +0000
+++ b/Path.h	Mon Apr 16 12:44:48 2018 +0000
@@ -6,9 +6,13 @@
 
 class Path {
     
-public:
+    public:
 
-private:
+        Path();
+        virtual ~Path();
+        void    reverse();
+        
+    private:
     
     
     
--- a/main.cpp	Wed Apr 11 15:26:03 2018 +0000
+++ b/main.cpp	Mon Apr 16 12:44:48 2018 +0000
@@ -5,20 +5,21 @@
 #include "IRSensor.h"
 #include "Motion.h"
 
-    Serial pc (SERIAL_TX, SERIAL_RX);
 //User Button
-    DigitalIn button(PC_13);
+    InterruptIn button(USER_BUTTON);
 
 //Sensors:
     
-    AnalogIn lineSensor(PA_0);
-    AnalogIn distanceL(PA_1);
-    AnalogIn distanceC(PA_4);
-    AnalogIn distanceR(PB_0);
+    AnalogIn lineSensor(PA_4);
+    AnalogIn distance2(PC_3);
+    AnalogIn distance4(PB_1);
+    AnalogIn distance1(PC_2);
+    AnalogIn distance3(PC_5);
 
-    IRSensor irSensorL (distanceL);
-    IRSensor irSensorC (distanceC);
-    IRSensor irSensorR (distanceR);
+    IRSensor irSensorL (distance2);
+    IRSensor irSensorC (distance4);
+    IRSensor irSensorR (distance1);
+    IRSensor irSensorB (distance3);
 
 //Motors:
     DigitalOut myled(LED1);
@@ -28,19 +29,38 @@
     DigitalIn motorDriverFault(PB_14);
     DigitalIn motorDriverWarning(PB_15);
     
-    PwmOut pwmLeft(PA_8);
-    PwmOut pwmRight(PA_10);
+    PwmOut pwmRight(PA_8);
+    PwmOut pwmLeft(PA_10);
     
-    EncoderCounter counterLeft(PB_6, PB_7);
-    EncoderCounter counterRight(PA_1, PA_0);
+    EncoderCounter counterRight(PB_6, PB_7);
+    EncoderCounter counterLeft(PA_0, PA_1);
+    
     
     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
     
-    Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR);
+    Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, enableMotorDriver);
     
-    int start = 0;
+//------------------------------------------------------------------------------
+    
+    volatile int start = 0;
     short countsLeft = 0;
     short countsRight = 0;
+    short countsLeftOld = 0;
+    short countsRightOld = 0;
+    //den jeweiligen Test auf 1 setzen
+    int testSensor = 0;
+    int testMotion = 0;
+    int testRotation = 0;
+    int testCounts = 0;
+    int testClass = 0;
+    
+//------------------------------------------------------------------------------
+
+//User button toggle
+void press() {
+    start = !start;
+}
+    
     
 //------------------------------------------------------------------------------
 
@@ -50,15 +70,17 @@
     Micromouse Test 
 */
     
-    //den jeweiligen Test auf 1 setzen
-    int testSensor = 0;
-    int testMotion = 0;
-    int testRotation = 0;
-    int testCounts = 0;
+    
+while(1) {
+    
+    button.fall(&press); //User button einlesen
     
-    //User Button einlesen: start Signal
-    if (button.read() && start != 1) start = 1;
-    else if(button.read() && start == 1) start = 0;
+    while(0) {
+        myled = 1;
+        printf("Hai\n");
+        if (start == 0) break;  
+    }
+    
 
 
     /*
@@ -67,85 +89,146 @@
     while(1) {
         
         
-        float distanceL = irSensorL.read();
-        float distanceC = irSensorC.read();
-        float distanceR = irSensorR.read();
-        pc.printf("Rechts: %f  Mitte: %f  Links: %f\n", distanceL, distanceC, distanceR);
+        float distanceL = irSensorL.readL();
+        float distanceC = irSensorC.readC();
+        float distanceR = irSensorR.readR();
+        float distanceB = irSensorB.readC();
+        printf("Links: %f  Mitte: %f  Rechts: %f  Hinten: %f  Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
         
-        wait(0.5f);
+        //wait(0.5f);
     }
     
     /*
     Motoren Test: Gerade Bewegung
     */
-    while(0) {
-        
-        controller.setDesiredSpeedLeft(50.0f); //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(-50.0f);
-        enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
-            while(1) {
+    while(start == 1 && testMotion == 1) {
         
-                myled =! myled;
-                wait(0.1f); //200 ms
-                
-                countsLeft = counterLeft.read();
-                countsRight = counterRight.read();
-                printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
-            }     
+        controller.setDesiredSpeedLeft(20.0f); //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(-20.0f);
+        enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
+                        
+        countsLeft = counterLeft.read();
+        countsRight = counterRight.read();
+        
+        printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
+        
+        if (start == 0) {
+            
+            motion.stop();
+        }
     }
     
     /*
     Motoren Test: 180° Rotation
     */
         
-    while(start && testRotation) {
-        
-        while(countsLeft  < 200 || countsRight > -200) {
+    while(start == 1 && testRotation == 1) {
+            
+            
+        while((countsLeft - countsLeftOld)  > -1614 || (countsRight - countsRightOld) > -1614) {
             
             countsLeft = counterLeft.read();
             countsRight = counterRight.read();
         
-            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedLeft(-50.0f);
             controller.setDesiredSpeedRight(-50.0f);
             enableMotorDriver = 1;
         }
         
+        motion.stop();
+        enableMotorDriver = 0;
+        printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
+        
+        if (start == 0) {
+           countsLeftOld = countsLeft - 100;
+           countsRightOld = countsRight - 100;
+            
+            break;
+        }
+    }
+
+
+/* Benötigte Ticks für 180* Rotation am Ort:
+
+    mm/count = 0.122
+    Dr = 124mm
+    Rr = 60mm
+    180°=> 1597 counts
+    90°=> 798 counts
+    
+        
+    RotationsDistanz Rd= DistanzRäder*pi/2 = 194.8
+    
+    UmfangRad = Drad*pi = 188.5
+    
+    
+    Counts = Rd/mm pro Count
+    
+    => Counts pro Drehung rausfinden */
+    while(start == 1 && testCounts == 1) {
+        
+        while(countsLeft  < 5000 || countsRight > -5000) {
+            
+            countsLeft = counterLeft.read();
+            countsRight = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(20.0f);
+            controller.setDesiredSpeedRight(-20.0f);
+            enableMotorDriver = 1;
+        }
+        
         controller.setDesiredSpeedLeft(0.0f);
         controller.setDesiredSpeedRight(0.0f); 
         enableMotorDriver = 0;
         
     }
-
-
-/* Benötigte Ticks für 180* Rotation am Ort:
-
-    RotationsDistanz Rd= DistanzRäder*pi/2
     
-    UmfangRad = Drad*pi
-    
-    mm pro Count = UmfangRad/Counts pro Drehung
-    
-    Counts = Rd/mm pro Count
-    
-    => Counts pro Drehung rausfinden */
-    while(start && testCounts) {
+    //Class test
+    if(start == 1 && testClass == 1) {
+        motion.rotate180();
+        counterLeft.reset();
+        counterRight.reset();
+        start = 0;
+        }
         
-        while(countsLeft  < 100 || countsRight < 100) {
-            
-            countsLeft = counterLeft.read();
-            countsRight = counterRight.read();
+    //Sensor calib
+    if(start == 1) {
+        int i;
+        myled = !myled;
+        for (i=0;i<5;i++) {
+            float distanceL = irSensorL.readL();
+            float distanceC = irSensorC.readC();
+            float distanceR = irSensorR.readR();
+            float distanceB = irSensorB.readC();
+            printf("Links: %f  Mitte: %f  Rechts: %f  Hinten: %f  Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
+       }
+        printf("------------------------\n");
+        start = 0;
         
-            controller.setDesiredSpeedLeft(20.0f);
-            controller.setDesiredSpeedRight(20.0f);
-            enableMotorDriver = 1;
+       /* while((countsLeft - countsLeftOld) > -82 || (countsRight - countsRightOld) < 82) {
+        
+        controller.setDesiredSpeedLeft(-10.0f); //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(10.0f);
+        enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
+                        
+        countsLeft = counterLeft.read();
+        countsRight = counterRight.read();
         }
         
         controller.setDesiredSpeedLeft(0.0f);
         controller.setDesiredSpeedRight(0.0f); 
         enableMotorDriver = 0;
         
+        float distanceL = irSensorB.read();
+        printf("Hinten: %f\n", distanceL);
+        
+        countsLeftOld = countsLeft;
+        countsRightOld = countsRight;
+        start = 0;*/
+        
     }
 }
+}