Austin Buchan / Mbed 2 deprecated PDControllerExample

PD Control for KL05Z

Dependencies:   QEI mbed

main.cpp@5:0998ea3869ff, 2015-11-19 (annotated)

Committer:
abuchan
Date:
Thu Nov 19 17:39:20 2015 +0000
Revision:
5:0998ea3869ff
Parent:
4:d52e31c73e50 
Downloading bin file seems to be working, PD control working

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;
abuchan 5:0998ea3869ff 18 float p_gain = 1.0;
benkatz 0:259b726d0807 19 float d_gain = 0;
abuchan 5:0998ea3869ff 20 float sample_period = 0.01;
abuchan 5:0998ea3869ff 21 float pwm_period = 0.0002;
benkatz 0:259b726d0807 22 float command = 0;
benkatz 0:259b726d0807 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 float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
benkatz 0:259b726d0807 29 {
benkatz 0:259b726d0807 30 return ((pulses/steps_per_rev)*(2*pi));
benkatz 0:259b726d0807 31 }
benkatz 0:259b726d0807 32
abuchan 4:d52e31c73e50 33 void signal_to_hbridge( float signal) //Input of -1 means full reverse, 1 means full forward
benkatz 3:f169c357a44d 34 { //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write function
benkatz 3:f169c357a44d 35 if (signal < 0) {
abuchan 4:d52e31c73e50 36 dir1.write(0);
abuchan 4:d52e31c73e50 37 dir2.write(1);
abuchan 4:d52e31c73e50 38 Motor.write(signal * -1);
benkatz 3:f169c357a44d 39 }
benkatz 3:f169c357a44d 40 else if (signal > 0) {
abuchan 4:d52e31c73e50 41 dir1.write(1);
abuchan 4:d52e31c73e50 42 dir2.write(0);
abuchan 4:d52e31c73e50 43 Motor.write(signal);
benkatz 3:f169c357a44d 44 }
benkatz 3:f169c357a44d 45 else {
abuchan 4:d52e31c73e50 46 dir1.write(0);
abuchan 4:d52e31c73e50 47 dir2.write(0);
benkatz 3:f169c357a44d 48 }
benkatz 3:f169c357a44d 49 }
abuchan 5:0998ea3869ff 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
abuchan 4:d52e31c73e50 60 DigitalOut led1(LED1);
abuchan 4:d52e31c73e50 61 DigitalOut led2(LED2);
benkatz 0:259b726d0807 62
benkatz 0:259b726d0807 63 int main() {
abuchan 5:0998ea3869ff 64 led1 = 1;
abuchan 5:0998ea3869ff 65 led2 = 1;
abuchan 5:0998ea3869ff 66 //led3 = 1;
abuchan 5:0998ea3869ff 67
abuchan 5:0998ea3869ff 68 wait(0.5);
abuchan 4:d52e31c73e50 69 led1 = 0;
abuchan 4:d52e31c73e50 70 led2 = 1;
abuchan 5:0998ea3869ff 71
benkatz 3:f169c357a44d 72 Timer timer;
benkatz 3:f169c357a44d 73 timer.start();
abuchan 5:0998ea3869ff 74
abuchan 5:0998ea3869ff 75 Motor.period(pwm_period);
abuchan 5:0998ea3869ff 76
abuchan 5:0998ea3869ff 77 wait(0.5);
abuchan 5:0998ea3869ff 78 led1 = 1;
abuchan 5:0998ea3869ff 79 led2 = 0;
abuchan 5:0998ea3869ff 80
abuchan 5:0998ea3869ff 81 bool state = false;
abuchan 5:0998ea3869ff 82 long count = 0;
abuchan 4:d52e31c73e50 83
benkatz 3:f169c357a44d 84 while(1) {
abuchan 5:0998ea3869ff 85 if (timer.read() > 0.0004) {
abuchan 5:0998ea3869ff 86 command = updateCommand();
abuchan 5:0998ea3869ff 87 timer.reset();
abuchan 5:0998ea3869ff 88 if(++count == 250) {
abuchan 5:0998ea3869ff 89 count = 0;
abuchan 5:0998ea3869ff 90 state = !state;
abuchan 5:0998ea3869ff 91 led2 = state;
abuchan 5:0998ea3869ff 92 }
abuchan 5:0998ea3869ff 93 }
abuchan 5:0998ea3869ff 94 signal_to_hbridge(command);
benkatz 0:259b726d0807 95 }
abuchan 5:0998ea3869ff 96 }