Diff: main.cpp

Revision:

0:8336c89b41ab

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 26 13:32:38 2017 +0000
@@ -0,0 +1,116 @@
+#include "mbed.h"
+#include "Serial.h"
+#include "QEI.h"
+#include "math.h"
+
+
+Serial      pc(USBTX, USBRX);        //Serial PC connectie
+QEI encoder(D13, D12, NC, 32);       //encoder instellen
+DigitalOut  motor1DirectionPin(D4);  //Motorrichting op D4 (connected op het bord)
+PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
+Ticker      controller;                //toets sample tijd
+double fs = 10000;                     // sample frequentie
+double ts = 1/fs;
+
+// variablen voor void
+int pulses;
+int ref;
+int er;
+float Kp=0.0005;
+float P;
+float D;
+float Dif;
+float Kd=0.00012;
+float PID;
+int er2=0;
+
+// constantes_k
+float L1 = 0.250;
+float L2 = 0.355;
+float L3 = 0.150;
+
+
+// Reference position_k
+float q1_1 = 0;
+float q2_1 = 0;
+float q1_2;
+float q2_2;
+
+// EMG Input_k
+float vx ;
+float vy ;
+
+
+// Encoder input_k
+float q1_enc; //encoder actuator 1
+float q2_enc; //encoder actuator 2
+
+
+void PD()
+{
+char key;
+    if(pc.readable()==true)  
+    {   key = pc.getc();
+        if (key=='q')
+        {
+        ref=-500;        //reference wordt 500 pulses
+        }
+        else if(key=='w')
+        {
+        ref=0;
+        }
+        else if(key=='e')
+        {
+        ref=500;          //reference wordt 0 pulses
+        }
+     }   
+//error bepalen    
+pulses=encoder.getPulses();
+er=ref-pulses;
+
+//PID
+//Proportional part
+P = Kp*er;
+
+//Differential part
+Dif=(er2-er)/ts;
+D=Kd*Dif;
+
+//PID sum
+PID=P+D;
+er2=er;
+
+    //Motor control
+    if (P<0)
+    {
+    motor1MagnitudePin = fabs(P);
+    motor1DirectionPin = 1;
+    }
+    else if (P>0)
+    {
+    motor1MagnitudePin = fabs(P);
+    motor1DirectionPin = 0;
+    }
+
+}
+
+// Calculating q_set_k
+void Kinematics()
+    {
+        q1_1 = q1_2;
+        q2_1 = q2_2;
+               
+        q1_2 = ((-(L3 + L2*cos(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vx  +  (-(L2*sin(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vy) * ts + q1_enc;
+        q2_2 = (((L3 + L2*cos(q2_1) - L1*sin(q1_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1)) * vx) + ((L1*cos(q1_1) + L2*sin(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vy) * ts + q2_enc;
+        }
+
+int main()
+{
+controller.attach_us(&PD, 10000);
+   
+while(true)
+{
+}
+
+    
+}
\ No newline at end of file