Revision 4:91a9737e4821, committed 2018-04-20

Comitter:

ahlervin

Date:

Fri Apr 20 17:49:36 2018 +0000

Parent:

3:6e28589a732f

Commit message:

Sensoren auswerten, Codieren und State Machine

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IRSensor.cpp	Fri Apr 20 17:49:36 2018 +0000
@@ -0,0 +1,88 @@
+
+//Implementation IR Sensoren
+// V04.18
+// V. Ahlers
+
+#include <cmath>
+#include "IRSensor.h"
+
+using namespace std;
+
+    const float IRSensor :: PR1 = 3.4734*0.000000001;    //Koeffizienten
+    const float IRSensor :: PR2 = -7.1846*0.000001;
+    const float IRSensor :: PR3 = 0.0055;
+    const float IRSensor :: PR4 = -1.9304;
+    const float IRSensor :: PR5 = 301.2428;
+    const float IRSensor :: PL1 = 3.4734*0.000000001;
+    const float IRSensor :: PL2 = -7.1846*0.000001;
+    const float IRSensor :: PL3 = 0.0055;
+    const float IRSensor :: PL4 = -1.9304;
+    const float IRSensor :: PL5 = 301.2428;
+    const float IRSensor :: PF1 = 6.1767f*pow(10.0f,-10);
+    const float IRSensor :: PF2 = -1.9975f*pow(10.0f,-6);
+    const float IRSensor :: PF3 = 0.0024f;
+    const float IRSensor :: PF4 = -1.3299f;
+    const float IRSensor :: PF5 = 351.1557f;
+    const int IRSensor :: minIrR = 0;   //Noch definieren
+    const int IRSensor :: minIrL = 0;
+    const int IRSensor :: minIrF = 0;
+
+
+
+// IR Sensor Distanz in mm
+IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F) : 
+IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F){}
+                     
+IRSensor::~IRSensor(){}
+
+float IRSensor::readR() {
+    
+        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)
+        
+        return disR;    
+}
+
+float IRSensor::readL(){
+    
+        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)
+        
+        return disL;    
+}
+
+float IRSensor::readF(){
+    
+        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)
+        
+        return disF;    
+}
+
+// IR Sensor Zusatand
+int IRSensor::codeR(){
+    
+        if(disR < minIrR) {
+            IrR = 1;
+            } else { IrR = 0; }
+        return IrR;
+}
+
+int IRSensor ::codeL(){
+    
+        if(disL < minIrL) {
+            IrL = 1;
+            } else { IrL = 0; }
+        return IrL;     
+}
+
+int IRSensor ::codeF(){
+    
+        if(disF < minIrR) {
+            IrF = 1;
+            } else { IrF = 0; }
+        return IrF;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IRSensor.h	Fri Apr 20 17:49:36 2018 +0000
@@ -0,0 +1,64 @@
+// Deklaration IR Sensoren
+// V04.18
+// V. Ahlers
+
+
+#ifndef IRSENSOR_H_
+#define IRSENSOR_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+ class IRSensor {
+    
+    public:
+        IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F); 
+        
+        float disR;
+        float disL;
+        float disF;
+        int IrR;
+        int IrL;
+        int IrF;
+
+        float measR;
+        float measL;
+        float measF;
+
+        virtual ~IRSensor();
+        float readR();
+        float readL();
+        float readF();
+        int codeR();
+        int codeL();
+        int codeF();
+        
+        private:
+        AnalogIn& IrRight;
+        AnalogIn& IrLeft;
+        AnalogIn& IrFront;
+        float dis2R;
+        float dis2L;
+        float dis2F;
+        static const float PR1;
+        static const float PR2;
+        static const float PR3;
+        static const float PR4;
+        static const float PR5;
+        static const float PL1;
+        static const float PL2;
+        static const float PL3;
+        static const float PL4;
+        static const float PL5;
+        static const float PF1;
+        static const float PF2;
+        static const float PF3;
+        static const float PF4;
+        static const float PF5;
+        static const int minIrR;
+        static const int minIrL;
+        static const int minIrF;
+        
+};   
+
+#endif /*IRSensor_H_*/

