Diff: main.cpp

Revision:

0:2aa29a0824df

Child:

1:23834862b877

diff -r 000000000000 -r 2aa29a0824df main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 30 14:17:16 2018 +0000
@@ -0,0 +1,244 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "HIDScope.h"
+
+DigitalOut gpo(D0);
+DigitalOut led(LED_RED);
+AnalogIn pot1(A0);
+AnalogIn pot2(A1);
+AnalogIn pot3(A2);
+QEI encoder1(D10, D11, NC, 32);
+QEI encoder2(D12, D13, NC, 32);
+FastPWM Motor1PWM(D6);
+DigitalOut Motor1Direction(D7);
+FastPWM Motor2PWM(D5);            //!!!!! Juiste poorten aangeven
+DigitalOut Motor2Direction(D4);   //!!!!! Juiste poort aangeven
+
+MODSERIAL pc(USBTX, USBRX);
+
+//HIDSCOPE
+HIDScope    scope(4);
+Ticker      scopeTimer;
+
+
+///Variables
+
+double q1Error;
+double q2Error;
+double PrevErrorq1[100];
+double PrevErrorq2[100];
+int n = 100;            // Zelfde waarde als PrevErrorarray
+double q1Ref = 1.0;     //VOOR TESTEN
+double q2Ref;
+//Double xPos;
+//Double yPos;
+double xRef;
+double yRef;
+double q1Pos;
+double q2Pos;
+double PIDerrorq1;
+double PIDerrorq2;
+double IntegralErrorq1 = 0.0;
+double IntegralErrorq2 = 0.0;
+double DerivativeErrorq1 = 0.0;
+double DerivativeErrorq2 = 0.0;
+double GainP1 = 2.0;
+double GainI1 = 1.2;
+double GainD1 = 0.0;
+double GainP2 = 2.0;
+double GainI2 = 2.0;
+double GainD2 = 2.0;
+double TickerPeriod = 1.0/400.0;
+
+Ticker Kikker;
+int count = 0;
+int countstep = 0;
+
+
+
+void UpdateRefPosition()
+{
+  //  Update, (based on EMG or pots) the reference position xRef, yRef
+    //q1Ref = pot1.read()*2*3.1416;
+    //q2Ref = pot2.read()*2*3.1416;
+}
+
+
+void InverseKinematics()
+{
+  //  Convert, using the inverse kinematics relations, xRef and yRef to q1Ref and q2Ref
+// (So, update the values of q1Ref and q2Ref)
+}
+
+
+void ReadCurrentPosition()  //Update the coordinates of the end-effector q1Pos, q2Pos
+{    
+    q1Pos = encoder1.getPulses()/32/131.25*2*3.1416;        //Position motor 1 in rad
+    q2Pos = encoder2.getPulses()/32/131.25*2*3.1416;        //Position motor 2 in rad 
+}   
+
+void UpdateError()    //Update the q1Error and q2Error values based on q1Ref, q2Ref, q1Pos, q2Pos
+{
+    q1Error = q1Ref - q1Pos;
+    q2Error = q2Ref - q2Pos;
+
+    //Update PrevErrorq1 and PrevErrorq2
+    
+    for (int i = 0; i <= n-2 ; i++)
+    {
+        PrevErrorq1[i] = PrevErrorq1[i+1];
+        PrevErrorq2[i] = PrevErrorq2[i+1];
+    }
+    
+    PrevErrorq1[n-1] = q1Error;
+    PrevErrorq2[n-1] = q2Error;    
+}
+
+void PIDControl(){
+    // Update PIDerrorq1 and PIDerrorq2 based on the gains and the values q1Error and q2Error
+    
+    
+    // PID control motor 1
+        //P-Control
+        double P_error1 = GainP1 * q1Error;
+     
+        //I-Control
+        IntegralErrorq1 = IntegralErrorq1 + q1Error * TickerPeriod;
+        double I_error1 = GainI1 * IntegralErrorq1;
+     
+        //D-Control
+        //First smoothen the error signal
+        double MovAvq1_1 = 0.0;
+        double MovAvq1_2 = 0.0;
+        for (int i=0; i<=n-2; i++){   // Creates two mov. av. with one element shifted
+            MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1));
+            MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1));
+        }
+        DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod;    
+        double D_error1 = GainD1 * DerivativeErrorq1;
+        // Derivative error sum over all time?   
+     
+    PIDerrorq1 = P_error1 + I_error1 + D_error1;
+    
+    // PID control motor 2
+        //P-Control
+        double P_error2 = GainP2 * q2Error;
+     
+        //I-Control
+        IntegralErrorq2 = IntegralErrorq2 + q2Error * TickerPeriod;
+        double I_error2 = GainI2 * IntegralErrorq2;
+     
+        //D-Control
+        //First smoothen the error signal
+        double MovAvq2_1 = 0.0;
+        double MovAvq2_2 = 0.0;
+        for (int i=0; i<=n-2; i++){   // Creates two mov. av. with one element shifted
+            MovAvq2_1 = MovAvq2_1 + PrevErrorq2[i]/((double)(n-1));
+            MovAvq2_2 = MovAvq2_2 + PrevErrorq2[i+1]/((double)(n-1));
+        }
+        DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod;    
+        double D_error2 = GainD2 * DerivativeErrorq2;
+        // Derivative error sum over all time?   
+     
+    PIDerrorq2 = P_error2 + I_error2 + D_error2;
+}
+
+    
+void MotorControl()
+{
+        //Motor 1 
+        //Keep signal within bounds
+        if (PIDerrorq1 > 1.0){
+            PIDerrorq1 = 1.0;
+            }
+        else if (PIDerrorq1 < -1.0){
+            PIDerrorq1 = -1.0;
+            }     
+        //Direction    
+        if (PIDerrorq1 <= 0){
+            Motor1Direction = 0;
+            Motor1PWM.write(-PIDerrorq1); //write Duty cycle 
+        }        
+        if (PIDerrorq1 >= 0){
+            Motor1Direction = 1;
+            Motor1PWM.write(PIDerrorq1); //write Duty cycle 
+        }    
+        
+        //Motor 2 
+        //Keep signal within bounds
+        if (PIDerrorq2 > 1.0){
+            PIDerrorq2 = 1.0;
+            }
+        else if (PIDerrorq2 < -1.0){
+            PIDerrorq2 = -1.0;
+            }     
+        //Direction           
+          
+        if (PIDerrorq2 <= 0){
+            Motor2Direction = 0;
+            Motor2PWM.write(-PIDerrorq2); //write Duty cycle 
+        }        
+        if (PIDerrorq2 >= 0){
+            Motor2Direction = 1;
+            Motor2PWM.write(PIDerrorq2); //write Duty cycle 
+        }    
+}
+
+void NormalOperatingModus()
+{
+     UpdateRefPosition();
+     InverseKinematics();
+     ReadCurrentPosition();
+     UpdateError();
+     PIDControl();
+     MotorControl();
+     
+     scope.set(0, q1Pos);
+     scope.set(1, q1Ref);
+     
+     GainP1 = pot3.read() * 10;
+     GainI1 = pot2.read() * 10;
+     GainD1 = pot1.read() ;
+          
+     countstep++;
+     count++;
+     if (count == 400) // print 1x per seconde waardes.
+    {
+        pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref);
+        count = 0;   
+    }
+     if (countstep == 4000)
+     {
+         q1Ref = !q1Ref;
+         countstep = 0;
+     }    
+         
+    
+}
+ 
+ 
+int main()
+{
+        pc.baud(115200);
+        //Initialize array errors to 0
+        for (int i = 0; i <= 9; i++){
+             PrevErrorq2[i] = 0;
+             PrevErrorq2[i] = 0;
+             }
+             
+        int frequency_pwm = 16700; //16.7 kHz PWM
+        Motor1PWM.period(1.0/frequency_pwm); // T = 1/f
+        Motor2PWM.period(1.0/frequency_pwm); // T = 1/f     
+        
+        Kikker.attach(NormalOperatingModus, TickerPeriod);
+        scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
+        
+        while(true);
+        {}        
+}           
+             
+             
+             
+