Diff: main.cpp

Revision:

28:f08665a5ef6c

Parent:

27:4fa916e1f6a9

Child:

29:dd2450e6eb57

--- a/main.cpp	Mon Oct 28 09:59:23 2019 +0000
+++ b/main.cpp	Mon Oct 28 16:01:30 2019 +0000
@@ -1,10 +1,12 @@
-
 /*
 To-do:
+    calibration with reverse kinematics
+    EMG calibration upper bound
+    
     Homing 
     Turning the magnet on/off (reading magnet status?)
     Gravity compensation (if necessary)
-    PID values
+    PID values (too large)
     Boundaries (after verification of the PID values)
 */
 #include "mbed.h"
@@ -17,18 +19,18 @@
 
 Serial pc(USBTX, USBRX);            //connect to pc
 HIDScope scope(4);                  //HIDScope instance
-DigitalOut motor0_direction(D4);    //rotation motor 1 on shield (always D6)
-FastPWM motor0_pwm(D5);             //pwm 1 on shield (always D7)
-DigitalOut motor1_direction(D7);    //rotation motor 2 on shield (always D4)
-FastPWM motor1_pwm(D6);             //pwm 2 on shield (always D5)
+DigitalOut motor0_direction(D7);    //rotation motor 1 on shield (always D6)
+FastPWM motor0_pwm(D6);             //pwm 1 on shield (always D7)
+DigitalOut motor1_direction(D4);    //rotation motor 2 on shield (always D4)
+FastPWM motor1_pwm(D5);             //pwm 2 on shield (always D5)
 Ticker loop_ticker;                 //used in main()
 Ticker scope_ticker;
 InterruptIn but1(SW2);              //button on mbed board
 InterruptIn but2(D9);               //debounced button on biorobotics shield
-AnalogIn EMG1_sig(A0);
-AnalogIn EMG1_ref(A1);
-AnalogIn EMG2_sig(A2);
-AnalogIn EMG2_ref(A3);
+AnalogIn EMG0_sig(A0);
+AnalogIn EMG0_ref(A1);
+AnalogIn EMG1_sig(A2);
+AnalogIn EMG1_ref(A3);
 
 void check_failure();
 int schmitt_trigger(float i);
@@ -88,7 +90,6 @@
 int past_EMG_count = 0;
 
 void do_nothing()
-
 /*
     Idle state. Used in the beginning, before the calibration states.
 */
@@ -171,15 +172,14 @@
     if (button1_pressed) {
         pc.printf("Encoder has been calibrated\r\n");
         enc_zero[0] = enc_value[0];
-        enc_zero[1] = enc_value[1];
+        enc_zero[1] = enc_value[1];//+130990; //the magic number!
         button1_pressed = false;
         state = s_moving_magnet_off;
         state_changed = true;
     }
-    theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
-    theta_desired[1] = theta[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
-    errors[0] = theta_desired[0] - theta[0];
-    errors[1] = theta_desired[1] - theta[0];
+    errors[0] = 1.0e-5*speed[0];
+    errors[1] = 1.0e-5*speed[1];
+    
 }
 
 void moving_magnet_off()
@@ -195,9 +195,11 @@
     theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
     theta_desired[1] = theta[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
     errors[0] = theta_desired[0] - theta[0];
-    errors[1] = theta_desired[1] - theta[0];
+    errors[1] = theta_desired[1] - theta[1];
     if (button2_pressed) {
-        pc.printf("positions: (rad, m): %f %f\r\nErrors:    %f %f\r\n", theta[0], theta[1], errors[0], errors[1]);
+        pc.printf("positions: (rad, m): %f %f\r\n"
+                  "Errors:              %f %f\r\n"
+                  "Previous actions:    %f %f\r\n", theta[0], theta[1], errors[0], errors[1], actuator.duty_cycle[0], actuator.duty_cycle[1]);
         button2_pressed = false;
     }
 }
