Diff: main.cpp

Revision:

2:55bff07c1058

Parent:

1:1c22ce9f370b

--- a/main.cpp	Fri Nov 01 10:46:19 2013 +0000
+++ b/main.cpp	Sat Nov 02 12:12:42 2013 +0000
@@ -307,7 +307,7 @@
     PwmOut pwm_motorB(PTA5);        // PWM control to motor
     DigitalOut motordirA(PTD3);     // Direction pin
     DigitalOut motordirB(PTD1);     // Direction pin
-    
+
     /* variable to store setpoint in */
     float setpointA;
     float setpointB;
@@ -323,7 +323,7 @@
     float pwm_to_motorB;
     float pwm_to_rechtsonder_motorA;
     float pwm_to_rechtsonder_motorB;
-    
+
     /* variable for PD controller*/
     const float dt = 0.002;
     float Kp = 0.001;               //0.0208
@@ -347,6 +347,32 @@
     int32_t positiondifference_motorA;
     int32_t positiondifference_motorB;
 
+    /* inverse kinematica */
+    float dy;       //dy waarde tussen -1 en 1     -1 -vmax; 1 vmax
+    float dx;       //dx waarde tussen -1 en 1     -1 -vmax; 1 vmax
+    const float vmax = 2;          // m/s
+    const float delta_t = 0.005;  // 1/samplefrequentie, dus tijd tussen twee meetpunten
+    float X_positie;
+    float Y_positie;
+    float X_positie_begin;
+    float Y_positie_begin;
+    float puls_motorA;
+    float puls_motorB;
+    float kwadraat_X_positie;
+    float kwadraat_Y_positie;
+    float phi_A_pulsen_positie_begin;
+    float phi_B_pulsen_positie_begin;
+    float phi_A_positie_begin;
+    float phi_B_positie_begin;
+    float phi_1;
+    float lengte_arm = 276;     // in mm anders rare imaginaire getallen
+    float phi_A;
+    float phi_B;
+    float Puls_motorA;
+    float Puls_motorB;
+    float phi_A_pulsen;
+    float phi_B_pulsen;
+
     //START OF CODE
 
     pc.baud(115200);                // Set the baudrate (use this number in RealTerm too!)
@@ -399,7 +425,7 @@
 
     wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
 
-    // hierna moet motor A naar de rechtsonder A4. Motor A 532.
+    // hierna moet motor A naar de rechtsonder A4. Motor A 532 (hoek 59.8 graden).
 
     motordirA.write(0);
     pwm_motorA.write(0.08);
@@ -432,12 +458,12 @@
 
     wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
 
-    // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
+    // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 268 (30.2 graden).
 
     motordirB.write(1);
     pwm_motorB.write(0.08);
     do {
-        setpoint_rechtsonderB = 460;      // rechtsonder positie A4
+        setpoint_rechtsonderB = 268;      // rechtsonder positie A4
         pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001);
         wait(0.2);
         keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 );
@@ -467,11 +493,12 @@
         float emg_value_flexoren;
         float emg_value_extensoren;
         float dy;                                               // verschil tussen biceps en triceps, daarmee snelheid en richting aangeven.
-        
+        float dx;
+
         emg_value_biceps = ((100*(filter(1))-0.18)/0.49);       // dit is om waarde tussen 0 en 1 te krijgen. filter(1) zegt biceps, 0.18 offset aftrekken, 0.49 maximale waarde, 100 omdat procent
         emg_value_triceps = ((100*(filter(2))-0.18)/0.35);      // 0.35 maximale waarde van triceps
-        //emg_value_flexoren = 100*filter(3);
-        //emg_value_extensoren = 100*filter(4);
+        //emg_value_flexoren = ((100*filter(3))-0.00000)/0.000);
+        //emg_value_extensoren = ((100*filter(4))-0.00000)/0.0000);
 
         if(emg_value_biceps < 0.10) {                           // lager dan 10% doe dan niks - threshold
             emg_value_biceps=0;
@@ -483,25 +510,57 @@
         } else {
             emg_value_triceps=emg_value_triceps;
         }