--- a/StateMachine.cpp	Thu Apr 19 12:04:49 2018 +0000
+++ b/StateMachine.cpp	Fri Apr 20 17:49:36 2018 +0000
@@ -1,78 +1,35 @@
-#include "mbed.h"
- 
-case FORWAERTSFAHREN:
+
+// Statemachine
+//V04.18
+// V. Ahlers
+
+#include <mbed.h>
+#include "StateMachine.h"
+
+using namespace std;
+
+StateMachine::StateMachine(int IrR, int IrL, int IrF) : IrR(IrR), IrL(IrL), IrF(IrF) {}
+
+StateMachine::~StateMachine (){}
 
-    buttonNow = button->read();
-    if (buttonNow && !buttonBefore){
-        controller-> setTranslationVelocity(0.0f);
-        state = LANGSAM_FAHREN;
-        }
-        else if(((irSensorF.read() < DISTANCE_F)
-        && (irSensorR.read() < DISTANCE_R)) {
-            controller.setTranslationVelocity(0.0f);
-            controller.setRotationVelocity(ROTATION_VELOCITY);
-            state = 270_GRAD_RECHTS;
-            }
-        else if(irSensorR.read() > DISTANCE_R){
-        controller.setTranslationVelocity(0.0f);
-         controller.setRotationVelocity(ROTATION_VELOCITY);
-            state = 90_GRAD_RECHTS;
-            }
-        else if((irSensorF.read() < DISTANCE_F)
-        && (irSensorR.read() < DISTANCE_R) (SensorL.read() < DISTANCE_THRESHOLD)) {
-            controller.setTranslationVelocity(0.0f);
-            controller.setRotationVelocity(ROTATION_VELOCITY);
-            state = 180_GRAD_RECHTS;
-            }
-            buttonBefore = buttonNow;
-            break;
-            
- 
-case 90_GRAD_RECHTS
- 
-    buttonNOW = button.read();
-    if (buttonNow && !buttonBefore {  // deect button rising edge
-    controller.setRotationalVelocity(0.0f);
-    state = LANGSAM_FAHREN;
-        } else if ((irSensorR < DISTANCE_R) {
-        controller.setTranslationalvelocity(TRANSLATIONAL_VELOCITY);
-        state = VORWAERTSFAHREN;
-    }
+int StateMachine :: drive() {
     
-    
- 
-case 270_GRAD_RECHTS
- 
-    buttonNow = button.read();
-    if (buttonNow && !buttonBefore {  //detect button rising edge
-        controller.set RotationalVelociy(0.0f);
-        state = LANGSAM_FAHREN;
-        } else if ((irSensorF.read() > DISTANCE_F)
-                && (irSensorR.read() > DISTANCE_R)
-                && (irSensorL.read() < DISTANCE_L) {
-    controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
-    state= VORWAERTSFAHREN;
-}
-
+    if((IrR==0) && (IrL==0) && (IrF==1)){
+        caseDrive = 2;
+    }else { if ((IrR==0) && (IrL==1) && (IrF==0)){
+        caseDrive = 2;
+    }else { if ((IrR==0) && (IrL==1) && (IrF==1)){
+        caseDrive = 2;
+    }else { if ((IrR==1) && (IrL==0) && (IrF==0)){
+        caseDrive = 1;
+    }else { if ((IrR==1) && (IrL==1) && (IrF==1)){
+        caseDrive = 3;
+    }else { if ((IrR==1) && (IrL==1) && (IrF==0)){
+        caseDrive = 1;
+    }else { if ((IrR==1) && (IrL==1) && (IrF==1)){
+        caseDrive = 4;
+    }else{ caseDrive=0;
+    }}}}}}}
 
-case 180_GRAD_RECHTS
- 
-    buttonNow && !button.read();
-    if (buttonNow && !buttonBefore {  // detect button rising edge
-        state = LANGSAM_FAHREN;
-        } else if (( irSensorR.read() > DISTANCE_R)
-            && (irSensorF.read() > DISTANCE_F)
-            && (irSensorL.read() > DISTANCE_L)) {
-    controller.setTranslationalVelocity(TRANCLATIONAL_VELOCITY) {
-    state = VORWAERTSFAHREN;
-    
-    
-    
-case LANGSAM_FAHREN;
- 
-    if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f )
-        && (fabs(controller.getActualRotationalVelocity()) < 0.1f ) {
-        enableMotorDriver = 0;
-        state = ROBOT_OFF;
-        }
-        break;
\ No newline at end of file
+    return caseDrive;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.h	Fri Apr 20 17:49:36 2018 +0000
@@ -0,0 +1,30 @@
+// Deklaration StateMachine
+// V04.18
+// V. Ahlers
+
+
+#ifndef STATEMACHINE_H_
+#define STATEMACHINE_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+class StateMachine {
+    
+    public:
+        StateMachine (int IrR, int IrL, int IrF);
+        
+        int IrR;
+        int IrL;
+        int IrF;
+        int caseDrive;
+        
+        virtual ~StateMachine();
+        int drive();
+        
+    private:
+    
+
+};   
+
+#endif /*StateMachine_H_*/
\ No newline at end of file

--- a/main.cpp	Thu Apr 19 12:04:49 2018 +0000
+++ b/main.cpp	Fri Apr 20 17:49:36 2018 +0000
@@ -1,75 +1,54 @@
-#include <mbed.h>
 
-#include "EncoderCounter.h"
-#include "Controller.h"
-#include "IRSensor.h"
+// Main zum Testen der IR Sensoren
+//V04.18
+// V. Ahlers
 
-// DigitalOut myled(LED1);
+#include <mbed.h>
+#include "IRSensor.h"
+#include"StateMachine.h"
 
-//DigitalOut led0(PC_8);
-//DigitalOut led1(PC_6);
-//DigitalOut led2(PB_12);
-//DigitalOut led3(PA_7);
-//DigitalOut led4(PC_0);
-//DigitalOut led5(PC_9);
 
-//AnalogIn distance(PB_1);
-//DigitalOut enable(PC_1);
-//DigitalOut bit0(PH_1);
-//DigitalOut bit1(PC_2);
-//DigitalOut bit2(PC_3);
-
-IRSensor irSensorF(PC_2);
-IRSensor irSensorR(PC_3);
-IRSensor irSensorL(PC_5);
+AnalogIn IrRight(PC_3);
+AnalogIn IrLeft (PC_5);
+AnalogIn IrFront(PC_2);
+float disR = 0;
+float disL = 0;
+float disF = 0;
 
-BottomSensor lineDetector(RX/D0);
-
-DigitalOut enableMotorDriver(PB_2);
-DigitalIn motorDriverFault(PB_14);
-DigitalIn motorDriverWarning(PB_15);
-
-PwmOut pwmLeft(PA_9);
-PwmOut pwmRight(PA_8);
-
-EncoderCounter counterLeft(PB_6, PB_7);
-EncoderCounter counterRight(PA_6, PC_7);
+float dis2R = 0;
+float dis2L = 0;
+float dis2F = 0;
+int IrR = 0;
+int IrL = 0;
+int IrF = 0;
+int caseDrive = 0;
 
-Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
+IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F);
+StateMachine stateMachine(IrR, IrL, IrF);
 
-int main()
-{
+
 
-    controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm]
-    controller.setDesiredSpeedRight(-150.0f);
-    enable = 1;
-    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
-
+int main() {
+ 
+    
+    printf("\n die Distanzen sind\n");
     while(1) {
-
-        myled =! myled;
-
-        if (irSensorf.read() < 0.2f) led0 = 1;
-        else led0 = 0;
+        float disR = iRSensor.readR(); // Distanz in mm
+        float disL = iRSensor.readL();
+        float disF = iRSensor.readF();
+        dis2R = disR;
+        dis2L = disL;
+        dis2F = disF;
+        int IrR = iRSensor.codeR();     // min Distanz unterschritten = 1
+        int IrL = iRSensor.codeL();
+        int IrF = iRSensor.codeF();
+        int caseDrive = stateMachine.drive(); //entscheidung welcher Drive Case
         
-        if (irSensorR.read() < 0.2f) led1 = 1;
-        else led1 = 0;
         
-        if (irSensorL.read() < 0.2f) led2 = 1;
-        else led2 = 0;
-        
-        //if (irSensor3.read() < 0.2f) led3 = 1;
-        //else led3 = 0;
-        
-        //if (irSensor4.read() < 0.2f) led4 = 1;
-        //else led4 = 0;
-        
-        //if (irSensor5.read() < 0.2f) led5 = 1;
-        //else led0 = 5;
-        
-        printf("iSumLeft: %f, iSumRight: %f, counterDiff; %d \n\r", controller.getIntegralLeft(), controller.getIntegralRight(), (counterLeft.read()+counterRight.read()));
-                
-        wait(0.2f); // 100 ms
-        
+        printf ("IR Right = %f \n", disR);
+        printf("IR Left = %f \n",disL);
+        printf("IR Front = %f\n",disF);
+
+        wait (1.0);
     }
-}
+}
\ No newline at end of file