First Last / Mbed 2 deprecated BMT-M9_motorcontrol

MOtor control Pololu

Dependencies:   Encoder HIDScope mbed-dsp mbed

main.cpp@0:c9e647421e54, 2013-09-26 (annotated)

Committer:
vsluiter
Date:
Thu Sep 26 11:31:08 2013 +0000
Revision:
0:c9e647421e54
Child:
1:5ac85aad9da4
placed pid in function, coerce in function;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c9e647421e54 1 #include "mbed.h"
vsluiter 0:c9e647421e54 2 #include "encoder.h"
vsluiter 0:c9e647421e54 3 #include "MODSERIAL.h"
vsluiter 0:c9e647421e54 4
vsluiter 0:c9e647421e54 5 #define K_P 0.015//184997836671646
vsluiter 0:c9e647421e54 6 #define K_I 0.//000824387821287097
vsluiter 0:c9e647421e54 7 #define K_D 0.077//972091946803081
vsluiter 0:c9e647421e54 8
vsluiter 0:c9e647421e54 9 void coerce(float * in, float min, float max);
vsluiter 0:c9e647421e54 10 float pid(float setpoint, float measurement);
vsluiter 0:c9e647421e54 11
vsluiter 0:c9e647421e54 12 volatile bool looptimerflag;
vsluiter 0:c9e647421e54 13
vsluiter 0:c9e647421e54 14 void setlooptimerflag(void)
vsluiter 0:c9e647421e54 15 {
vsluiter 0:c9e647421e54 16 looptimerflag = true;
vsluiter 0:c9e647421e54 17 }
vsluiter 0:c9e647421e54 18
vsluiter 0:c9e647421e54 19 int main() {
vsluiter 0:c9e647421e54 20 Encoder motor1(PTD0,PTC9);
vsluiter 0:c9e647421e54 21 MODSERIAL pc(USBTX,USBRX);
vsluiter 0:c9e647421e54 22 PwmOut pwm_motor(PTA12);
vsluiter 0:c9e647421e54 23 DigitalOut motordir(PTD3);
vsluiter 0:c9e647421e54 24 AnalogIn potmeter(PTC2);
vsluiter 0:c9e647421e54 25 pc.baud(115200);
vsluiter 0:c9e647421e54 26 Ticker looptimer;
vsluiter 0:c9e647421e54 27 looptimer.attach(setlooptimerflag,0.01);
vsluiter 0:c9e647421e54 28 while(1) {
vsluiter 0:c9e647421e54 29 static float setpoint = 0;
vsluiter 0:c9e647421e54 30 float new_pwm;
vsluiter 0:c9e647421e54 31 while(!looptimerflag);
vsluiter 0:c9e647421e54 32 looptimerflag = false;
vsluiter 0:c9e647421e54 33 setpoint = (potmeter.read()-0.5)*2000;
vsluiter 0:c9e647421e54 34 new_pwm = pid(setpoint, motor1.getPosition());
vsluiter 0:c9e647421e54 35 coerce(&new_pwm, -1,1);
vsluiter 0:c9e647421e54 36 if(new_pwm > 0)
vsluiter 0:c9e647421e54 37 motordir = 1;
vsluiter 0:c9e647421e54 38 else
vsluiter 0:c9e647421e54 39 motordir = 0;
vsluiter 0:c9e647421e54 40 pwm_motor.write(abs(new_pwm));
vsluiter 0:c9e647421e54 41 }
vsluiter 0:c9e647421e54 42 }
vsluiter 0:c9e647421e54 43
vsluiter 0:c9e647421e54 44 void coerce(float * in, float min, float max)
vsluiter 0:c9e647421e54 45 {
vsluiter 0:c9e647421e54 46 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:c9e647421e54 47 }
vsluiter 0:c9e647421e54 48
vsluiter 0:c9e647421e54 49
vsluiter 0:c9e647421e54 50 float pid(float setpoint, float measurement)
vsluiter 0:c9e647421e54 51 {
vsluiter 0:c9e647421e54 52 //static int16_t setpoint = 0 //static int16_t setpoint = 0
vsluiter 0:c9e647421e54 53 float error;
vsluiter 0:c9e647421e54 54 static float prev_error = 0;
vsluiter 0:c9e647421e54 55 float out_p = 0;
vsluiter 0:c9e647421e54 56 static float out_i = 0;
vsluiter 0:c9e647421e54 57 float out_d = 0;
vsluiter 0:c9e647421e54 58 error = setpoint-measurement;
vsluiter 0:c9e647421e54 59 out_p = error*K_P;
vsluiter 0:c9e647421e54 60 out_i += error*K_I;
vsluiter 0:c9e647421e54 61 out_d = (error-prev_error)*K_D;
vsluiter 0:c9e647421e54 62 coerce(&out_i,-0.5,0.5);
vsluiter 0:c9e647421e54 63 prev_error = error;
vsluiter 0:c9e647421e54 64 return out_p + out_i + out_d;
vsluiter 0:c9e647421e54 65 }
vsluiter 0:c9e647421e54 66