Austin Buchan
PD Control for KL05Z
main.cpp@2:f84a70291ac8, 2013-09-02 (annotated)
- Committer:
- benkatz
- Date:
- Mon Sep 02 13:22:34 2013 +0000
- Revision:
- 2:f84a70291ac8
- Parent:
- 1:8ff6c7474d95
- Child:
- 3:f169c357a44d
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 | 0:259b726d0807 | 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 | 0:259b726d0807 | 18 | float p_gain = 0; |
| benkatz | 0:259b726d0807 | 19 | float d_gain = 0; |
| benkatz | 0:259b726d0807 | 20 | float sample_period = .0004; |
| benkatz | 0:259b726d0807 | 21 | float command = 0; |
| benkatz | 0:259b726d0807 | 22 | |
| benkatz | 0:259b726d0807 | 23 | // |
| benkatz | 0:259b726d0807 | 24 | |
| benkatz | 0:259b726d0807 | 25 | float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians |
| benkatz | 0:259b726d0807 | 26 | { |
| benkatz | 0:259b726d0807 | 27 | return ((pulses/steps_per_rev)*(2*pi)); |
| benkatz | 0:259b726d0807 | 28 | } |
| benkatz | 0:259b726d0807 | 29 | |
| benkatz | 0:259b726d0807 | 30 | |
| benkatz | 2:f84a70291ac8 | 31 | void updateCommand(void){ |
| benkatz | 0:259b726d0807 | 32 | |
| benkatz | 0:259b726d0807 | 33 | position = pulsesToRadians(encoder.getPulses()); //use the getPulses functin to read encoder position, convert value to radians |
| benkatz | 0:259b726d0807 | 34 | old_error = error; |
| benkatz | 2:f84a70291ac8 | 35 | error = position - goal_position; |
| benkatz | 2:f84a70291ac8 | 36 | command = p_gain*error + d_gain*sample_period*(error-old_error); //modify the command based on position feedback |
| benkatz | 0:259b726d0807 | 37 | |
| benkatz | 0:259b726d0807 | 38 | |
| benkatz | 0:259b726d0807 | 39 | } |
| benkatz | 0:259b726d0807 | 40 | |
| benkatz | 2:f84a70291ac8 | 41 | |
| benkatz | 0:259b726d0807 | 42 | |
| benkatz | 0:259b726d0807 | 43 | |
| benkatz | 0:259b726d0807 | 44 | |
| benkatz | 0:259b726d0807 | 45 | int main() { |
| benkatz | 0:259b726d0807 | 46 | |
| benkatz | 0:259b726d0807 | 47 | |
| benkatz | 2:f84a70291ac8 | 48 | SampleFrequency.attach(&updateCommand, sample_period); //Encoder will be sampled at 2.5 kHz |
| benkatz | 0:259b726d0807 | 49 | |
| benkatz | 0:259b726d0807 | 50 | while(1) { |
| benkatz | 0:259b726d0807 | 51 | |
| benkatz | 0:259b726d0807 | 52 | } |
| benkatz | 0:259b726d0807 | 53 | } |