Totale script, 2 rotationele joints met middelpunt van swiffer als end-effector die verticaal en horizontaal wordt bestuurd middels EMG-signalen. Automatische kalibratie, grenshoeken
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 2:6523e21391e5
- Parent:
- 1:078e96685ed3
diff -r 078e96685ed3 -r 6523e21391e5 main.cpp --- a/main.cpp Mon Oct 31 14:21:31 2016 +0000 +++ b/main.cpp Tue Nov 01 09:23:01 2016 +0000 @@ -63,10 +63,12 @@ volatile bool calibrate_switch = false; volatile bool calibration_finished = false; +const double pi = 3.14159265359; + volatile double q1 = 0; volatile double q2 = 0; const double l1 = 0.3626; -const double l2 = 0.420; +const double l2 = (0.420-0.065); // middelpunt swiffer volatile double q1_v; volatile double q2_v; volatile double q1_ref = 0; @@ -183,18 +185,22 @@ { dir_M1 = 0; //ccw dir_M2 = 1; //cw - while (q1 < 20*2*3.1415/360) { + while ( (q1 < 20*2*pi/360) || (q2 > -45*2*pi/360) ) { q1 = wheel_M1.getPulses()/(1334.355/2); - pwm_M1 = 0.05; + q2 = wheel_M2.getPulses()/(1334.355/2); + if (q1 < 20*2*pi/360) { + pwm_M1 = 0.05; + } else { + pwm_M1 = 0; + } + if (q2 > -45*2*pi/360) { + pwm_M2 = 0.06; + } else { + pwm_M2 = 0; + } wait(0.005f); } pwm_M1 = 0; - - while (q2 > -45*2*3.1415/360) { - q2 = wheel_M2.getPulses()/(1334.355/2); - pwm_M2 = 0.05; - wait(0.005f); - } pwm_M2 = 0; begin_hoeken(); } @@ -357,7 +363,7 @@ q1_v = J_1 * vx + J_2 * vy; q2_v = J_3 * vx + J_4 * vy; - if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden) + if ( (q1 > (135*2*pi/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden) q1_v = 0; q2_v = 0; q1_ref = q1; @@ -366,7 +372,7 @@ q2IntError = 0; q1_error_prev = 0; q2_error_prev = 0; - } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) { + } else if ( (q1 < -(135*2*pi/360)) && (q1_v < 0) ) { q1_v = 0; q2_v = 0; q1_ref = q1; @@ -375,7 +381,7 @@ q2IntError = 0; q1_error_prev = 0; q2_error_prev = 0; - } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) { // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452 + } else if ( (q2 < (-2.6*2*pi/360)) && (q2_v < 0) ) { // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452 q1_v = 0; q2_v = 0; q1_ref = q1; @@ -384,7 +390,7 @@ q2IntError = 0; q1_error_prev = 0; q2_error_prev = 0; - } else if ( (q2 > 0) && (q2_v > 0) ) { + } else if ( (q2 >= 0) && (q2_v > 0) ) { q1_v = 0; q2_v = 0; q1_ref = q1;