First Last / Mbed 2 deprecated BMT-M9_motorcontrol

MOtor control Pololu

Dependencies:   Encoder HIDScope mbed-dsp mbed

main.cpp@1:5ac85aad9da4, 2013-09-26 (annotated)

Committer:
vsluiter
Date:
Thu Sep 26 13:08:21 2013 +0000
Revision:
1:5ac85aad9da4
Parent:
0:c9e647421e54
Child:
2:5f5b229b004d
Can choose between simple and PID control. Added averager for potmeter to smooth setpoint

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 1:5ac85aad9da4 5 #define K_P 0.007//0.0184997836671646 //0.015
vsluiter 1:5ac85aad9da4 6 #define K_I 0.00//.000824387821287097 //0
vsluiter 1:5ac85aad9da4 7 #define K_D 0.01//.0972091946803081 //0.077
vsluiter 1:5ac85aad9da4 8
vsluiter 1:5ac85aad9da4 9 #define POT_AVG 30
vsluiter 0:c9e647421e54 10
vsluiter 0:c9e647421e54 11 void coerce(float * in, float min, float max);
vsluiter 0:c9e647421e54 12 float pid(float setpoint, float measurement);
vsluiter 1:5ac85aad9da4 13 AnalogIn potmeter(PTC2);
vsluiter 0:c9e647421e54 14 volatile bool looptimerflag;
vsluiter 1:5ac85aad9da4 15 float potsamples[POT_AVG];
vsluiter 0:c9e647421e54 16
vsluiter 0:c9e647421e54 17 void setlooptimerflag(void)
vsluiter 0:c9e647421e54 18 {
vsluiter 0:c9e647421e54 19 looptimerflag = true;
vsluiter 0:c9e647421e54 20 }
vsluiter 0:c9e647421e54 21
vsluiter 1:5ac85aad9da4 22 void potAverager(void)
vsluiter 1:5ac85aad9da4 23 {
vsluiter 1:5ac85aad9da4 24
vsluiter 1:5ac85aad9da4 25 static uint16_t sample_index = 0;
vsluiter 1:5ac85aad9da4 26 float voltage = potmeter.read()-.5;
vsluiter 1:5ac85aad9da4 27
vsluiter 1:5ac85aad9da4 28 potsamples[sample_index] = voltage;
vsluiter 1:5ac85aad9da4 29
vsluiter 1:5ac85aad9da4 30 sample_index++;
vsluiter 1:5ac85aad9da4 31 if(sample_index >= POT_AVG)
vsluiter 1:5ac85aad9da4 32 sample_index = 0;
vsluiter 1:5ac85aad9da4 33 }
vsluiter 1:5ac85aad9da4 34
vsluiter 1:5ac85aad9da4 35 float getpotAverage(void)
vsluiter 1:5ac85aad9da4 36 {
vsluiter 1:5ac85aad9da4 37 uint16_t valuecounter;
vsluiter 1:5ac85aad9da4 38 float sum = 0;
vsluiter 1:5ac85aad9da4 39 for(valuecounter = 0 ; valuecounter < POT_AVG ; valuecounter++)
vsluiter 1:5ac85aad9da4 40 {
vsluiter 1:5ac85aad9da4 41 sum += potsamples[valuecounter];
vsluiter 1:5ac85aad9da4 42 }
vsluiter 1:5ac85aad9da4 43 return sum / (POT_AVG*1.);
vsluiter 1:5ac85aad9da4 44
vsluiter 1:5ac85aad9da4 45 }
vsluiter 1:5ac85aad9da4 46
vsluiter 0:c9e647421e54 47 int main() {
vsluiter 0:c9e647421e54 48 Encoder motor1(PTD0,PTC9);
vsluiter 1:5ac85aad9da4 49 Ticker potaverage;
vsluiter 0:c9e647421e54 50 MODSERIAL pc(USBTX,USBRX);
vsluiter 0:c9e647421e54 51 PwmOut pwm_motor(PTA12);
vsluiter 0:c9e647421e54 52 DigitalOut motordir(PTD3);
vsluiter 1:5ac85aad9da4 53 potaverage.attach(potAverager,0.002);
vsluiter 0:c9e647421e54 54 pc.baud(115200);
vsluiter 0:c9e647421e54 55 Ticker looptimer;
vsluiter 0:c9e647421e54 56 looptimer.attach(setlooptimerflag,0.01);
vsluiter 0:c9e647421e54 57 while(1) {
vsluiter 1:5ac85aad9da4 58 float setpoint;
vsluiter 0:c9e647421e54 59 float new_pwm;
vsluiter 0:c9e647421e54 60 while(!looptimerflag);
vsluiter 0:c9e647421e54 61 looptimerflag = false;
vsluiter 1:5ac85aad9da4 62 setpoint = (getpotAverage())*2000;
vsluiter 1:5ac85aad9da4 63 new_pwm = (setpoint - motor1.getPosition())*.001;
vsluiter 1:5ac85aad9da4 64 //new_pwm = pid(setpoint, motor1.getPosition());
vsluiter 0:c9e647421e54 65 coerce(&new_pwm, -1,1);
vsluiter 0:c9e647421e54 66 if(new_pwm > 0)
vsluiter 0:c9e647421e54 67 motordir = 1;
vsluiter 0:c9e647421e54 68 else
vsluiter 0:c9e647421e54 69 motordir = 0;
vsluiter 0:c9e647421e54 70 pwm_motor.write(abs(new_pwm));
vsluiter 0:c9e647421e54 71 }
vsluiter 0:c9e647421e54 72 }
vsluiter 0:c9e647421e54 73
vsluiter 1:5ac85aad9da4 74
vsluiter 1:5ac85aad9da4 75 //coerces value 'in' to min or max when exceeding those values
vsluiter 1:5ac85aad9da4 76 //if you'd like to understand the statement below take a google for
vsluiter 1:5ac85aad9da4 77 //'ternary operators'.
vsluiter 0:c9e647421e54 78 void coerce(float * in, float min, float max)
vsluiter 0:c9e647421e54 79 {
vsluiter 0:c9e647421e54 80 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:c9e647421e54 81 }
vsluiter 0:c9e647421e54 82
vsluiter 0:c9e647421e54 83
vsluiter 0:c9e647421e54 84 float pid(float setpoint, float measurement)
vsluiter 0:c9e647421e54 85 {
vsluiter 0:c9e647421e54 86 float error;
vsluiter 0:c9e647421e54 87 static float prev_error = 0;
vsluiter 0:c9e647421e54 88 float out_p = 0;
vsluiter 0:c9e647421e54 89 static float out_i = 0;
vsluiter 0:c9e647421e54 90 float out_d = 0;
vsluiter 0:c9e647421e54 91 error = setpoint-measurement;
vsluiter 0:c9e647421e54 92 out_p = error*K_P;
vsluiter 0:c9e647421e54 93 out_i += error*K_I;
vsluiter 1:5ac85aad9da4 94 out_d = (error-prev_error)*K_D;
vsluiter 0:c9e647421e54 95 coerce(&out_i,-0.5,0.5);
vsluiter 0:c9e647421e54 96 prev_error = error;
vsluiter 0:c9e647421e54 97 return out_p + out_i + out_d;
vsluiter 0:c9e647421e54 98 }
vsluiter 0:c9e647421e54 99