Tristan Vlogman / Mbed 2 deprecated locomotion_pid_action_refactor_EMG

Control up to two motors using filtered EMG signals and a PID controller

Dependencies:   FastPWM HIDScope MODSERIAL QEI Matrix biquadFilter controller errorFetch mbed motorConfig refGen MatrixMath inverseKinematics

Fork of Minor_test_serial by First Last

Diff: main.cpp

Revision:
44:d157094b48d5
Parent:
43:dd0888f86357
--- a/main.cpp	Wed Nov 01 14:54:50 2017 +0000
+++ b/main.cpp	Wed Nov 01 16:17:21 2017 +0000
@@ -133,7 +133,7 @@
     m2counts = Encoder2.getPulses();
     
     double m1position = e1.fetchMotorPosition(m1counts);
-    double m2position = e2.fetchMotorPosition(m2counts);
+    double m2position = e2.fetchMotorPosition(m2counts) - m1position;
     
     double pot1 = ref1.getReference();
     double pot2 = ref2.getReference();
@@ -156,6 +156,16 @@
                 ledB = 1;
 
                 sendDataToPc(pot1, pot2, m1counts, m2counts); // just send the EMG signal value to HIDscope
+                
+                double Mp1Xa = -L1*sin(m1position) - L2*sin(m1position+m2position);
+                double Mp2Xa = L1*cos(m1position) + L2*cos(m1position+m2position);
+                
+//                pc.printf("Mp1Xa is %.2f \r\n", Mp1Xa);
+//                pc.printf("Mp2Xa is %.2f \r\n", Mp2Xa);
+//                
+//                pc.printf("theta1 is %.2f \r\n", m1position);
+//                pc.printf("theta2 is %.2f \r\n", m2position);
+                
                 break;
                 }
                 
@@ -183,6 +193,12 @@
                 double Mp1N = Mp1C + Ts*q_dot1;
                 double Mp2N = Mp2C + Ts*q_dot2;
                 
+                double Mp1X = -L1*sin(Mp1N) - L2*sin(Mp1N+Mp2N);
+                double Mp2X = L1*cos(Mp1N) + L2*cos(Mp1N+Mp2N);
+                
+                double Mp1Xa = -L1*sin(m1position) - L2*sin(m1position+m2position);
+                double Mp2Xa = L1*cos(m1position) + L2*cos(m1position+m2position);
+                
                 // Compute error between actual CURRENT motor position and NEXT position setpoint
                 e1.fetchError(m1position, Mp1N);
                 e2.fetchError(m2position, Mp2N);
@@ -194,7 +210,14 @@
                 motor2.setMotor(motorValue2);
                 
                 // Send data to HIDscope
-                sendDataToPc(Mp1N, Mp2N, q_dot1, q_dot2);
+                //sendDataToPc(vx, vy, Mp1N, Mp2N);
+                
+//                pc.printf("Mp1X is %.2f \r\n", Mp1X);
+//                pc.printf("Mp2X is %.2f \r\n", Mp2X);
+//                pc.printf("Mp1Xa is %.2f \r\n", Mp1Xa);
+//                pc.printf("Mp2Xa is %.2f \r\n", Mp2Xa);
+//                pc.printf("vx is %.2f \r\n", vx);
+//                pc.printf("vy is %.2f \r\n", vy);
                 
                 // Prepare for next round
                 Mp1C = Mp1N;