jkfodk
Dependencies: Encoder MODSERIAL mbed
Revision 7:1f88215b504c, committed 2013-10-30
- Comitter:
- Tess
- Date:
- Wed Oct 30 12:25:09 2013 +0000
- Parent:
- 6:2cfdda6721ab
- Child:
- 8:62e968f78878
- Commit message:
- new with only the calibration stuff
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 30 10:28:10 2013 +0000 +++ b/main.cpp Wed Oct 30 12:25:09 2013 +0000 @@ -72,19 +72,23 @@ /*Set the baudrate (use this number in RealTerm too!) */ pc.baud(921600); - // in dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen - /* motordirB.write(0); + // in dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen. + // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn). + + motordirB.write(0); pwm_motorB.write(.08); positionmotorB_t0 = motorB.getPosition(); do { wait(0.2); - positionmotorB_t_1 = positionmotorB_t0; + positionmotorB_t_1 = positionmotorB_t0 ; positionmotorB_t0 = motorB.getPosition(); positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1); } while(positiondifference_motorB > 10); motorB.setPosition(0); pwm_motorB.write(0); + wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. + motordirA.write(1); pwm_motorA.write(.08); positionmotorA_t0 = motorA.getPosition(); @@ -97,19 +101,20 @@ motorA.setPosition(0); pwm_motorA.write(0); - // Hierna willen we de motor van zijn aller uiterste 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. + 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. -*/ - Ticker looptimer; - looptimer.attach(setlooptimerflag,0.01); + motordirA.write(0); + pwm_motorA.write(.08); do { - pwm_to_begin_motorA = abs((setpoint_beginA - motorA.getPosition()) *.001); - } while(pwm_to_begin_motorA <= 0.001); { - while(looptimerflag != true); - looptimerflag = false; - setpoint_beginA = -1000; //-63 negatief omdat de motor op zijn kop hangt - //pwm_to_begin_motorA = abs((setpoint_beginA - motorA.getPosition()) *.001); + 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); keep_in_range(&pwm_to_begin_motorA, -1, 1 ); motordirA.write(0); pwm_motorA.write(pwm_to_begin_motorA); @@ -117,144 +122,54 @@ motorA.setPosition(0); pwm_motorA.write(0); - do{ - pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001); - } - while(pwm_to_begin_motorB <= 0.001); { - while(looptimerflag != true); - looptimerflag = false; - setpoint_beginB = 1000; //192 - keep_in_range(&pwm_to_begin_motorB, -1,1); + wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. + + motordirB.write(1); + pwm_motorB.write(.08); + 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); + keep_in_range(&pwm_to_begin_motorB, -1, 1 ); motordirB.write(1); pwm_motorB.write(pwm_to_begin_motorB); } motorB.setPosition(0); pwm_motorB.write(0); -// nu naar positie rechtsonderhoek A4 deze is voor motor A 532 en voor motor B 460 - -//while(0) ///1 -//{ -// while(looptimerflag != true); -// -// looptimerflag = false; + wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. -// setpoint_rechtsonderA = 531; // naar rechter onderhoek - // pwm_to_rechtsonder_motorA = setpoint_rechtsonderA *.001; - // keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 ); -// if(pwm_to_rechtsonder_motorA > 0.531) { -// motordirA.write(0); -// pwm_motorA.write(0); -// } else -// motordirA.write(1); -// pwm_motorA.write(abs(pwm_to_rechtsonder_motorA)); + // 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 -// while(looptimerflag != true); -// looptimerflag = false; - -// setpoint_rechtsonderB = 460; -// pwm_to_rechtsonder_motorB = setpoint_rechtsonderB *.001; -// keep_in_range(&pwm_to_rechtsonder_motorB, -1,1); -// if(pwm_to_rechtsonder_motorB > 0.460) { -// motordirB.write(0); -// pwm_motorB.write(0); -// } else - // motordirB.write(1); -// pwm_motorB.write(abs(pwm_to_rechtsonder_motorB)); - -//} - - while((pwm_to_begin_motorA >= 0)&&(pwm_to_begin_motorB >= 0)) { - while(looptimerflag != true); - looptimerflag = false; - - setpoint_beginA = 1000;//532 // naar rechteronderhoek A4 - pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001; + motordirB.write(1); + motordirA.write(0); + pwm_motorB.write(0.08); + pwm_motorA.write(0.08); + do { + setpoint_beginA = -1000; //-532 + pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); + setpoint_beginB = 1000; //460 + pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001); + } while((pwm_to_begin_motorA <= 0)&&(pwm_to_begin_motorB <= 0)); { + wait(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(abs(pwm_to_begin_motorA)); - - while(looptimerflag != true); - looptimerflag = false; - - setpoint_beginB = 1000; //460 - pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001; - keep_in_range(&pwm_to_begin_motorB, -1,1); - motordirB.write(1); - pwm_motorB.write(abs(pwm_to_begin_motorB)); + pwm_motorA.write(pwm_to_begin_motorA); } pwm_motorA.write(0); pwm_motorB.write(0); - - /*Create a ticker, and let it call the */ - /*function 'setlooptimerflag' every 0.01s */ - -//INFINITE LOOP - while(1) { - /* Wait until looptimer flag is true. */ - /* '!=' means not-is-equal */ - while(looptimerflag != true); - /* Clear the looptimerflag, otherwise */ - /* the program would simply continue */ - /* without waitingin the next iteration*/ - looptimerflag = false; - - - - - /* Read potmeter value, apply some math */ - /* to get useful setpoint value */ - setpointA = (potmeterA.read()-0.5)*(631/2); // bereik van 71 graden dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen - setpointB = (potmeterB.read()-0.5)*(415/2); // bereik van 46.7 graden 1000 dan rotatie langzamer maken als lager maakt. - - - // 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, 474, 1105); - keep_in_range(&setpointB, -147, 269); - - /* Print setpoint and current position to serial terminal*/ - pc.printf("s: %f, %d ", setpointA, motorA.getPosition()); - pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition()); - - /* 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; - /* Coerce pwm value if outside range */ - /* Not strictly needed here, but useful */ - /* if doing other calculations with pwm value */ - keep_in_range(&pwm_to_motorA, -1,1); - keep_in_range(&pwm_to_motorB, -1,1); - - /* Control the motor direction pin. based on */ - /* the sign of your pwm value. If your */ - /* motor keeps spinning when running this code */ - /* you probably need to swap the motor wires, */ - /* or swap the 'write(1)' and 'write(0)' below */ - if(pwm_to_motorA > 0) - motordirA.write(1); - else - motordirA.write(0); - if(pwm_to_motorB > 0) - motordirB.write(1); - else - motordirB.write(0); - - - //WRITE VALUE TO MOTOR - /* Take the absolute value of the PWM to send */ - /* to the motor. */ - pwm_motorA.write(abs(pwm_to_motorA)); - pwm_motorB.write(abs(pwm_to_motorB)); - } } -//coerces value 'in' to min or max when exceeding those values -//if you'd like to understand the statement below take a google for -//'ternary operators'. void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; @@ -262,3 +177,7 @@ + + + +