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

Files at this revision

API Documentation at this revision

Revision 44:d157094b48d5, committed 2017-11-01

Comitter:
tvlogman
Date:
Wed Nov 01 16:17:21 2017 +0000
Parent:
43:dd0888f86357
Commit message:
potmeter controlled robot

Changed in this revision

inverseKinematics.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r dd0888f86357 -r d157094b48d5 inverseKinematics.lib
--- a/inverseKinematics.lib	Wed Nov 01 14:54:50 2017 +0000
+++ b/inverseKinematics.lib	Wed Nov 01 16:17:21 2017 +0000
@@ -1,1 +1,1 @@
-inverseKinematics#81e4001f1082
+https://os.mbed.com/users/tvlogman/code/inverseKinematics/#81e4001f1082

diff -r dd0888f86357 -r d157094b48d5 main.cpp
--- 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;