Start van regelaar
Dependencies: Encoder MODSERIAL mbed
Dependents: K9motoraansturing_copy lichtpoortjes
main.cpp@4:2ad0cc818166, 2013-10-09 (annotated)
- Committer:
- vsluiter
- Date:
- Wed Oct 09 10:27:29 2013 +0000
- Revision:
- 4:2ad0cc818166
- Parent:
- 3:c9c1c0ebf89d
added heaps of comments
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:7bc93f851767 | 1 | #include "mbed.h" |
vsluiter | 0:7bc93f851767 | 2 | #include "encoder.h" |
vsluiter | 0:7bc93f851767 | 3 | #include "MODSERIAL.h" |
vsluiter | 0:7bc93f851767 | 4 | |
vsluiter | 3:c9c1c0ebf89d | 5 | /******************************************************************************* |
vsluiter | 3:c9c1c0ebf89d | 6 | * * |
vsluiter | 3:c9c1c0ebf89d | 7 | * Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/ * |
vsluiter | 3:c9c1c0ebf89d | 8 | * * |
vsluiter | 3:c9c1c0ebf89d | 9 | ********************************************************************************/ |
vsluiter | 3:c9c1c0ebf89d | 10 | |
vsluiter | 1:9d05c0236c7e | 11 | /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ |
vsluiter | 1:9d05c0236c7e | 12 | void keep_in_range(float * in, float min, float max); |
vsluiter | 1:9d05c0236c7e | 13 | |
vsluiter | 4:2ad0cc818166 | 14 | /** variable to show when a new loop can be started*/ |
vsluiter | 4:2ad0cc818166 | 15 | /** volatile means that it can be changed in an */ |
vsluiter | 4:2ad0cc818166 | 16 | /** interrupt routine, and that that change is vis-*/ |
vsluiter | 4:2ad0cc818166 | 17 | /** ible in the main loop. */ |
vsluiter | 4:2ad0cc818166 | 18 | |
vsluiter | 0:7bc93f851767 | 19 | volatile bool looptimerflag; |
vsluiter | 0:7bc93f851767 | 20 | |
vsluiter | 4:2ad0cc818166 | 21 | /** function called by Ticker "looptimer" */ |
vsluiter | 4:2ad0cc818166 | 22 | /** variable 'looptimerflag' is set to 'true' */ |
vsluiter | 4:2ad0cc818166 | 23 | /** each time the looptimer expires. */ |
vsluiter | 0:7bc93f851767 | 24 | void setlooptimerflag(void) |
vsluiter | 0:7bc93f851767 | 25 | { |
vsluiter | 0:7bc93f851767 | 26 | looptimerflag = true; |
vsluiter | 0:7bc93f851767 | 27 | } |
vsluiter | 0:7bc93f851767 | 28 | |
vsluiter | 0:7bc93f851767 | 29 | |
vsluiter | 0:7bc93f851767 | 30 | int main() { |
vsluiter | 1:9d05c0236c7e | 31 | //LOCAL VARIABLES |
vsluiter | 4:2ad0cc818166 | 32 | /*Potmeter input*/ |
vsluiter | 1:9d05c0236c7e | 33 | AnalogIn potmeter(PTC2); |
vsluiter | 4:2ad0cc818166 | 34 | /* Encoder, using my encoder library */ |
vsluiter | 4:2ad0cc818166 | 35 | /* First pin should be PTDx or PTAx */ |
vsluiter | 4:2ad0cc818166 | 36 | /* because those pins can be used as */ |
vsluiter | 4:2ad0cc818166 | 37 | /* InterruptIn */ |
vsluiter | 4:2ad0cc818166 | 38 | Encoder motor1(PTD0,PTC9); |
vsluiter | 4:2ad0cc818166 | 39 | /* MODSERIAL to get non-blocking Serial*/ |
vsluiter | 0:7bc93f851767 | 40 | MODSERIAL pc(USBTX,USBRX); |
vsluiter | 4:2ad0cc818166 | 41 | /* PWM control to motor */ |
vsluiter | 0:7bc93f851767 | 42 | PwmOut pwm_motor(PTA12); |
vsluiter | 4:2ad0cc818166 | 43 | /* Direction pin */ |
vsluiter | 0:7bc93f851767 | 44 | DigitalOut motordir(PTD3); |
vsluiter | 4:2ad0cc818166 | 45 | /* variable to store setpoint in */ |
vsluiter | 0:7bc93f851767 | 46 | float setpoint; |
vsluiter | 4:2ad0cc818166 | 47 | /* variable to store pwm value in*/ |
vsluiter | 1:9d05c0236c7e | 48 | float pwm_to_motor; |
vsluiter | 1:9d05c0236c7e | 49 | //START OF CODE |
vsluiter | 4:2ad0cc818166 | 50 | |
vsluiter | 4:2ad0cc818166 | 51 | /*Set the baudrate (use this number in RealTerm too! */ |
vsluiter | 1:9d05c0236c7e | 52 | pc.baud(230400); |
vsluiter | 4:2ad0cc818166 | 53 | |
vsluiter | 4:2ad0cc818166 | 54 | /*Create a ticker, and let it call the */ |
vsluiter | 4:2ad0cc818166 | 55 | /*function 'setlooptimerflag' every 0.01s */ |
vsluiter | 0:7bc93f851767 | 56 | Ticker looptimer; |
vsluiter | 0:7bc93f851767 | 57 | looptimer.attach(setlooptimerflag,0.01); |
vsluiter | 4:2ad0cc818166 | 58 | /* Oops... I left some test code in my program */ |
vsluiter | 1:9d05c0236c7e | 59 | pc.printf("bla"); |
vsluiter | 4:2ad0cc818166 | 60 | |
vsluiter | 1:9d05c0236c7e | 61 | //INFINITE LOOP |
vsluiter | 0:7bc93f851767 | 62 | while(1) { |
vsluiter | 4:2ad0cc818166 | 63 | /* Wait until looptimer flag is true. */ |
vsluiter | 4:2ad0cc818166 | 64 | /* '!=' means not-is-equal */ |
vsluiter | 1:9d05c0236c7e | 65 | while(looptimerflag != true); |
vsluiter | 4:2ad0cc818166 | 66 | /* Clear the looptimerflag, otherwise */ |
vsluiter | 4:2ad0cc818166 | 67 | /* the program would simply continue */ |
vsluiter | 4:2ad0cc818166 | 68 | /* without waitingin the next iteration*/ |
vsluiter | 0:7bc93f851767 | 69 | looptimerflag = false; |
vsluiter | 4:2ad0cc818166 | 70 | |
vsluiter | 4:2ad0cc818166 | 71 | /* Read potmeter value, apply some math */ |
vsluiter | 4:2ad0cc818166 | 72 | /* to get useful setpoint value */ |
vsluiter | 1:9d05c0236c7e | 73 | setpoint = (potmeter.read()-0.5)*2000; |
vsluiter | 4:2ad0cc818166 | 74 | |
vsluiter | 4:2ad0cc818166 | 75 | /* Print setpoint and current position to serial terminal*/ |
vsluiter | 0:7bc93f851767 | 76 | pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition()); |
vsluiter | 4:2ad0cc818166 | 77 | |
vsluiter | 4:2ad0cc818166 | 78 | /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */ |
vsluiter | 1:9d05c0236c7e | 79 | pwm_to_motor = (setpoint - motor1.getPosition())*.001; |
vsluiter | 4:2ad0cc818166 | 80 | /* Coerce pwm value if outside range */ |
vsluiter | 4:2ad0cc818166 | 81 | /* Not strictly needed here, but useful */ |
vsluiter | 4:2ad0cc818166 | 82 | /* if doing other calculations with pwm value */ |
vsluiter | 1:9d05c0236c7e | 83 | keep_in_range(&pwm_to_motor, -1,1); |
vsluiter | 4:2ad0cc818166 | 84 | |
vsluiter | 4:2ad0cc818166 | 85 | /* Control the motor direction pin. based on */ |
vsluiter | 4:2ad0cc818166 | 86 | /* the sign of your pwm value. If your */ |
vsluiter | 4:2ad0cc818166 | 87 | /* motor keeps spinning when running this code */ |
vsluiter | 4:2ad0cc818166 | 88 | /* you probably need to swap the motor wires, */ |
vsluiter | 4:2ad0cc818166 | 89 | /* or swap the 'write(1)' and 'write(0)' below */ |
vsluiter | 1:9d05c0236c7e | 90 | if(pwm_to_motor > 0) |
vsluiter | 4:2ad0cc818166 | 91 | motordir.write(1); |
vsluiter | 0:7bc93f851767 | 92 | else |
vsluiter | 4:2ad0cc818166 | 93 | motordir.write(0); |
vsluiter | 4:2ad0cc818166 | 94 | //WRITE VALUE TO MOTOR |
vsluiter | 4:2ad0cc818166 | 95 | /* Take the absolute value of the PWM to send */ |
vsluiter | 4:2ad0cc818166 | 96 | /* to the motor. */ |
vsluiter | 1:9d05c0236c7e | 97 | pwm_motor.write(abs(pwm_to_motor)); |
vsluiter | 0:7bc93f851767 | 98 | } |
vsluiter | 0:7bc93f851767 | 99 | } |
vsluiter | 0:7bc93f851767 | 100 | |
vsluiter | 0:7bc93f851767 | 101 | |
vsluiter | 0:7bc93f851767 | 102 | //coerces value 'in' to min or max when exceeding those values |
vsluiter | 0:7bc93f851767 | 103 | //if you'd like to understand the statement below take a google for |
vsluiter | 0:7bc93f851767 | 104 | //'ternary operators'. |
vsluiter | 1:9d05c0236c7e | 105 | void keep_in_range(float * in, float min, float max) |
vsluiter | 0:7bc93f851767 | 106 | { |
vsluiter | 0:7bc93f851767 | 107 | *in > min ? *in < max? : *in = max: *in = min; |
vsluiter | 0:7bc93f851767 | 108 | } |
vsluiter | 0:7bc93f851767 | 109 | |
vsluiter | 0:7bc93f851767 | 110 | |
vsluiter | 0:7bc93f851767 | 111 |