Tristan Vlogman / Mbed 2 deprecated locomotion_pid_action_refactor_EMG

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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 
--- 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

--- 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;