Version 1: coming together. With PD controller but without inverse kinematics
Dependencies: Encoder MODSERIAL mbed
Fork of motoraansturing_met_EMG by
Revision 2:55bff07c1058, committed 2013-11-02
- Comitter:
- Tess
- Date:
- Sat Nov 02 12:12:42 2013 +0000
- Parent:
- 1:1c22ce9f370b
- Commit message:
- Version final; everything is in here.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 1c22ce9f370b -r 55bff07c1058 main.cpp --- 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;