Diff: main.cpp

Revision:

2:7c9974f0947a

Parent:

1:070092564648

Child:

3:2aaf54ce090b

--- a/main.cpp	Wed Oct 31 19:09:22 2018 +0000
+++ b/main.cpp	Wed Mar 27 17:33:56 2019 +0000
@@ -1,18 +1,279 @@
 //Voor het toevoegen van een button:
 #include "mbed.h"
-#include <iostream>
-DigitalOut gpo(D0);
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "BiQuad.h"
 
-DigitalIn button2(SW3);  
-DigitalIn button1(SW2); //or SW2
+// Algemeen
+DigitalIn button3(SW3);  
+DigitalIn button2(SW2); 
+AnalogIn But2(A5);
+AnalogIn But1(A3);
 
 DigitalOut led1(LED_GREEN);
 DigitalOut led2(LED_RED);
 DigitalOut led3(LED_BLUE);
 
+MODSERIAL pc(USBTX, USBRX);
 Timer t;
+Timer t2;
+//Motoren
+DigitalOut direction1(D4);
+PwmOut pwmpin1(D5);
+PwmOut pwmpin2(D6);
+DigitalOut direction2(D7);
+volatile float pwm1;
+volatile float pwm2;
+
+//Encoder
+QEI encoder1 (D10, D9, NC, 1200, QEI::X4_ENCODING);
+QEI encoder2 (D13, D12, NC, 4800, QEI::X4_ENCODING);
+double Pulses1;
+double motor_position1;
+double Pulses2;
+double motor_position2;
+double error1;
+
+//Pot meter
+AnalogIn pot(A1);
+AnalogIn pot0(A0);
+float Pot2;
+float Pot1;
+
+//Ticker
+Ticker Pwm;
+Ticker PotRead;
+Ticker Kdc;
+
+//Kinematica
+double stap1;
+double stap2;
+double KPot;
+
+float ElbowReference;
+float Ellebooghoek1;
+float Ellebooghoek2;
+float Ellebooghoek3;
+float Ellebooghoek4;
+
+float PolsReference;
+float Polshoek1;
+float Polshoek2;
+float Polshoek3;
+float Polshoek4;
+
+float Hoeknieuw1;
+float Hoeknieuw2;
+
+//Limiet in graden
+float lowerlim1 = 0;
+float upperlim1 = 748.8;
+float lowerlim2 = 0;
+float upperlim2 = 1300;   
+
+// VARIABLES PID CONTROLLER
+double Kp1 = 5;          
+double Ki1 = 0;          
+double Kd1 = 1;
+double Kp2 = 6;          // Zonder arm: 6,0,1
+double Ki2 = 0;          
+double Kd2 = 1;          
+double Ts = 0.0005;      // Sample time in seconds
+
+// Functies Kinematica
+float Kinematics1(float KPot)
+{
+
+    if (KPot > 0.45f) {
+        stap1 = KPot*150*Ts;
+        Hoeknieuw1 = PolsReference + stap1;
+        return Hoeknieuw1;
+    }
+
+    else if (KPot < -0.45f) {
+        stap1 = KPot*150*Ts;
+        Hoeknieuw1 = PolsReference + stap1;
+        return Hoeknieuw1;
+    }
+
+    else {
+        return PolsReference;
+    }
+}
+float Kinematics2(float KPot)
+{
+
+    if (KPot > 0.45f) {
+        stap2 = KPot*300*Ts;         
+        Hoeknieuw2 = ElbowReference + stap2;
+        return Hoeknieuw2;
+    }
+
+    else if (KPot < -0.45f) {
+        stap2 = KPot*300*Ts;
+        Hoeknieuw2 = ElbowReference + stap2;
+        return Hoeknieuw2;
+    }
+
+    else {
+        return ElbowReference;
+    }
+}
+
+float Limits1(float Polshoek2)
+{
+
+    if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) {       //Binnen de limieten
+        Polshoek3 = Polshoek2;
+    }
+
+    else {
+        if (Polshoek2 >= upperlim1) {                            //Boven de limiet
+            Polshoek3 = upperlim1;
+        } else {                                                    //Onder de limiet
+            Polshoek3 = lowerlim1;
+        }
+    }
+
+    return Polshoek3;
+}
+
+
+float Limits2(float Ellebooghoek2)
+{
 
-enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK}; 
+    if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) {       //Binnen de limieten
+        Ellebooghoek3 = Ellebooghoek2;
+    }
+
+    else {
+        if (Ellebooghoek2 >= upperlim2) {                            //Boven de limiet
+            Ellebooghoek3 = upperlim2;
+        } else {                                                    //Onder de limiet
+            Ellebooghoek3 = lowerlim2;
+        }
+    }
+
+    return Ellebooghoek3;
+}
+
+
+// PID Controller
+double PID_controller1(double error1)
+{
+    static double error1_integral = 0;
+    static double error1_prev = error1; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
+      
+    // Proportional part:
+    double u_k1 = Kp1 * error1;
+
+    // Integral part
+    error1_integral = error1_integral + error1 * Ts;
+    double u_i1 = Ki1* error1_integral;
+
+    // Derivative part
+    double error1_derivative = (error1 - error1_prev)/Ts;
+    double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
+    double u_d1 = Kd1 * filtered_error1_derivative;
+    error1_prev = error1;
+
+    // Sum all parts and return it
+    return u_k1 + u_i1 + u_d1;
+}
+double PID_controller2(double error2)
+{
+    static double error2_integral = 0;
+    static double error2_prev = error2; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
+       
+    // Proportional part:
+    double u_k2 = Kp2 * error2;
+
+    // Integral part
+    error2_integral = error2_integral + error2 * Ts;
+    double u_i2 = Ki2 * error2_integral;
+
+    // Derivative part
+    double error2_derivative = (error2 - error2_prev)/Ts;
+    double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
+    double u_d2 = Kd2 * filtered_error2_derivative;
+    error2_prev = error2;
+
+    // Sum all parts and return it
+    return u_k2 + u_i2 + u_d2;
+}
+
+// Functies Motor
+void moter1_control(double u1)
+{
+    direction1= u1 > 0.0f; //positief = CW
+    if (fabs(u1)> 0.5f) {
+        u1 = 0.5f;
+    } else {
+        u1= u1;
+    }
+    pwmpin1= fabs(u1); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+}
+
+void moter2_control(double u2)
+{
+    direction2= u2 < 0.0f; //positief = CW
+    if (fabs(u2)> 0.99f) {
+        u2 = 0.99f;
+    } else {
+        u2= u2;
+    }
+    pwmpin2= fabs(u2); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+}
+
+void PwmMotor(void)
+{
+    // Reference hoek berekenen, in graden
+    float Ellebooghoek1 = Kinematics2(pwm2);
+    float Ellebooghoek4 = Limits2(Ellebooghoek1);
+    ElbowReference = Ellebooghoek4;
+    
+    float Polshoek1 = Kinematics1(pwm2);
+    float Polshoek4 = Limits1(Polshoek1);
+    PolsReference = Polshoek4;
+
+    // Positie motor berekenen, in graden
+    Pulses1 = encoder1.getPulses();
+    motor_position1 = -(Pulses1/1200)*360;
+    Pulses2 = encoder2.getPulses();
+    motor_position2 = -(Pulses2/4800)*360;
+
+    double error1 = PolsReference - motor_position1;
+    double u1 = PID_controller1(error1);
+    moter1_control(u1);
+    
+    double error2 = ElbowReference - motor_position2;
+    double u2 = PID_controller2(error2);
+    moter2_control(u2);
+
+}
+
+void MotorOn(void)
+{
+    pwmpin1 = 0;
+    pwmpin2 = 0;
+    Pwm.attach (PwmMotor, Ts);
+
+}
+
+
+void ContinuousReader(void)
+{
+    Pot2 = pot.read();
+    Pot1 = pot0.read();
+    pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
+    pwm1 =(Pot1*2)-1;
+}
+
+// StateMachine
+
+enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE}; 
 int f = 1;
 states currentState = MOTORS_OFF; 
 bool stateChanged = true; // Make sure the initialization of first state is executed
