Diff: main.cpp

Revision:

7:9e0ded88fe60

Parent:

6:0162a633768d

Child:

8:fd00916552e0

--- a/main.cpp	Mon Oct 22 09:18:30 2018 +0000
+++ b/main.cpp	Mon Oct 22 10:00:09 2018 +0000
@@ -1,14 +1,18 @@
 #include "mbed.h"
 #include "math.h"
 #include "MODSERIAL.h"
+#include "HIDScope.h"
+#include "encoder.h"
 
 DigitalIn button(SW2);
 DigitalOut directionpin1(D7);
 DigitalOut directionpin2(D8);
 MODSERIAL pc(USBTX, USBRX);
+HIDScope scope(2);
+Encoder motor1(D13,D12);
+Encoder motor2(D13,D12); //has to be changed!!!
 
-Ticker ticker;
-
+//values of inverse kinematics
 volatile bool emg0Bool = false;
 volatile bool emg1Bool = false;
 volatile bool emg2Bool = false;
@@ -29,6 +33,27 @@
 const float length = 0.300; //length in m (placeholder)
 const float C1 = 3; //motor 1 gear ratio
 const float C2 = 3; //motor 2 gear ratio/radius of the circular gear (placeholder)
+
+//values of PID controller
+const float Kp = 2;
+const float Ki = 0.2;
+const float Kd = 0;
+const float Timestep = 0.001;
+float G ;               //desired motor angle
+float Output1 = 0 ;      //Starting value
+float Output2 = 0 ;      //Starting value
+float P = 0;           //encoder value
+float e1 = 0 ;          //Starting value 
+float e2 = 0 ;          //Starting value
+float e3;  
+float f1 = 0 ;          //Starting value 
+float f2 = 0 ;          //Starting value
+float f3;     
+float Output_Last1;      // Remember previous position
+float Output_Last2;      // Remember previous position
+float Y;                // Value that is outputted to motor control
+float P_Last = 0;       // Starting position
+const float Max_Speed = 400;      //Max speed of the motor
       
 void xDirection() {
     //direction of the motion
@@ -81,6 +106,50 @@
     }
 }
 
+void PIDController1() {   
+    P = motor1.getPosition() / 8400 * 360;        
+    e1 = e2;
+    e2 = e3;
+    e3 = G - P;
+    Output_Last1 = Output1;
+    Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
+    Y = Output1;
+     
+    if (Output1 >= 1){
+        Y = 1;
+    }
+    else if (Output1 <= -1){
+        Y = -1;
+    }    
+    P = P_Last + Y * Timestep * Max_Speed;
+
+    scope.set(0,Output1);
+    scope.set(1,P);
+    scope.send();
+}
+
+void PIDController2() {   
+    P = motor2.getPosition() / 8400 * 360;        
+    f1 = f2;
+    f2 = f3;
+    f3 = G - P;
+    Output_Last2 = Output2;
+    Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
+    Y = Output2;
+     
+    if (Output2 >= 1){
+        Y = 1;
+    }
+    else if (Output2 <= -1){
+        Y = -1;
+    }    
+    P = P_Last + Y * Timestep * Max_Speed;
+
+    scope.set(0,Output2);
+    scope.set(1,P);
+    scope.send();
+}
+
 int main() {
     pc.baud(115200);
     pc.printf(" ** program reset **\n\r");
@@ -108,8 +177,10 @@
                     break;
             }
         }
+        yDirection(); //call the function to move in the y direction
         xDirection(); //call the function to move in the x direction
-        yDirection(); //call the function to move in the y direction
+        PIDController1();
+        PIDController2();
         
         // print the motor angles and coordinates
         pc.printf("position: (%f, %f)\n\r", x_position, y_position);