First Last / Mbed 2 deprecated BMT-M9_motorcontrol

MOtor control Pololu

Dependencies:   Encoder HIDScope mbed-dsp mbed

main.cpp@7:37b06688b449, 2014-09-30 (annotated)

Committer:
vsluiter
Date:
Tue Sep 30 20:30:35 2014 +0000
Revision:
7:37b06688b449
Parent:
6:0832c6c6bcba
Child:
8:15c6cb82c725
Working setup for demo, need to clean up code;

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 7:37b06688b449 6 #define K_P (2 *TSAMP)
vsluiter 5:06381e54b94a 7 #define K_I (0.00 *TSAMP)
vsluiter 7:37b06688b449 8 #define K_D (0 *TSAMP)
vsluiter 1:5ac85aad9da4 9
vsluiter 4:1a53b06eeb7f 10 #define POT_AVG 50
vsluiter 6:0832c6c6bcba 11
vsluiter 0:c9e647421e54 12 void coerce(float * in, float min, float max);
vsluiter 0:c9e647421e54 13 float pid(float setpoint, float measurement);
vsluiter 1:5ac85aad9da4 14 AnalogIn potmeter(PTC2);
vsluiter 0:c9e647421e54 15 volatile bool looptimerflag;
vsluiter 1:5ac85aad9da4 16 float potsamples[POT_AVG];
vsluiter 7:37b06688b449 17 HIDScope scope(5);
vsluiter 6:0832c6c6bcba 18
vsluiter 0:c9e647421e54 19
vsluiter 0:c9e647421e54 20 void setlooptimerflag(void)
vsluiter 0:c9e647421e54 21 {
vsluiter 0:c9e647421e54 22 looptimerflag = true;
vsluiter 0:c9e647421e54 23 }
vsluiter 0:c9e647421e54 24
vsluiter 7:37b06688b449 25 /*
vsluiter 1:5ac85aad9da4 26 void potAverager(void)
vsluiter 1:5ac85aad9da4 27 {
vsluiter 1:5ac85aad9da4 28 static uint16_t sample_index = 0;
vsluiter 1:5ac85aad9da4 29 float voltage = potmeter.read()-.5;
vsluiter 1:5ac85aad9da4 30
vsluiter 1:5ac85aad9da4 31 potsamples[sample_index] = voltage;
vsluiter 4:1a53b06eeb7f 32
vsluiter 1:5ac85aad9da4 33 sample_index++;
vsluiter 1:5ac85aad9da4 34 if(sample_index >= POT_AVG)
vsluiter 1:5ac85aad9da4 35 sample_index = 0;
vsluiter 1:5ac85aad9da4 36 }
vsluiter 1:5ac85aad9da4 37
vsluiter 1:5ac85aad9da4 38 float getpotAverage(void)
vsluiter 1:5ac85aad9da4 39 {
vsluiter 4:1a53b06eeb7f 40 uint16_t valuecounter;
vsluiter 4:1a53b06eeb7f 41 float sum = 0;
vsluiter 4:1a53b06eeb7f 42 for(valuecounter = 0 ; valuecounter < POT_AVG ; valuecounter++)
vsluiter 4:1a53b06eeb7f 43 {
vsluiter 4:1a53b06eeb7f 44 sum += potsamples[valuecounter];
vsluiter 4:1a53b06eeb7f 45 }
vsluiter 1:5ac85aad9da4 46 return sum / (POT_AVG*1.);
vsluiter 1:5ac85aad9da4 47 }
vsluiter 7:37b06688b449 48 */
vsluiter 1:5ac85aad9da4 49
vsluiter 4:1a53b06eeb7f 50 int main()
vsluiter 4:1a53b06eeb7f 51 {
vsluiter 6:0832c6c6bcba 52 //start Encoder
vsluiter 0:c9e647421e54 53 Encoder motor1(PTD0,PTC9);
vsluiter 6:0832c6c6bcba 54 //Ticker to average potmeter values
vsluiter 7:37b06688b449 55 // Ticker potaverage;
vsluiter 7:37b06688b449 56 /*PwmOut to motor driver*/
vsluiter 7:37b06688b449 57 PwmOut pwm_motor(PTA5);
vsluiter 6:0832c6c6bcba 58 //10kHz PWM frequency
vsluiter 6:0832c6c6bcba 59 pwm_motor.period_us(100);
vsluiter 7:37b06688b449 60 DigitalOut motordir(PTD1);
vsluiter 7:37b06688b449 61 // potaverage.attach(potAverager,0.002);
vsluiter 0:c9e647421e54 62 Ticker looptimer;
vsluiter 5:06381e54b94a 63 looptimer.attach(setlooptimerflag,TSAMP);
vsluiter 0:c9e647421e54 64 while(1) {
vsluiter 1:5ac85aad9da4 65 float setpoint;
vsluiter 0:c9e647421e54 66 float new_pwm;
vsluiter 0:c9e647421e54 67 while(!looptimerflag);
vsluiter 0:c9e647421e54 68 looptimerflag = false;
vsluiter 7:37b06688b449 69 setpoint = (potmeter.read()-.5)*2000;
vsluiter 2:5f5b229b004d 70 //new_pwm = (setpoint - motor1.getPosition())*.001;
vsluiter 2:5f5b229b004d 71 new_pwm = pid(setpoint, motor1.getPosition());
vsluiter 0:c9e647421e54 72 coerce(&new_pwm, -1,1);
vsluiter 7:37b06688b449 73 scope.set(4,motor1.getPosition());
vsluiter 6:0832c6c6bcba 74 scope.set(0, setpoint);
vsluiter 6:0832c6c6bcba 75 scope.set(1,new_pwm);
vsluiter 6:0832c6c6bcba 76 // ch 2 and 3 set in pid controller */
vsluiter 6:0832c6c6bcba 77 scope.send();
vsluiter 0:c9e647421e54 78 if(new_pwm > 0)
vsluiter 7:37b06688b449 79 motordir = 0;
vsluiter 7:37b06688b449 80 else
vsluiter 0:c9e647421e54 81 motordir = 1;
vsluiter 0:c9e647421e54 82 pwm_motor.write(abs(new_pwm));
vsluiter 0:c9e647421e54 83 }
vsluiter 0:c9e647421e54 84 }
vsluiter 0:c9e647421e54 85
vsluiter 1:5ac85aad9da4 86
vsluiter 1:5ac85aad9da4 87 //coerces value 'in' to min or max when exceeding those values
vsluiter 1:5ac85aad9da4 88 //if you'd like to understand the statement below take a google for
vsluiter 1:5ac85aad9da4 89 //'ternary operators'.
vsluiter 0:c9e647421e54 90 void coerce(float * in, float min, float max)
vsluiter 0:c9e647421e54 91 {
vsluiter 6:0832c6c6bcba 92 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:c9e647421e54 93 }
vsluiter 0:c9e647421e54 94
vsluiter 0:c9e647421e54 95
vsluiter 0:c9e647421e54 96 float pid(float setpoint, float measurement)
vsluiter 0:c9e647421e54 97 {
vsluiter 4:1a53b06eeb7f 98 float error;
vsluiter 4:1a53b06eeb7f 99 static float prev_error = 0;
vsluiter 4:1a53b06eeb7f 100 float out_p = 0;
vsluiter 4:1a53b06eeb7f 101 static float out_i = 0;
vsluiter 4:1a53b06eeb7f 102 float out_d = 0;
vsluiter 4:1a53b06eeb7f 103 error = setpoint-measurement;
vsluiter 4:1a53b06eeb7f 104 out_p = error*K_P;
vsluiter 4:1a53b06eeb7f 105 out_i += error*K_I;
vsluiter 4:1a53b06eeb7f 106 out_d = (error-prev_error)*K_D;
vsluiter 4:1a53b06eeb7f 107 coerce(&out_i,-0.5,0.5);
vsluiter 4:1a53b06eeb7f 108 prev_error = error;
vsluiter 6:0832c6c6bcba 109 scope.set(2,error);
vsluiter 6:0832c6c6bcba 110 scope.set(3,out_p);
vsluiter 4:1a53b06eeb7f 111 return out_p + out_i + out_d;
vsluiter 0:c9e647421e54 112 }
vsluiter 0:c9e647421e54 113