@@ -26,6 +287,7 @@
       if (stateChanged)
       {
         // state initialization: rood
+
         led1 = 1;
         led2 = 0; 
         led3 = 1;
@@ -34,14 +296,9 @@
       }
     
       // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
-        if (!button1)
+        if (!button3)
         {        
-        currentState = CALIBRATION;
-        stateChanged = true;
-        }
-        else if (!button2)
-        {        
-        currentState = HOMING  ;
+        currentState = CALIBRATION  ;
         stateChanged = true;
         }
         else
@@ -67,11 +324,11 @@
       
       // State transition logic: automatisch terug naar motors off.
 
-        currentState = MOTORS_OFF;
-        stateChanged = true; 
+        currentState = HOMING1;
+        stateChanged = true;
         break; 
-      
-    case HOMING:
+
+case HOMING1:
     // Actions
       if (stateChanged)
       {
@@ -80,21 +337,36 @@
         led1 = 0;
         led2 = 1;
         led3 = 1;
-        wait (1);
+        
+        if (!But1)
+        {
+        led1 = 1;
+        float H1 = 0.98f;
+        moter1_control(H1);
+        wait(0.001f);
+        }
+        else if (!But2)
+        {
+        led1 = 1;
+        float H1 = -0.98f;
+        moter1_control(H1);
+        wait(0.001f);
+        }
+        Pulses1 = 0;
+        pwmpin1 = 0;
+        pwmpin2 = 0;
+        ;
         
         stateChanged = false;
       }
           
