jkfodk
Dependencies: Encoder MODSERIAL mbed
Revision 0:f492ec86159e, committed 2013-10-25
- Comitter:
- Tess
- Date:
- Fri Oct 25 08:07:43 2013 +0000
- Child:
- 1:7cafc9042056
- Commit message:
- Heey, this is a file to make your two motors work together. Have fun
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Fri Oct 25 08:07:43 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/vsluiter/code/Encoder/#2dd7853c911a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Fri Oct 25 08:07:43 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#f42def64c4ee
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 25 08:07:43 2013 +0000 @@ -0,0 +1,147 @@ +#include "mbed.h" +#include "encoder.h" +#include "MODSERIAL.h" + +/******************************************************************************* +* * +* Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/ * +* * +********************************************************************************/ + +/** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ +void keep_in_range(float * in, float min, float max); + +/** variable to show when a new loop can be started*/ +/** volatile means that it can be changed in an */ +/** interrupt routine, and that that change is vis-*/ +/** ible in the main loop. */ + +volatile bool looptimerflag; + +/** function called by Ticker "looptimer" */ +/** variable 'looptimerflag' is set to 'true' */ +/** each time the looptimer expires. */ +void setlooptimerflag(void) +{ + looptimerflag = true; +} + +int main() +{ + //LOCAL VARIABLES + /*Potmeter input*/ + AnalogIn potmeterA(PTC2); + AnalogIn potmeterB(PTB2); + /* Encoder, using my encoder library */ + /* First pin should be PTDx or PTAx */ + /* because those pins can be used as */ + /* InterruptIn */ + Encoder motorA(PTD4,PTC8); + Encoder motorB(PTD0,PTD2); + /* MODSERIAL to get non-blocking Serial*/ + MODSERIAL pc(USBTX,USBRX); + /* PWM control to motor */ + PwmOut pwm_motorA(PTA12); + PwmOut pwm_motorB(PTA5); + /* Direction pin */ + DigitalOut motordirA(PTD3); + DigitalOut motordirB(PTD1); + /* variable to store setpoint in */ + float setpointA; + float setpointB; + /* variable to store pwm value in*/ + float pwm_to_motorA; + float pwm_to_motorB; + int32_t beginpositiemotorA_t0; + int32_t beginpositiemotorB_t0; + //START OF CODE + + /*Set the baudrate (use this number in RealTerm too! */ + pc.baud(921600); + + +// wil dat 20 seconden de motor rechtsom gaat draaien + motordirA.write(0); + motordirB.write(0); + pwm_motorA.write(.1); + pwm_motorB.write(.1); + + do + { + //wacht + //schuif t0 / t1 + lees encoder + + }while(posititie ongelijk); + + + + + + /*Create a ticker, and let it call the */ + /*function 'setlooptimerflag' every 0.01s */ + Ticker looptimer; + looptimer.attach(setlooptimerflag,0.01); + + //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); + setpointB = (potmeterB.read()-0.5)*(871/2); //1000 dan rotatie langzamer maken als lager maakt. + + /* 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; +} + + +