jkfodk
Dependencies: Encoder MODSERIAL mbed
Revision 9:c49363372755, committed 2013-10-31
- Comitter:
- Tess
- Date:
- Thu Oct 31 12:48:12 2013 +0000
- Parent:
- 8:62e968f78878
- Child:
- 10:9d7110f25908
- Commit message:
- Port PTC2 not working
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 30 13:16:12 2013 +0000 +++ b/main.cpp Thu Oct 31 12:48:12 2013 +0000 @@ -53,6 +53,7 @@ float setpoint_beginB; float setpoint_rechtsonderA; float setpoint_rechtsonderB; + /* variable to store pwm value in*/ float pwm_to_motorA; float pwm_to_begin_motorA = 0; @@ -60,6 +61,30 @@ float pwm_to_motorB; float pwm_to_rechtsonder_motorA; float pwm_to_rechtsonder_motorB; + + const float dt = 0.002; + float Kp = 0.001; //0.0113 + float Ki = 0.0759; + float Kd = 0.0004342; + float error_t0_A = 0; + float error_t0_B = 0; + float error_ti_A; + float error_ti_B; + float error_t_1_A; + float error_t_1_B; + float P_regelaar_A; + float P_regelaar_B; + float I_regelaar_A; + float I_regelaar_B; + float D_regelaar_A; + float D_regelaar_B; + float output_regelaar_A; + float output_regelaar_B; + float integral_i_A; + float integral_i_B; + float integral_0_A = 0; + float integral_0_B = 0; + int32_t positionmotorA_t0; int32_t positionmotorB_t0; int32_t positionmotorA_t_1; @@ -104,12 +129,11 @@ wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links. - // Hierna moet motor B 21.6 (192) graden naar links. Dus eerst motor A en dan motor B. - + motordirA.write(0); pwm_motorA.write(.08); do { - setpoint_beginA = -63; + setpoint_beginA = -63; // x-as pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt. wait(0.2); keep_in_range(&pwm_to_begin_motorA, -1, 1 ); @@ -121,10 +145,28 @@ 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. + + motordirA.write(0); + pwm_motorA.write(0.08); + do { + setpoint_beginA = -532; // rechtsonder positie A4 + pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); + wait(0.2); + keep_in_range(&pwm_to_begin_motorA, -1, 1 ); + motordirA.write(0); + pwm_motorA.write(pwm_to_begin_motorA); + } while(pwm_to_begin_motorA <= 0); + pwm_motorA.write(0); + + wait(1); + + // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan. + motordirB.write(1); pwm_motorB.write(.08); do { - setpoint_beginB = 192; + setpoint_beginB = 192; // x-as pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001); wait(0.2); keep_in_range(&pwm_to_begin_motorB, -1, 1 ); @@ -135,33 +177,26 @@ pwm_motorB.write(0); wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. - - // Hierna willen we de motor van zijn x-as naar de rechtsonder positie van A4 te krijgen. Volgorde van motoren maakt nu niet uit. - // nu naar positie rechtsonderhoek A4 deze is voor motor A 532 en voor motor B 460 - + + // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460. + motordirB.write(1); - motordirA.write(0); pwm_motorB.write(0.08); - pwm_motorA.write(0.08); do { - setpoint_beginA = -532; - pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); - setpoint_beginB = 460; + setpoint_beginB = 460; // rechtsonder positie A4 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001); wait(0.2); keep_in_range(&pwm_to_begin_motorB, -1, 1 ); motordirB.write(1); pwm_motorB.write(pwm_to_begin_motorB); - keep_in_range(&pwm_to_begin_motorA, -1, 1 ); - motordirA.write(0); - pwm_motorA.write(pwm_to_begin_motorA); - } while((pwm_to_begin_motorA <= 0)&&(pwm_to_begin_motorB <= 0)); - pwm_motorA.write(0); + } while(pwm_to_begin_motorB <= 0); pwm_motorB.write(0); - + + wait(1); + // Nu zijn de motoren gekalibreed en staan ze op de startpositie. // Hierna het script dat EMG wordt omgezet in een positie verandering - + /*Create a ticker, and let it call the */ /*function 'setlooptimerflag' every 0.01s */ Ticker looptimer; @@ -169,24 +204,48 @@ //INFINITE LOOP while(1) { - + while(looptimerflag != true); looptimerflag = false; // hier EMG - setpointA = (potmeterA.read()-0.09027)*(631); // bereik van 71 graden dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen bij EMG wordt 0.5 - 0.09027 - setpointB = (potmeterB.read())*(415); // bereik van 46.7 graden - pc.printf("s: %f, %d ", setpointA, motorA.getPosition()); - + //setpointA = (potmeterA.read()-0.09027)*(631); // bereik van 71 graden dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen bij EMG wordt 0.5 - 0.09027 + //setpointB = (potmeterB.read())*(415); // bereik van 46.7 graden + //pc.printf("s: %f, %d ", setpointA, motorA.getPosition()); + //pc.printf("s: %f, %d ", setpointB, motorB.getPosition()); + setpointA = (potmeterA.read() - 0.5)*(631/2); + setpointB = (potmeterB.read() - 0.5) * (871/2); // 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 - /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */ - pwm_to_motorA = (setpointA - motorA.getPosition())*.001; - pwm_to_motorB = (setpointB - motorB.getPosition())*.001; - + // PID regelaar voor motor A + //wait(dt); + //error_ti_A = setpointA - motorA.getPosition(); + //P_regelaar_A = Kp * error_ti_A; + //D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt); + //integral_i_A = integral_0_A + (error_ti_A * dt); + //I_regelaar_A = Ki * integral_i_A; + //integral_0_A = integral_i_A; + //error_t0_A = error_ti_A; + //output_regelaar_A = P_regelaar_A; + + // PID regelaar voor motor B + //wait(dt); + //error_ti_B = setpointB - motorB.getPosition(); + //P_regelaar_B = Kp * error_ti_B; + //D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt); + //integral_i_B = integral_0_B + (error_ti_B * dt); + //I_regelaar_B = Ki * integral_i_B; + //integral_0_B = integral_i_B; + //error_t0_B = error_ti_B; + //output_regelaar_B = P_regelaar_B; + + /* This is a PID-action! store in pwm_to_motor */ + pwm_to_motorA = (setpointA - motorA.getPosition())*.001; //output_regelaar_A; + pwm_to_motorB = (setpointB - motorB.getPosition())*.001; //output_regelaar_B; + keep_in_range(&pwm_to_motorA, -1,1); keep_in_range(&pwm_to_motorB, -1,1);