Diff: main.cpp

Revision:

0:4c07bb5a9f9f

Child:

1:b6a079ca16e0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 27 11:42:57 2017 +0000
@@ -0,0 +1,184 @@
+#include "mbed.h"
+#include "Serial.h"
+#include "math.h"
+#include "QEI.h"
+
+// Connecties
+Serial      pc(USBTX, USBRX);        //Serial PC connectie
+DigitalOut  motor1DirectionPin(D4);  //Motorrichting op D4 (connected op het bord)
+PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
+DigitalOut  motor2DirectionPin(D6);  //Motorrichting op D4 (connected op het bord)
+PwmOut      motor2MagnitudePin(D7);  //Motorkracht op D5 (connected op het bord)
+QEI q1_enc(D13, D12, NC, 32);       //encoder motor 1 instellen
+QEI q2_enc(D11, D10, NC, 32);       // encoder motor 2 instellen
+const double pi = 3.1415926535897;  // waarde voor pi aanmaken
+
+// globale gegevens
+Ticker tick_sample; // ticker voor aanroepen aansturing
+char key;
+double ts=0.001;
+int q1_puls;
+int q2_puls;
+
+// kinematica gegevens
+// lengte armen
+double L1 = 0.250;
+double L2 = 0.355;
+double L3 = 0.150;
+
+// reference position
+double q1=0;    // positie q2 in radialen
+double q2=0;    // positie q2 in radialen
+double q1_pos;
+double q2_pos;
+
+// EMG Input_k
+double vx = 0;
+double vy = 0;
+
+
+// PID gegevens
+double pulses2rad;
+double position;
+double ref1;
+double ref2;
+double PD1;
+double PD2;
+double qset1=0;
+double qset1m=0;
+double qset2=0;
+double qset2m=0;
+
+double Kp;   // proportional coefficient       (
+double Kd;   // differential coefficient
+
+
+void wasd(double& vx, double& vy)
+{
+    if(pc.readable()==true)  
+    {   key = pc.getc();
+        if (key=='a')
+        {
+        vx = 0.02;     //referentie snelheid m/s
+        vy = 0;
+        }
+        else if(key=='d')
+        {
+        vx = -0.02;
+        vy = 0;
+        }
+        else if(key=='w')
+        {
+        vx = 0;
+        vy = 0.02;          
+        }
+        else if(key=='s')
+        {
+        vx = 0;
+        vy = -0.02;          
+        }
+        else 
+        {
+            vx=0;
+            vy=0;
+        }
+     }
+     else
+     {
+     vx=0;
+     vy=0;    
+     }
+}
+
+double kinematics(double& q1, double& q2, double q1_pos, double q2_pos, double vx, double vy )
+    {
+               
+        q1 = ((-(L3 + L2*cos(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vx  +  (-(L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q1_pos;
+        q2 = (((L3 + L2*cos(q2_pos) - L1*sin(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos)) * vx) + ((L1*cos(q1_pos) + L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q2_pos;
+    return 0; 
+    }
+
+void controller(double qset1, double qset2)
+    {
+        // error = qset
+        // referentie bepalen    
+        ref1 = qset1 + q1_pos;                // genereert het referentiesignaal
+        ref2 = qset2 + q2_pos;
+        
+
+        //PD 
+        PD1 = Kp*(qset1) + Kd*((qset1m-qset1)/ts) ; // PD sum; PD = proportianal + differential
+        qset1m = qset1;                              // waarde voor qset wordt opgeslagen (m = memory)
+        PD2 = Kp*(qset2) + Kd*((qset2m-qset2)/ts) ;
+        qset2m = qset2;
+        
+        //Motor control
+        if (PD1<0 && PD2<0 )
+        {
+        motor1MagnitudePin = fabs(PD1);
+        motor1DirectionPin = 1;
+        motor2MagnitudePin = fabs(PD2);
+        motor2DirectionPin = 1;
+        }
+        else if (PD1>0 && PD2<0)
+        {
+        motor1MagnitudePin = fabs(PD1);
+        motor1DirectionPin = 0;
+        motor2MagnitudePin = fabs(PD2);
+        motor2DirectionPin = 1;
+        }
+        
+        else if (PD1<0 && PD2>0)
+        {
+        motor1MagnitudePin = fabs(PD1);
+        motor1DirectionPin = 1;
+        motor2MagnitudePin = fabs(PD2);
+        motor2DirectionPin = 0;
+        }
+
+        else if (PD1>0 && PD2>0)
+        {
+        motor1MagnitudePin = fabs(PD1);
+        motor1DirectionPin = 0;
+        motor2MagnitudePin = fabs(PD2);
+        motor2DirectionPin = 0;
+        }
+   
+        
+        
+    }
+
+void aansturing()
+    {
+    // encoders uitlezen    
+    q1_puls = q1_enc.getPulses();
+    q1_pos = q1_puls*pulses2rad;       // berekent positie q1 in radialen
+    q2_puls = q2_enc.getPulses();
+    q2_pos = q2_puls*pulses2rad;       // berekent positie q2 in radialen    
+    
+    // pijltjestoetsen
+    wasd(vx, vy);
+    
+     // kinematica              bepaald gewenste q1 en q2 referenties afhankelijk van ingegeven vx en vy
+     kinematics(q1, q2, q1_pos, q2_pos, vx, vy);
+     
+     // PD controller       gebruikt PD control en stuurt motor aan
+     controller(q1, q2);
+        
+    }
+
+
+int main()
+{
+    pc.baud(115200);
+    pulses2rad=(2*pi)/4200;
+    Kp = 0.5/pi;
+    Kd = 0.1*Kp;
+    tick_sample.attach_us(&aansturing, 1000); //sample frequency 1 mHz;
+    
+    while(true)
+    {
+    }
+    
+
+}
\ No newline at end of file