-      // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
-        if (!button1)
+      // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
+
+        if (!button3)
         {        
-        currentState = DEMO;
+        currentState = HOMING2  ;
         stateChanged = true;
-        }
-        else if (!button2)
-        {        
-        currentState = MOVEMENT  ;
-        stateChanged = true;
+        wait(1);
         }
         else if (t>300) 
         {        
@@ -105,7 +377,68 @@
         }
         else
         {        
-        currentState = HOMING  ;
+        currentState = HOMING1  ;
+        stateChanged = true;
+        }
+        break;      
+    
+    case HOMING2:
+    // Actions
+      if (stateChanged)
+      {
+        // state initialization: green
+        t.start();
+        led1 = 0;
+        led2 = 1;
+        led3 = 1;
+        
+        if (!But1)
+        {
+        led1 = 1;
+        float H2 = 0.98f;
+        moter2_control(H2);
+        wait(0.001f);
+        }
+        else if (!But2)
+        {
+        led1 = 1;
+        float H2 = -0.98f;
+        moter2_control(H2);
+        wait(0.001f);
+        }
+        Pulses2 = 0;
+        pwmpin1 = 0;
+        pwmpin2 = 0;
+        ;
+        
+        stateChanged = false;
+      }
+          
+      // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
+        if (!button2)
+        {        
+        currentState = DEMO;
+        stateChanged = true;
+        }
+        else if (!button3)
+        {        
+        currentState = MOVEMENT  ;
+        stateChanged = true;
+        led1 = 1;
+        led2 = 0;
+        led3 = 0;
+        wait(1);
+        }
+        else if (t>300) 
+        {        
+        t.stop();
+        t.reset();
+        currentState = MOTORS_OFF  ;
+        stateChanged = true;
+        }
+        else
+        {        
+        currentState = HOMING2  ;
         stateChanged = true;
         }
         break;
@@ -124,7 +457,7 @@
       }
           
       // State transition logic: automatisch terug naar HOMING
-        currentState = HOMING;
+        currentState = HOMING1;
         stateChanged = true;
         break;
               
@@ -134,22 +467,27 @@
       {
         // state initialization: purple
         t.start();
+        pwmpin1 = 0;
+        pwmpin2 = 0;
+        Pwm.attach (PwmMotor, Ts);
         led1 = 1;
         led2 = 0;
         led3 = 0;
-        wait (1);
-        
+              
         stateChanged = false;
       }
           
-      // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
-        if (!button1)
+      // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3) anders naar MOVEMENT
+        if (!button2)
         {        
-        currentState = CLICK;
+        currentState = FREEZE;
         stateChanged = true; 
         }
-        else if (!button2)
+        else if (!button3)
         {        
+        Pwm.detach ();
+        pwmpin2 = 0;
+        pwmpin1 = 0;
         currentState = MOTORS_OFF  ;
         stateChanged = true;
         }
@@ -157,7 +495,7 @@
         {        
         t.stop();
         t.reset();
-        currentState = HOMING  ;
+        currentState = HOMING1  ;
         stateChanged = true;
         }
         else
@@ -167,7 +505,7 @@
         }
         break;
         
-        case CLICK:
+        case FREEZE:
     // Actions
       if (stateChanged)
       {
@@ -191,19 +529,42 @@
  
 int main()
 {
-    while (true)
+    
+    t2.start();
+    int counter = 0;
+    pwmpin2.period_us(60);
+    PotRead.attach(ContinuousReader,Ts);
+    pc.baud(115200);
+    
+    while(true)
     {
     led1 = 1;
-    led2 = 1;
-    led3 = 1;
+    led2 =1;
+    led3 =1;
+    if(counter==10) 
+        {
+        float tmp = t2.read();
+        printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2);
+        counter = 0;
+        }
+    counter++;
+    
     ProcessStateMachine();
+
     
+    wait(0.001);
+    }
     }
     
-}
+
+    
+    
+    
+    
 
 
 
 
 
 
+