Austin Buchan / Mbed 2 deprecated PDControllerExample

PD Control for KL05Z

Dependencies:   QEI mbed

main.cpp@0:259b726d0807, 2013-09-01 (annotated)

Committer:
benkatz
Date:
Sun Sep 01 19:45:48 2013 +0000
Revision:
0:259b726d0807
Child:
1:8ff6c7474d95
;

Who changed what in which revision?

UserRevisionLine numberNew 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 0:259b726d0807 31 void updateCommand(float desired_position, float KP, float KD){
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 0:259b726d0807 35 error = position - desired_position;
benkatz 0:259b726d0807 36 command = KP*error + KD*sample_period*(error-old_error); //modify the command
benkatz 0:259b726d0807 37
benkatz 0:259b726d0807 38
benkatz 0:259b726d0807 39 }
benkatz 0:259b726d0807 40
benkatz 0:259b726d0807 41 void update(void){ //The function attached to the ticker can't take inputs
benkatz 0:259b726d0807 42 updateCommand(goal_position, p_gain, d_gain);
benkatz 0:259b726d0807 43 }
benkatz 0:259b726d0807 44
benkatz 0:259b726d0807 45
benkatz 0:259b726d0807 46
benkatz 0:259b726d0807 47 int main() {
benkatz 0:259b726d0807 48
benkatz 0:259b726d0807 49
benkatz 0:259b726d0807 50 SampleFrequency.attach(&update, sample_period); //Encoder will be sampled at 2.5 kHz
benkatz 0:259b726d0807 51
benkatz 0:259b726d0807 52 while(1) {
benkatz 0:259b726d0807 53
benkatz 0:259b726d0807 54 }
benkatz 0:259b726d0807 55 }