voor thomas
Dependencies: BMT-K9-Regelaar Encoder MODSERIAL mbed
Fork of BMT-K9-Regelaar by
main.cpp@5:19687a179088, 2013-10-25 (annotated)
- Committer:
- gerard1993
- Date:
- Fri Oct 25 11:13:01 2013 +0000
- Revision:
- 5:19687a179088
- Parent:
- 4:9ecf57487c72
Versie die nog niet werkt :(
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 | 1:9d05c0236c7e | 5 | /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ |
vsluiter | 1:9d05c0236c7e | 6 | void keep_in_range(float * in, float min, float max); |
vsluiter | 1:9d05c0236c7e | 7 | |
vsluiter | 0:7bc93f851767 | 8 | volatile bool looptimerflag; |
vsluiter | 0:7bc93f851767 | 9 | |
vsluiter | 0:7bc93f851767 | 10 | void setlooptimerflag(void) |
vsluiter | 0:7bc93f851767 | 11 | { |
vsluiter | 0:7bc93f851767 | 12 | looptimerflag = true; |
vsluiter | 0:7bc93f851767 | 13 | } |
vsluiter | 0:7bc93f851767 | 14 | |
vsluiter | 0:7bc93f851767 | 15 | |
vsluiter | 0:7bc93f851767 | 16 | int main() { |
vsluiter | 1:9d05c0236c7e | 17 | //LOCAL VARIABLES |
vsluiter | 1:9d05c0236c7e | 18 | AnalogIn potmeter(PTC2); |
vsluiter | 0:7bc93f851767 | 19 | MODSERIAL pc(USBTX,USBRX); |
gerard1993 | 4:9ecf57487c72 | 20 | |
gerard1993 | 4:9ecf57487c72 | 21 | Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx |
gerard1993 | 4:9ecf57487c72 | 22 | Encoder motor2(PTD5,PTC8);//first pin on PTAx or PTDx |
gerard1993 | 4:9ecf57487c72 | 23 | PwmOut pwm_motor1(PTA12); |
gerard1993 | 4:9ecf57487c72 | 24 | PwmOut pwm_motor2(PTA5); |
gerard1993 | 4:9ecf57487c72 | 25 | DigitalOut motordir1(PTD3); |
gerard1993 | 4:9ecf57487c72 | 26 | DigitalOut motordir2(PTD1); |
gerard1993 | 5:19687a179088 | 27 | |
gerard1993 | 5:19687a179088 | 28 | pwm_motor1.period(1.0/22000.0); |
gerard1993 | 5:19687a179088 | 29 | |
gerard1993 | 4:9ecf57487c72 | 30 | //MOTOR A |
vsluiter | 0:7bc93f851767 | 31 | float setpoint; |
gerard1993 | 4:9ecf57487c72 | 32 | float pwm_to_motor1; |
gerard1993 | 2:3d4a843be2a9 | 33 | float setspeed; |
gerard1993 | 5:19687a179088 | 34 | // float speed; |
gerard1993 | 5:19687a179088 | 35 | // float position2; |
gerard1993 | 5:19687a179088 | 36 | // float setpoint2; |
gerard1993 | 4:9ecf57487c72 | 37 | //MOTOR B |
gerard1993 | 4:9ecf57487c72 | 38 | float setpointB; |
gerard1993 | 4:9ecf57487c72 | 39 | float pwm_to_motor2; |
gerard1993 | 4:9ecf57487c72 | 40 | float setspeedB; |
gerard1993 | 4:9ecf57487c72 | 41 | float speedB; |
gerard1993 | 4:9ecf57487c72 | 42 | float position2B; |
gerard1993 | 4:9ecf57487c72 | 43 | float setpoint2B; |
gerard1993 | 4:9ecf57487c72 | 44 | |
vsluiter | 1:9d05c0236c7e | 45 | //START OF CODE |
gerard1993 | 5:19687a179088 | 46 | pc.baud(115200); |
vsluiter | 0:7bc93f851767 | 47 | Ticker looptimer; |
gerard1993 | 5:19687a179088 | 48 | looptimer.attach(setlooptimerflag,0.001); |
gerard1993 | 4:9ecf57487c72 | 49 | //A |
gerard1993 | 5:19687a179088 | 50 | // speed = 0; |
gerard1993 | 5:19687a179088 | 51 | // position2 = 0; |
gerard1993 | 5:19687a179088 | 52 | // setpoint = 0; |
gerard1993 | 4:9ecf57487c72 | 53 | //B |
gerard1993 | 4:9ecf57487c72 | 54 | speedB = 0; |
gerard1993 | 4:9ecf57487c72 | 55 | position2B = 0; |
gerard1993 | 4:9ecf57487c72 | 56 | setpoint2B = 0; |
vsluiter | 1:9d05c0236c7e | 57 | //INFINITE LOOP |
vsluiter | 0:7bc93f851767 | 58 | while(1) { |
vsluiter | 1:9d05c0236c7e | 59 | while(looptimerflag != true); |
vsluiter | 0:7bc93f851767 | 60 | looptimerflag = false; |
gerard1993 | 4:9ecf57487c72 | 61 | |
gerard1993 | 4:9ecf57487c72 | 62 | //MOTOR A |
gerard1993 | 5:19687a179088 | 63 | setspeed = (potmeter.read()-0.5)*.0001; |
gerard1993 | 5:19687a179088 | 64 | setpoint = setpoint + setspeed; |
gerard1993 | 5:19687a179088 | 65 | pwm_to_motor1 = (setpoint - (motor1.getPosition()/4128))*20 + (setspeed - (motor1.getSpeed()/4128))*1.4 ; |
gerard1993 | 4:9ecf57487c72 | 66 | keep_in_range(&pwm_to_motor1, -1,1); |
gerard1993 | 5:19687a179088 | 67 | |
gerard1993 | 4:9ecf57487c72 | 68 | if(pwm_to_motor1 > 0) |
gerard1993 | 4:9ecf57487c72 | 69 | motordir1 = 1; |
vsluiter | 0:7bc93f851767 | 70 | else |
gerard1993 | 4:9ecf57487c72 | 71 | motordir1 = 0; |
vsluiter | 1:9d05c0236c7e | 72 | //WRITE VALUE TO MOTOR |
gerard1993 | 4:9ecf57487c72 | 73 | pwm_motor1.write(abs(pwm_to_motor1)); |
gerard1993 | 4:9ecf57487c72 | 74 | |
gerard1993 | 4:9ecf57487c72 | 75 | //MOTOR B |
gerard1993 | 4:9ecf57487c72 | 76 | setpointB = (potmeter.read()-0.5)*8000; |
gerard1993 | 4:9ecf57487c72 | 77 | setspeedB =(setpointB - setpoint2B)/0.01; |
gerard1993 | 5:19687a179088 | 78 | speedB = (motor2.getPosition() - position2B)/0.001; |
gerard1993 | 4:9ecf57487c72 | 79 | pc.printf("s: %f, %d \n\r", setpointB, motor2.getPosition()); |
gerard1993 | 4:9ecf57487c72 | 80 | pwm_to_motor2 = (setpointB - motor2.getPosition())*.0001 + (setspeedB - speedB)*.00005 ; |
gerard1993 | 4:9ecf57487c72 | 81 | keep_in_range(&pwm_to_motor2, -1,1); |
gerard1993 | 4:9ecf57487c72 | 82 | setpoint2B = setpointB; |
gerard1993 | 4:9ecf57487c72 | 83 | position2B = motor2.getPosition(); |
gerard1993 | 4:9ecf57487c72 | 84 | if(pwm_to_motor2 > 0) |
gerard1993 | 4:9ecf57487c72 | 85 | motordir2 = 1; |
gerard1993 | 4:9ecf57487c72 | 86 | else |
gerard1993 | 4:9ecf57487c72 | 87 | motordir2 = 0; |
gerard1993 | 4:9ecf57487c72 | 88 | //WRITE VALUE TO MOTOR |
gerard1993 | 4:9ecf57487c72 | 89 | pwm_motor2.write(abs(pwm_to_motor2)); |
vsluiter | 0:7bc93f851767 | 90 | } |
vsluiter | 0:7bc93f851767 | 91 | } |
vsluiter | 0:7bc93f851767 | 92 | |
vsluiter | 0:7bc93f851767 | 93 | |
vsluiter | 0:7bc93f851767 | 94 | //coerces value 'in' to min or max when exceeding those values |
vsluiter | 0:7bc93f851767 | 95 | //if you'd like to understand the statement below take a google for |
vsluiter | 0:7bc93f851767 | 96 | //'ternary operators'. |
vsluiter | 1:9d05c0236c7e | 97 | void keep_in_range(float * in, float min, float max) |
vsluiter | 0:7bc93f851767 | 98 | { |
vsluiter | 0:7bc93f851767 | 99 | *in > min ? *in < max? : *in = max: *in = min; |
vsluiter | 0:7bc93f851767 | 100 | } |
vsluiter | 0:7bc93f851767 | 101 | |
vsluiter | 0:7bc93f851767 | 102 | |
vsluiter | 0:7bc93f851767 | 103 |