First Last / Mbed 2 deprecated BMT-M9_motorcontrol

MOtor control Pololu

Dependencies:   Encoder HIDScope mbed-dsp mbed

main.cpp@13:0d4bc34e410f, 2014-10-08 (annotated)

Committer:
vsluiter
Date:
Wed Oct 08 15:11:49 2014 +0000
Revision:
13:0d4bc34e410f
Parent:
12:19ea553567a3
Child:
14:91403dd3dcf5
Updated D Gain;

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