First Last / Mbed 2 deprecated BMT-M9_motorcontrol

MOtor control Pololu

Dependencies:   Encoder HIDScope mbed-dsp mbed

main.cpp@9:e09d81850a05, 2014-10-01 (annotated)

Committer:
vsluiter
Date:
Wed Oct 01 09:34:40 2014 +0000
Revision:
9:e09d81850a05
Parent:
8:15c6cb82c725
Child:
10:6f8af13cc3f4
Fixed errror of KI,KD, KP relation ot TSAMP

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 6:0832c6c6bcba 3 #include "HIDScope.h"
vsluiter 6:0832c6c6bcba 4
vsluiter 5:06381e54b94a 5 #define TSAMP 0.01
vsluiter 9:e09d81850a05 6 #define K_P (3)
vsluiter 8:15c6cb82c725 7 #define K_I (0.02 *TSAMP)
vsluiter 9:e09d81850a05 8 #define K_D (0 /TSAMP)
vsluiter 8:15c6cb82c725 9 #define I_LIMIT 1.
vsluiter 1:5ac85aad9da4 10
vsluiter 4:1a53b06eeb7f 11 #define POT_AVG 50
vsluiter 6:0832c6c6bcba 12
vsluiter 8:15c6cb82c725 13 void clamp(float * in, float min, float max);
vsluiter 0:c9e647421e54 14 float pid(float setpoint, float measurement);
vsluiter 1:5ac85aad9da4 15 AnalogIn potmeter(PTC2);
vsluiter 0:c9e647421e54 16 volatile bool looptimerflag;
vsluiter 1:5ac85aad9da4 17 float potsamples[POT_AVG];
vsluiter 8:15c6cb82c725 18 HIDScope scope(6);
vsluiter 6:0832c6c6bcba 19
vsluiter 0:c9e647421e54 20
vsluiter 0:c9e647421e54 21 void setlooptimerflag(void)
vsluiter 0:c9e647421e54 22 {
vsluiter 0:c9e647421e54 23 looptimerflag = true;
vsluiter 0:c9e647421e54 24 }
vsluiter 0:c9e647421e54 25
vsluiter 1:5ac85aad9da4 26
vsluiter 1:5ac85aad9da4 27
vsluiter 4:1a53b06eeb7f 28 int main()
vsluiter 4:1a53b06eeb7f 29 {
vsluiter 6:0832c6c6bcba 30 //start Encoder
vsluiter 0:c9e647421e54 31 Encoder motor1(PTD0,PTC9);
vsluiter 7:37b06688b449 32 /*PwmOut to motor driver*/
vsluiter 7:37b06688b449 33 PwmOut pwm_motor(PTA5);
vsluiter 6:0832c6c6bcba 34 //10kHz PWM frequency
vsluiter 6:0832c6c6bcba 35 pwm_motor.period_us(100);
vsluiter 7:37b06688b449 36 DigitalOut motordir(PTD1);
vsluiter 0:c9e647421e54 37 Ticker looptimer;
vsluiter 5:06381e54b94a 38 looptimer.attach(setlooptimerflag,TSAMP);
vsluiter 0:c9e647421e54 39 while(1) {
vsluiter 1:5ac85aad9da4 40 float setpoint;
vsluiter 0:c9e647421e54 41 float new_pwm;
vsluiter 8:15c6cb82c725 42 /*wait until timer has elapsed*/
vsluiter 0:c9e647421e54 43 while(!looptimerflag);
vsluiter 8:15c6cb82c725 44 looptimerflag = false; //clear flag
vsluiter 8:15c6cb82c725 45 /*potmeter value: 0-1*/
vsluiter 8:15c6cb82c725 46 setpoint = (potmeter.read()-.5)*500;
vsluiter 8:15c6cb82c725 47 /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
vsluiter 2:5f5b229b004d 48 new_pwm = pid(setpoint, motor1.getPosition());
vsluiter 8:15c6cb82c725 49 clamp(&new_pwm, -1,1);
vsluiter 6:0832c6c6bcba 50 scope.set(0, setpoint);
vsluiter 8:15c6cb82c725 51 scope.set(4, new_pwm);
vsluiter 8:15c6cb82c725 52 scope.set(5, motor1.getPosition());
vsluiter 8:15c6cb82c725 53 // ch 1, 2 and 3 set in pid controller */
vsluiter 6:0832c6c6bcba 54 scope.send();
vsluiter 0:c9e647421e54 55 if(new_pwm > 0)
vsluiter 7:37b06688b449 56 motordir = 0;
vsluiter 7:37b06688b449 57 else
vsluiter 0:c9e647421e54 58 motordir = 1;
vsluiter 0:c9e647421e54 59 pwm_motor.write(abs(new_pwm));
vsluiter 0:c9e647421e54 60 }
vsluiter 0:c9e647421e54 61 }
vsluiter 0:c9e647421e54 62
vsluiter 1:5ac85aad9da4 63
vsluiter 8:15c6cb82c725 64 //clamps value 'in' to min or max when exceeding those values
vsluiter 1:5ac85aad9da4 65 //if you'd like to understand the statement below take a google for
vsluiter 1:5ac85aad9da4 66 //'ternary operators'.
vsluiter 8:15c6cb82c725 67 void clamp(float * in, float min, float max)
vsluiter 0:c9e647421e54 68 {
vsluiter 6:0832c6c6bcba 69 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:c9e647421e54 70 }
vsluiter 0:c9e647421e54 71
vsluiter 0:c9e647421e54 72
vsluiter 0:c9e647421e54 73 float pid(float setpoint, float measurement)
vsluiter 0:c9e647421e54 74 {
vsluiter 4:1a53b06eeb7f 75 float error;
vsluiter 4:1a53b06eeb7f 76 static float prev_error = 0;
vsluiter 4:1a53b06eeb7f 77 float out_p = 0;
vsluiter 4:1a53b06eeb7f 78 static float out_i = 0;
vsluiter 4:1a53b06eeb7f 79 float out_d = 0;
vsluiter 4:1a53b06eeb7f 80 error = setpoint-measurement;
vsluiter 4:1a53b06eeb7f 81 out_p = error*K_P;
vsluiter 4:1a53b06eeb7f 82 out_i += error*K_I;
vsluiter 4:1a53b06eeb7f 83 out_d = (error-prev_error)*K_D;
vsluiter 8:15c6cb82c725 84 clamp(&out_i,-I_LIMIT,I_LIMIT);
vsluiter 4:1a53b06eeb7f 85 prev_error = error;
vsluiter 8:15c6cb82c725 86 scope.set(1,out_p);
vsluiter 8:15c6cb82c725 87 scope.set(2,out_i);
vsluiter 8:15c6cb82c725 88 scope.set(3,out_d);
vsluiter 4:1a53b06eeb7f 89 return out_p + out_i + out_d;
vsluiter 0:c9e647421e54 90 }
vsluiter 0:c9e647421e54 91