Aukie Hooglugt / Mbed 2 deprecated BMT-M9_motorcontrol_groep3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

Fork of BMT-M9_motorcontrol by First Last

main.cpp@5:06381e54b94a, 2014-09-29 (annotated)

Committer:
vsluiter
Date:
Mon Sep 29 21:20:29 2014 +0000
Revision:
5:06381e54b94a
Parent:
4:1a53b06eeb7f
Child:
6:0832c6c6bcba
Removed superfluous stuff (sort test), and added scope library

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