@@ -235,10 +237,10 @@
 void measure_signals()
 {
     //only one emg input, reference and plus
-    EMG_raw[0][0] = EMG1_sig.read();
-    EMG_raw[0][1] = EMG1_ref.read();
-    EMG_raw[1][0] = EMG2_sig.read();
-    EMG_raw[1][1] = EMG2_ref.read();
+    EMG_raw[0][0] = EMG0_sig.read();
+    EMG_raw[0][1] = EMG0_ref.read();
+    EMG_raw[1][0] = EMG1_sig.read();
+    EMG_raw[1][1] = EMG1_ref.read();
     filter_value[0] = fabs(bq1_2.step(fabs(bq1_1.step(EMG_raw[0][0] - EMG_raw[0][1]))));
     filter_value[1] = fabs(bq2_2.step(fabs(bq2_1.step(EMG_raw[1][0] - EMG_raw[1][1]))));
     
@@ -267,13 +269,13 @@
         }
         past_speed[c] = speed[c];
         if (speed[c] == 0){
-            velocity_desired[c] = 0;
+            velocity_desired[c] = 0.00f;
         }
         if (speed[c] == 1){
-            velocity_desired[c] = 0.01;
+            velocity_desired[c] = 0.01f;
         }
         if (speed[c] == 2){
-            velocity_desired[c] = 0.02;
+            velocity_desired[c] = 0.02f;
         }
     }
 }
@@ -301,8 +303,8 @@
         
         actuator.direction[c] = (action[c] > 0); //1 if action is positive, 0 otherwise
         actuator.duty_cycle[c] = fabs(action[c]);
-        if (actuator.duty_cycle[c] > 1.0) {actuator.duty_cycle[c] = 1.0;}
-        if (actuator.duty_cycle[c] < 0.0) {actuator.duty_cycle[c] = 0.0;}
+        if (actuator.duty_cycle[c] > 1.0f) {actuator.duty_cycle[c] = 1.0f;}
+        if (actuator.duty_cycle[c] < 0.0f) {actuator.duty_cycle[c] = 0.0f;}
     }
 }
 
@@ -313,10 +315,9 @@
     motor0_pwm.write(actuator.duty_cycle[0]);
     motor1_pwm.write(actuator.duty_cycle[1]);
     
-    //scope.set(0, EMG_filtered[0]);
-    //scope.set(1, past_speed[0]);    
-    //scope.set(2, EMG_filtered[1]);
-    //scope.set(3, past_speed[1]);
+    scope.set(0, EMG_filtered[0]);
+    scope.set(1, speed[0]);
+    scope.set(2, actuator.duty_cycle[0]);
 }
 
 void state_machine()
@@ -400,8 +401,8 @@
     pc.printf("Executing main()... \r\n");
     state = s_idle;
 
-    motor0_pwm.period(1/160000);   // 1/frequency van waarop hij draait
-    motor1_pwm.period(1/160000);   // 1/frequency van waarop hij draait
+    motor0_pwm.period(1.0f/160000.0f);   // 1/frequency van waarop hij draait
+    motor1_pwm.period(1.0f/160000.0f);   // 1/frequency van waarop hij draait
 
     actuator.direction[0] = 0;
     actuator.direction[1] = 0;
@@ -409,8 +410,8 @@
     actuator.default_direction[0] = -1;
     actuator.default_direction[1] = 1;
     
-    PID.P[0] = 50.0;
-    PID.P[1] = 300.0;
+    PID.P[0] = 1.0;
+    PID.P[1] = 1.0;
     PID.I[0] = 0.0;
     PID.I[1] = 0.0;
     PID.D[0] = 0.0;
@@ -424,7 +425,7 @@
     
     but1.fall(&but1_interrupt);
     but2.fall(&but2_interrupt);
-    //scope_ticker.attach(&scope, &HIDScope::send, 0.02);
+    scope_ticker.attach(&scope, &HIDScope::send, 0.05);
     loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
     pc.printf("Main_loop is running\n\r");
     while (true) {