Austin Buchan / Mbed 2 deprecated PDControllerExample

PD Control for KL05Z

Dependencies:   QEI mbed

main.cpp@4:d52e31c73e50, 2015-11-19 (annotated)

Committer:
abuchan
Date:
Thu Nov 19 00:02:49 2015 +0000
Revision:
4:d52e31c73e50
Parent:
3:f169c357a44d
Child:
5:0998ea3869ff
Adapted to KL05Z

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
abuchan 4:d52e31c73e50 7 QEI encoder(PTB6, PTB7, 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;
abuchan 4:d52e31c73e50 15 float p_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
abuchan 4:d52e31c73e50 24 PwmOut Motor = PTB13; //h-bridge pwm pins
abuchan 4:d52e31c73e50 25 DigitalOut dir1 = PTA0; //h-bridge direction 1 pins
abuchan 4:d52e31c73e50 26 DigitalOut dir2 = PTA8;
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
abuchan 4:d52e31c73e50 34 void signal_to_hbridge( float signal) //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) {
abuchan 4:d52e31c73e50 37 dir1.write(0);
abuchan 4:d52e31c73e50 38 dir2.write(1);
abuchan 4:d52e31c73e50 39 Motor.write(signal * -1);
benkatz 3:f169c357a44d 40 }
benkatz 3:f169c357a44d 41 else if (signal > 0) {
abuchan 4:d52e31c73e50 42 dir1.write(1);
abuchan 4:d52e31c73e50 43 dir2.write(0);
abuchan 4:d52e31c73e50 44 Motor.write(signal);
benkatz 3:f169c357a44d 45 }
benkatz 3:f169c357a44d 46 else {
abuchan 4:d52e31c73e50 47 dir1.write(0);
abuchan 4:d52e31c73e50 48 dir2.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
abuchan 4:d52e31c73e50 54 old_error = p_error;
abuchan 4:d52e31c73e50 55 p_error = position - goal_position;
abuchan 4:d52e31c73e50 56 return p_gain*p_error + d_gain*(p_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
abuchan 4:d52e31c73e50 62 DigitalOut led1(LED1);
abuchan 4:d52e31c73e50 63 DigitalOut led2(LED2);
benkatz 0:259b726d0807 64
benkatz 0:259b726d0807 65 int main() {
abuchan 4:d52e31c73e50 66 led1 = 0;
abuchan 4:d52e31c73e50 67 led2 = 1;
benkatz 3:f169c357a44d 68 Timer timer;
benkatz 3:f169c357a44d 69 timer.start();
abuchan 4:d52e31c73e50 70 Motor.period(.00005);
benkatz 3:f169c357a44d 71 //SampleFrequency.attach(&updateCommand, sample_period); //Encoder will be sampled at 2.5 kHz
abuchan 4:d52e31c73e50 72
benkatz 3:f169c357a44d 73 while(1) {
benkatz 0:259b726d0807 74
benkatz 3:f169c357a44d 75 if (timer.read() > .0004){
benkatz 3:f169c357a44d 76 command = updateCommand();
benkatz 3:f169c357a44d 77 timer.reset();
benkatz 3:f169c357a44d 78
benkatz 3:f169c357a44d 79 }
abuchan 4:d52e31c73e50 80 signal_to_hbridge(command);
benkatz 3:f169c357a44d 81
benkatz 3:f169c357a44d 82 }
benkatz 0:259b726d0807 83 }
benkatz 3:f169c357a44d 84