Diff: main.cpp

Revision:

25:e1577c9e8c7e

Parent:

24:710d7d99b915

Child:

26:3456b03d5bce

--- a/main.cpp	Thu Oct 24 11:30:32 2019 +0000
+++ b/main.cpp	Thu Oct 24 13:24:53 2019 +0000
@@ -66,7 +66,7 @@
 } PID;
 
 float dt = 0.001;
-float theta[2];
+float theta[2]; //theta0 = rot, theta1 = lin
 int enc_zero[2] = {0, 0};//the zero position of the encoders, to be determined from the encoder calibration
 float EMG_filtered[2];
 int enc_value[2];
@@ -79,7 +79,10 @@
 volatile bool failure_occurred = false;
 bool EMG_has_been_calibrated;
 float filter_value[2];
+int speed[2];
 int past_speed[2] = {0, 0};
+float desired_velocity[2];
+float theta_desired[2];
 
 void do_nothing()
 
@@ -157,7 +160,7 @@
         state_changed = false;
     }
     if (button1_pressed) {
-        pc.printf("Encoder has been calibrated");
+        pc.printf("Encoder has been calibrated\r\n");
         enc_zero[0] = enc_value[0];
         enc_zero[1] = enc_value[1];
         button1_pressed = false;
@@ -197,7 +200,7 @@
 */
 {
     if (state_changed) {
-        pc.printf("Started homing");
+        pc.printf("Started homing\r\n");
         state_changed = false;
     }
     return;
@@ -229,18 +232,30 @@
         }
         enc_value[c] -= enc_zero[c];
         theta[c] = (float)(enc_value[c])/(float)(8400*2*PI);
-        
     }
+    theta[1] = theta[1]*(5.05*0.008*PI)+0.63;
+    
+    for(int c = 0; c<2; c++) {
+        speed[c] = schmitt_trigger(EMG_filtered[c]);
+        if (speed[c] == -1) {
+            speed[c] = past_speed[c];
+        }
+        past_speed[c] = speed[c];
+        if (speed[c] == 0){
+            desired_velocity[c] = 0;
+        }
+        if (speed[c] == 1){
+            desired_velocity[c] = 0.01;
+        }
+        if (speed[c] == 2){
+            desired_velocity[c] = 0.02;
+        }
+    }
+    theta_desired[0] = theta[0] + dt*(desired_velocity[1]*cos(theta[0])/theta[1] - desired_velocity[0]*sin(theta[0])/theta[1]);
+    theta_desired[1] = theta[1] + dt*(desired_velocity[0]*cos(theta[0]) - desired_velocity[1]*sin(theta[0]));
 }
 
 void motor_controller() {
-    int speed[2];
-    for(int c = 0; c<2; c++) {
-        speed[c] = schmitt_trigger(EMG_filtered[c]);
-        if (speed[c] == -1) {speed[c] = past_speed[c];}
-        past_speed[c] = speed[c];
-    }
-    
     float errors[2];
     float P_action[2];
     float I_action[2];
@@ -346,9 +361,9 @@
 {
     int speed;
     speed = -1; //default value, this means the state should not change
-    if (i >  0.000 && i < 0.125) {speed = 0;}
-    if (i >  0.250 && i < 0.375) {speed = 1;}
-    if (i >  0.500 && i < 1.000) {speed = 2;}   
+    if (i >  0.000f && i < 0.125f) {speed = 0;}
+    if (i >  0.250f && i < 0.375f) {speed = 1;}
+    if (i >  0.500f && i < 1.000f) {speed = 2;}   
     return speed;
 }