jkfodk
Dependencies: Encoder MODSERIAL mbed
Revision 8:62e968f78878, committed 2013-10-30
- Comitter:
- Tess
- Date:
- Wed Oct 30 13:16:12 2013 +0000
- Parent:
- 7:1f88215b504c
- Child:
- 9:c49363372755
- Commit message:
- The best version!! It works and now EMG is needed
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 30 12:25:09 2013 +0000 +++ b/main.cpp Wed Oct 30 13:16:12 2013 +0000 @@ -111,14 +111,11 @@ do { setpoint_beginA = -63; pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt. - pc.printf("s: %f", pwm_to_begin_motorA); - } while(pwm_to_begin_motorA <= 0); - { - wait(0.5); + 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); motorA.setPosition(0); pwm_motorA.write(0); @@ -129,14 +126,11 @@ do { setpoint_beginB = 192; pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001); - pc.printf("s: %f", pwm_to_begin_motorB); - } while(pwm_to_begin_motorB <= 0); - { - wait(0.5); + wait(0.2); keep_in_range(&pwm_to_begin_motorB, -1, 1 ); motordirB.write(1); pwm_motorB.write(pwm_to_begin_motorB); - } + } while(pwm_to_begin_motorB <= 0); motorB.setPosition(0); pwm_motorB.write(0); @@ -150,23 +144,64 @@ pwm_motorB.write(0.08); pwm_motorA.write(0.08); do { - setpoint_beginA = -1000; //-532 + setpoint_beginA = -532; pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); - setpoint_beginB = 1000; //460 + setpoint_beginB = 460; pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001); - } while((pwm_to_begin_motorA <= 0)&&(pwm_to_begin_motorB <= 0)); { - wait(2); + 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); pwm_motorB.write(0); + // 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; + looptimer.attach(setlooptimerflag,0.01); +//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()); + + // 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; + + keep_in_range(&pwm_to_motorA, -1,1); + keep_in_range(&pwm_to_motorB, -1,1); + + if(pwm_to_motorA > 0) + motordirA.write(1); + else + motordirA.write(0); + if(pwm_to_motorB > 0) + motordirB.write(1); + else + motordirB.write(0); + + pwm_motorA.write(abs(pwm_to_motorA)); + pwm_motorB.write(abs(pwm_to_motorB)); + } }