mit
main.cpp@5:e90c8b57811c, 2018-12-24 (annotated)
- Committer:
- coldplay
- Date:
- Mon Dec 24 03:47:49 2018 +0000
- Revision:
- 5:e90c8b57811c
- Parent:
- 4:5ae9f8b3a16f
mit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
abuchan | 4:5ae9f8b3a16f | 1 | #include "mbed.h" |
abuchan | 4:5ae9f8b3a16f | 2 | #include "Motor.h" |
benkatz | 2:89bb6272869b | 3 | #include "PID.h" |
benkatz | 2:89bb6272869b | 4 | |
coldplay | 5:e90c8b57811c | 5 | #define PWM_PIN p22 |
coldplay | 5:e90c8b57811c | 6 | #define MIN_1_PIN p19 |
coldplay | 5:e90c8b57811c | 7 | #define MIN_2_PIN p20 |
coldplay | 5:e90c8b57811c | 8 | #define ENC_A_PIN p16 |
coldplay | 5:e90c8b57811c | 9 | #define ENC_B_PIN p17 |
coldplay | 5:e90c8b57811c | 10 | #define CURRENT_PIN p18 |
benkatz | 2:89bb6272869b | 11 | |
coldplay | 5:e90c8b57811c | 12 | #define pi 3.141592654 |
benkatz | 2:89bb6272869b | 13 | |
coldplay | 5:e90c8b57811c | 14 | #define PWM_PERIOD 0.00005 |
abuchan | 4:5ae9f8b3a16f | 15 | #define PID_CUR_PERIOD 0.001 |
abuchan | 4:5ae9f8b3a16f | 16 | #define PID_POS_PERIOD 0.005 |
abuchan | 4:5ae9f8b3a16f | 17 | |
abuchan | 4:5ae9f8b3a16f | 18 | #define PID_POS_P_GAIN 0.5 |
abuchan | 4:5ae9f8b3a16f | 19 | #define PID_POS_D_GAIN 10.0 |
abuchan | 4:5ae9f8b3a16f | 20 | #define PID_POS_I_GAIN 0.1 |
aberk | 0:bcff39fac858 | 21 | |
abuchan | 4:5ae9f8b3a16f | 22 | #define PID_CUR_P_GAIN 0.5 |
abuchan | 4:5ae9f8b3a16f | 23 | #define PID_CUR_D_GAIN 0.0 |
abuchan | 4:5ae9f8b3a16f | 24 | //#define PID_CUR_I_GAIN 0.001 |
abuchan | 4:5ae9f8b3a16f | 25 | #define PID_CUR_I_GAIN 0.01 |
benkatz | 3:cae0b305d54c | 26 | |
coldplay | 5:e90c8b57811c | 27 | Serial pc(USBTX, USBRX); |
coldplay | 5:e90c8b57811c | 28 | |
benkatz | 2:89bb6272869b | 29 | |
abuchan | 4:5ae9f8b3a16f | 30 | Motor motor( |
abuchan | 4:5ae9f8b3a16f | 31 | PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD, |
abuchan | 4:5ae9f8b3a16f | 32 | CURRENT_PIN, |
abuchan | 4:5ae9f8b3a16f | 33 | ENC_A_PIN, ENC_B_PIN, |
abuchan | 4:5ae9f8b3a16f | 34 | PID_CUR_P_GAIN, PID_CUR_D_GAIN, PID_CUR_I_GAIN |
abuchan | 4:5ae9f8b3a16f | 35 | ); |
benkatz | 3:cae0b305d54c | 36 | |
coldplay | 5:e90c8b57811c | 37 | HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD); |
abuchan | 4:5ae9f8b3a16f | 38 | //CurrentSense current_sense(CURRENT_PIN); |
coldplay | 5:e90c8b57811c | 39 | Encoder encoder(ENC_A_PIN, ENC_B_PIN); |
abuchan | 4:5ae9f8b3a16f | 40 | |
abuchan | 4:5ae9f8b3a16f | 41 | PIDController position_controller( |
abuchan | 4:5ae9f8b3a16f | 42 | PID_POS_P_GAIN, PID_POS_D_GAIN, PID_POS_I_GAIN |
abuchan | 4:5ae9f8b3a16f | 43 | ); |
abuchan | 4:5ae9f8b3a16f | 44 | |
abuchan | 4:5ae9f8b3a16f | 45 | Ticker current_ticker; |
abuchan | 4:5ae9f8b3a16f | 46 | |
abuchan | 4:5ae9f8b3a16f | 47 | void update_current(void) { |
abuchan | 4:5ae9f8b3a16f | 48 | motor.update(); |
benkatz | 2:89bb6272869b | 49 | } |
benkatz | 2:89bb6272869b | 50 | |
abuchan | 4:5ae9f8b3a16f | 51 | Ticker position_ticker; |
benkatz | 3:cae0b305d54c | 52 | |
abuchan | 4:5ae9f8b3a16f | 53 | void update_position(void) { |
abuchan | 4:5ae9f8b3a16f | 54 | motor.set_command(position_controller.command_position(motor.get_position())); |
aberk | 0:bcff39fac858 | 55 | } |
benkatz | 2:89bb6272869b | 56 | |
abuchan | 4:5ae9f8b3a16f | 57 | DigitalOut led1(LED1); |
abuchan | 4:5ae9f8b3a16f | 58 | DigitalOut led2(LED2); |
abuchan | 4:5ae9f8b3a16f | 59 | DigitalOut led3(LED3); |
benkatz | 2:89bb6272869b | 60 | |
abuchan | 4:5ae9f8b3a16f | 61 | int main() { |
coldplay | 5:e90c8b57811c | 62 | wait(5); |
coldplay | 5:e90c8b57811c | 63 | pc.printf("Built " __DATE__ " " __TIME__ "\r\n"); |
abuchan | 4:5ae9f8b3a16f | 64 | |
abuchan | 4:5ae9f8b3a16f | 65 | led1 = 1; |
abuchan | 4:5ae9f8b3a16f | 66 | led2 = 1; |
abuchan | 4:5ae9f8b3a16f | 67 | led3 = 1; |
abuchan | 4:5ae9f8b3a16f | 68 | |
coldplay | 5:e90c8b57811c | 69 | wait(5); |
abuchan | 4:5ae9f8b3a16f | 70 | |
abuchan | 4:5ae9f8b3a16f | 71 | led2 = 0; |
coldplay | 5:e90c8b57811c | 72 | pc.printf("Initializing\n\r"); |
abuchan | 4:5ae9f8b3a16f | 73 | |
coldplay | 5:e90c8b57811c | 74 | // current_ticker.attach(&update_current, PID_CUR_PERIOD); |
coldplay | 5:e90c8b57811c | 75 | // position_ticker.attach(&update_position, PID_POS_PERIOD); |
abuchan | 4:5ae9f8b3a16f | 76 | |
abuchan | 4:5ae9f8b3a16f | 77 | int count = 0; |
abuchan | 4:5ae9f8b3a16f | 78 | float command = 0.0; |
coldplay | 5:e90c8b57811c | 79 | // wait(5); |
abuchan | 4:5ae9f8b3a16f | 80 | led3 = 0; |
coldplay | 5:e90c8b57811c | 81 | pc.printf("Starting\n\r"); |
coldplay | 5:e90c8b57811c | 82 | |
coldplay | 5:e90c8b57811c | 83 | hbridge.initialize(); |
benkatz | 3:cae0b305d54c | 84 | |
abuchan | 4:5ae9f8b3a16f | 85 | while(1){ |
coldplay | 5:e90c8b57811c | 86 | //command = 0.05 * abs((50-count)/10) - 0.1; |
coldplay | 5:e90c8b57811c | 87 | //motor.set_command(command); |
coldplay | 5:e90c8b57811c | 88 | command = sin(count*pi/50); |
coldplay | 5:e90c8b57811c | 89 | if(command>0) |
coldplay | 5:e90c8b57811c | 90 | { |
coldplay | 5:e90c8b57811c | 91 | hbridge.forward(); |
coldplay | 5:e90c8b57811c | 92 | } |
coldplay | 5:e90c8b57811c | 93 | else if(command<0) |
coldplay | 5:e90c8b57811c | 94 | { |
coldplay | 5:e90c8b57811c | 95 | hbridge.back(); |
coldplay | 5:e90c8b57811c | 96 | } |
abuchan | 4:5ae9f8b3a16f | 97 | |
abuchan | 4:5ae9f8b3a16f | 98 | led1 = (count % 2); |
coldplay | 5:e90c8b57811c | 99 | |
abuchan | 4:5ae9f8b3a16f | 100 | |
coldplay | 5:e90c8b57811c | 101 | pc.printf("Position:%d\n\r",(int) encoder.get_position()); |
abuchan | 4:5ae9f8b3a16f | 102 | //printf("PC:%F\n\r", position_controller.command); |
coldplay | 5:e90c8b57811c | 103 | //pc.printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r", |
coldplay | 5:e90c8b57811c | 104 | // motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command |
coldplay | 5:e90c8b57811c | 105 | //); |
abuchan | 4:5ae9f8b3a16f | 106 | |
abuchan | 4:5ae9f8b3a16f | 107 | |
coldplay | 5:e90c8b57811c | 108 | count++; |
coldplay | 5:e90c8b57811c | 109 | wait(0.01); |
benkatz | 3:cae0b305d54c | 110 | } |
abuchan | 4:5ae9f8b3a16f | 111 | } |