Diff: main.cpp

Revision:

18:dddc8d9f7638

Parent:

17:615c5d8b3710

Child:

19:a37cae6964ca

--- a/main.cpp	Fri Oct 11 09:51:35 2019 +0000
+++ b/main.cpp	Tue Oct 15 09:14:37 2019 +0000
@@ -1,3 +1,18 @@
+/*
+To-do:
+    Add reference generator
+    fully implement schmitt trigger 
+    EMG normalizing
+    Homing 
+    Turning the magnet on/off
+    Inverse kinematics
+    Gravity compensation
+    PID values
+    General program layout
+*/
+
+
+
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "FastPWM.h"
@@ -44,14 +59,22 @@
     float min;
 } EMG_values;
 
+struct PID {
+    float P;
+    float I;
+    float D;
+    float I_counter;
+} PID1 PID2;
+
+float dt = 0.001;
 int enc1_zero;  //the zero position of the encoders, to be determined from the
 int enc2_zero;  //encoder calibration
 int EMG1_filtered;
 int EMG2_filtered;
 int enc1_value;
 int enc2_value;
-
-//variables used throughout the program
+float error1 = 0.0;
+float error2 = 0.0;
 bool state_changed = false; //used to see if the state is "starting"
 volatile bool but1_pressed = false;
 volatile bool but2_pressed = false;
@@ -200,7 +223,28 @@
 }
 
 void motor_controller() {
+    float error1, error2;
+    //P part of the controller
+    float P_action1 = PID1.P * error1;
+    float P_action2 = PID2.P * error2;
     
+    //I part
+    PID1.I_counter += error1;
+    PID2.I_counter += error2;
+    float I_action1 = PID1.I_counter * PID1.I;
+    float I_action2 = PID2.I_counter * PID2.I;
+    
+    //D part
+    float velocity_estimate_1 = (error1-last_error1)/dt; //estimate of the time derivative of the error
+    float velocity_estimate_2 = (error2-last_error2)/dt;
+    float D_action1 = velocity_estimate_1 * PID1.D;
+    float D_action2 = velocity_estimate_2 * PID2.D;
+    
+    action1 = P_action1 + I_action1 + D_action1;
+    action2 = P_action2 + I_action2 + D_action2;
+        
+    last_error1 = error1;
+    last_error2 = error2;
 }
 
 void output()
@@ -302,7 +346,7 @@
 
     but1.fall(&but1_interrupt);
     but2.fall(&but2_interrupt);
-    loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
+    loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
     pc.printf("Main_loop is running\n\r");
     while (true) {
         wait(0.1f);