Austin Buchan
PD Control for KL05Z
main.cpp@3:f169c357a44d, 2013-09-03 (annotated)
- Committer:
- benkatz
- Date:
- Tue Sep 03 03:59:03 2013 +0000
- Revision:
- 3:f169c357a44d
- Parent:
- 2:f84a70291ac8
- Child:
- 4:d52e31c73e50
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| benkatz | 0:259b726d0807 | 1 | #include "mbed.h" |
| benkatz | 0:259b726d0807 | 2 | #include "QEI.h" //QEI is a quadrature encoder library |
| benkatz | 0:259b726d0807 | 3 | #define pi 3.14159265358979323846 |
| benkatz | 0:259b726d0807 | 4 | |
| benkatz | 0:259b726d0807 | 5 | |
| benkatz | 0:259b726d0807 | 6 | //create a new quadrature encoder object |
| benkatz | 0:259b726d0807 | 7 | QEI encoder(p25, p26, NC, 1200, QEI::X4_ENCODING); //(encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2)) |
| benkatz | 0:259b726d0807 | 8 | |
| benkatz | 3:f169c357a44d | 9 | //Ticker SampleFrequency; |
| benkatz | 0:259b726d0807 | 10 | |
| benkatz | 0:259b726d0807 | 11 | //Values that will be used for the PID controller |
| benkatz | 0:259b726d0807 | 12 | |
| benkatz | 0:259b726d0807 | 13 | int steps_per_rev = 1200; //Encoder CPR * gear ratio |
| benkatz | 0:259b726d0807 | 14 | float position = 0; |
| benkatz | 0:259b726d0807 | 15 | float error = 0; |
| benkatz | 0:259b726d0807 | 16 | float old_error = 0; |
| benkatz | 0:259b726d0807 | 17 | float goal_position = 0; |
| benkatz | 3:f169c357a44d | 18 | float p_gain = 1; |
| benkatz | 0:259b726d0807 | 19 | float d_gain = 0; |
| benkatz | 3:f169c357a44d | 20 | float sample_period = .01; |
| benkatz | 0:259b726d0807 | 21 | float command = 0; |
| benkatz | 0:259b726d0807 | 22 | |
| benkatz | 3:f169c357a44d | 23 | |
| benkatz | 3:f169c357a44d | 24 | PwmOut Motor[2] = {p21, p22}; //h-bridge pwm pins |
| benkatz | 3:f169c357a44d | 25 | DigitalOut dir1[2] = {p29, p15}; //h-bridge direction 1 pins |
| benkatz | 3:f169c357a44d | 26 | DigitalOut dir2[2] = {p30, p16}; |
| benkatz | 0:259b726d0807 | 27 | // |
| benkatz | 0:259b726d0807 | 28 | |
| benkatz | 0:259b726d0807 | 29 | float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians |
| benkatz | 0:259b726d0807 | 30 | { |
| benkatz | 0:259b726d0807 | 31 | return ((pulses/steps_per_rev)*(2*pi)); |
| benkatz | 0:259b726d0807 | 32 | } |
| benkatz | 0:259b726d0807 | 33 | |
| benkatz | 3:f169c357a44d | 34 | void signal_to_hbridge( float signal, int motor) //Input of -1 means full reverse, 1 means full forward |
| benkatz | 3:f169c357a44d | 35 | { //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write function |
| benkatz | 3:f169c357a44d | 36 | if (signal < 0) { |
| benkatz | 3:f169c357a44d | 37 | dir1[motor].write(0); |
| benkatz | 3:f169c357a44d | 38 | dir2[motor].write(1); |
| benkatz | 3:f169c357a44d | 39 | Motor[motor].write(signal * -1); |
| benkatz | 3:f169c357a44d | 40 | } |
| benkatz | 3:f169c357a44d | 41 | else if (signal > 0) { |
| benkatz | 3:f169c357a44d | 42 | dir1[motor].write(1); |
| benkatz | 3:f169c357a44d | 43 | dir2[motor].write(0); |
| benkatz | 3:f169c357a44d | 44 | Motor[motor].write(signal); |
| benkatz | 3:f169c357a44d | 45 | } |
| benkatz | 3:f169c357a44d | 46 | else { |
| benkatz | 3:f169c357a44d | 47 | dir1[motor].write(0); |
| benkatz | 3:f169c357a44d | 48 | dir2[motor].write(0); |
| benkatz | 3:f169c357a44d | 49 | } |
| benkatz | 3:f169c357a44d | 50 | } |
| benkatz | 3:f169c357a44d | 51 | float updateCommand(void){ |
| benkatz | 0:259b726d0807 | 52 | |
| benkatz | 0:259b726d0807 | 53 | position = pulsesToRadians(encoder.getPulses()); //use the getPulses functin to read encoder position, convert value to radians |
| benkatz | 0:259b726d0807 | 54 | old_error = error; |
| benkatz | 2:f84a70291ac8 | 55 | error = position - goal_position; |
| benkatz | 3:f169c357a44d | 56 | return p_gain*error + d_gain*(error-old_error)/sample_period; //modify the command based on position feedback |
| benkatz | 0:259b726d0807 | 57 | |
| benkatz | 0:259b726d0807 | 58 | } |
| benkatz | 0:259b726d0807 | 59 | |
| benkatz | 2:f84a70291ac8 | 60 | |
| benkatz | 0:259b726d0807 | 61 | |
| benkatz | 0:259b726d0807 | 62 | |
| benkatz | 0:259b726d0807 | 63 | |
| benkatz | 0:259b726d0807 | 64 | int main() { |
| benkatz | 3:f169c357a44d | 65 | Timer timer; |
| benkatz | 3:f169c357a44d | 66 | timer.start(); |
| benkatz | 3:f169c357a44d | 67 | Motor[0].period(.00005); |
| benkatz | 3:f169c357a44d | 68 | Motor[1].period(.00005); |
| benkatz | 3:f169c357a44d | 69 | //SampleFrequency.attach(&updateCommand, sample_period); //Encoder will be sampled at 2.5 kHz |
| benkatz | 3:f169c357a44d | 70 | while(1) { |
| benkatz | 0:259b726d0807 | 71 | |
| benkatz | 3:f169c357a44d | 72 | if (timer.read() > .0004){ |
| benkatz | 3:f169c357a44d | 73 | command = updateCommand(); |
| benkatz | 3:f169c357a44d | 74 | timer.reset(); |
| benkatz | 3:f169c357a44d | 75 | |
| benkatz | 3:f169c357a44d | 76 | } |
| benkatz | 3:f169c357a44d | 77 | signal_to_hbridge(command, 0); |
| benkatz | 3:f169c357a44d | 78 | |
| benkatz | 3:f169c357a44d | 79 | } |
| benkatz | 0:259b726d0807 | 80 | } |
| benkatz | 3:f169c357a44d | 81 |