First Last / Mbed 2 deprecated BMT-M9_motorcontrol

MOtor control Pololu

Dependencies:   Encoder HIDScope mbed-dsp mbed

main.cpp@6:0832c6c6bcba, 2014-09-29 (annotated)

Committer:
vsluiter
Date:
Mon Sep 29 21:39:29 2014 +0000
Revision:
6:0832c6c6bcba
Parent:
5:06381e54b94a
Child:
7:37b06688b449
Removed Serial references. Added HIDScope and arm math libraries

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