+        /*if(emg_value_flexoren < 0.10){
+            emg_value_flexoren = 0;
+            } else {
+            emg_value_flexoren = emg_value_flexoren;
+            }
+            if(emg_value_extensoren<0.20){
+            emg_value_extensoren = 0;
+            } else {
+            emg_value_extensoren = emg_value_extensoren;
+            }*/
 
         dy = emg_value_biceps - emg_value_triceps;
-        dy = dy * 10;                                           // Waarde om beter te zien, officieel zonder *10 -> straks veranderen
+        // dx =  emg_value_flexoren - emg_value_extensoren;
         if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
             pc.printf("%.6f\n",dy);
 
-        setpointB = (dy) * 415;                                   // 451 staat voor een verdraaiing van 46.7 graden. Dit wil men met de uiteindelijke robot
-        setpointA = (dy) * 631;                                   // 631 staat voor een verdraaiing van 71 graden. Dit wil men met de uiteindelijke robot.
-        // Zelf vraag ik mij af of de verdraaiing wordt genomen vanaf nul of niet.
-                
-        /* motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
+        // inverse kinematica
+        phi_A_pulsen_positie_begin = motorA.getPosition();
+        phi_B_pulsen_positie_begin = motorB.getPosition();
+        
+        phi_A_positie_begin = (360/3200) * phi_A_pulsen_positie_begin;
+        phi_B_positie_begin = (360/3200) * phi_B_pulsen_positie_begin;
+        
+        phi_1 = phi_A_positie_begin - phi_B_positie_begin;
+       
+        X_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * cos(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin);
+        Y_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * sin(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin);
+               
+        X_positie = dx * vmax * delta_t + X_positie_begin;
+        Y_positie = dy * vmax * delta_t + Y_positie_begin;
+
+        kwadraat_X_positie = pow(X_positie,2);
+        kwadraat_Y_positie = pow(Y_positie,2);
+
+        phi_A = 180 - acos(sqrt(kwadraat_X_positie+kwadraat_Y_positie)/(2*lengte_arm)) - atan(Y_positie/X_positie);
+        phi_B = 180 - phi_A - acos(-(kwadraat_X_positie + kwadraat_Y_positie) / (2 * pow(lengte_arm,2))+1);
+        
+        phi_A_pulsen = (3200/360) * phi_A;
+        phi_B_pulsen = (3200/360) * phi_B;
+        
+        // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
         // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
-        keep_in_range(&setpointA, -1105, -474);     // voor motor moet bereik zijn -1105 tot -474
-        keep_in_range(&setpointB, -147, 269);       // voor motor moet bereik zijn -147 tot 269 */
-        // Dit moeten we testen!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+        keep_in_range(&phi_A_pulsen, -1104, -474);     // voor motor moet bereik zijn -1104 tot -474
+        keep_in_range(&phi_B_pulsen, -146, 268);       // voor motor moet bereik zijn -146 tot 268
+        
+        Puls_motorA = phi_A_pulsen - phi_A_pulsen_positie_begin;
+        Puls_motorB = phi_B_pulsen - phi_B_pulsen_positie_begin;
 
         //PD regelaar voor motor A
         wait(dt);
-        error_ti_A = setpointA - motorA.getPosition();
+        error_ti_A = puls_motorA - motorA.getPosition();
         P_regelaar_A = Kp * error_ti_A;
         D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
         error_t0_A = error_ti_A;
@@ -509,7 +568,7 @@
 
         //PD regelaar voor motor B
         wait(dt);
-        error_ti_B = setpointB - motorB.getPosition();
+        error_ti_B = puls_motorB - motorB.getPosition();
         P_regelaar_B = Kp * error_ti_B;
         D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
         error_t0_B = error_ti_B;