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@11:4f65e70290ac, 2014-10-15 (annotated)

Committer:
Hooglugt
Date:
Wed Oct 15 13:57:45 2014 +0000
Revision:
11:4f65e70290ac
Parent:
10:6f8af13cc3f4
Child:
12:02abb60385c8
Child:
13:7ec4762ec4ce
step function;

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 10:6f8af13cc3f4 6 #define K_P (0.01)
vsluiter 10:6f8af13cc3f4 7 #define K_I (0 *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];
Hooglugt 11:4f65e70290ac 18 HIDScope scope(2);
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);
Hooglugt 11:4f65e70290ac 32 // @param int_a Pin to be used as InterruptIn! Be careful, as not all pins on all platforms may be used as InterruptIn.
Hooglugt 11:4f65e70290ac 33 // @param int_b second encoder pin, used as DigitalIn. Can be any DigitalIn pin, not necessarily on InterruptIn location
vsluiter 7:37b06688b449 34 /*PwmOut to motor driver*/
vsluiter 7:37b06688b449 35 PwmOut pwm_motor(PTA5);
vsluiter 6:0832c6c6bcba 36 //10kHz PWM frequency
vsluiter 6:0832c6c6bcba 37 pwm_motor.period_us(100);
vsluiter 7:37b06688b449 38 DigitalOut motordir(PTD1);
vsluiter 0:c9e647421e54 39 Ticker looptimer;
vsluiter 5:06381e54b94a 40 looptimer.attach(setlooptimerflag,TSAMP);
vsluiter 0:c9e647421e54 41 while(1) {
vsluiter 1:5ac85aad9da4 42 float setpoint;
Hooglugt 11:4f65e70290ac 43 float new_pwm;
vsluiter 0:c9e647421e54 44 while(!looptimerflag);
vsluiter 8:15c6cb82c725 45 looptimerflag = false; //clear flag
Hooglugt 11:4f65e70290ac 46 //potmeter value: 0-1
Hooglugt 11:4f65e70290ac 47 //setpoint = (potmeter.read()-.5)*500;
Hooglugt 11:4f65e70290ac 48 //new_pwm = (setpoint - motor1.getPosition())*.001; -> P action
Hooglugt 11:4f65e70290ac 49 new_pwm = 1; // DIT IS PUUR VOOR DE STEPFUNCTION
Hooglugt 11:4f65e70290ac 50 //new_pwm = pid(setpoint, motor1.getPosition());
vsluiter 8:15c6cb82c725 51 clamp(&new_pwm, -1,1);
Hooglugt 11:4f65e70290ac 52 //scope.set(0, setpoint);
Hooglugt 11:4f65e70290ac 53 scope.set(1, new_pwm);
Hooglugt 11:4f65e70290ac 54 scope.set(2, motor1.getPosition());
Hooglugt 11:4f65e70290ac 55 // ch 1, 2 and 3 set in pid controller
vsluiter 6:0832c6c6bcba 56 scope.send();
vsluiter 0:c9e647421e54 57 if(new_pwm > 0)
vsluiter 7:37b06688b449 58 motordir = 0;
vsluiter 7:37b06688b449 59 else
vsluiter 0:c9e647421e54 60 motordir = 1;
vsluiter 0:c9e647421e54 61 pwm_motor.write(abs(new_pwm));
vsluiter 0:c9e647421e54 62 }
vsluiter 0:c9e647421e54 63 }
vsluiter 0:c9e647421e54 64
vsluiter 1:5ac85aad9da4 65
vsluiter 8:15c6cb82c725 66 //clamps value 'in' to min or max when exceeding those values
vsluiter 1:5ac85aad9da4 67 //if you'd like to understand the statement below take a google for
vsluiter 1:5ac85aad9da4 68 //'ternary operators'.
vsluiter 8:15c6cb82c725 69 void clamp(float * in, float min, float max)
vsluiter 0:c9e647421e54 70 {
vsluiter 6:0832c6c6bcba 71 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:c9e647421e54 72 }
vsluiter 0:c9e647421e54 73
Hooglugt 11:4f65e70290ac 74 /*
vsluiter 0:c9e647421e54 75 float pid(float setpoint, float measurement)
vsluiter 0:c9e647421e54 76 {
vsluiter 4:1a53b06eeb7f 77 float error;
vsluiter 4:1a53b06eeb7f 78 static float prev_error = 0;
vsluiter 4:1a53b06eeb7f 79 float out_p = 0;
vsluiter 4:1a53b06eeb7f 80 static float out_i = 0;
vsluiter 4:1a53b06eeb7f 81 float out_d = 0;
vsluiter 4:1a53b06eeb7f 82 error = setpoint-measurement;
vsluiter 4:1a53b06eeb7f 83 out_p = error*K_P;
vsluiter 4:1a53b06eeb7f 84 out_i += error*K_I;
vsluiter 4:1a53b06eeb7f 85 out_d = (error-prev_error)*K_D;
vsluiter 8:15c6cb82c725 86 clamp(&out_i,-I_LIMIT,I_LIMIT);
vsluiter 4:1a53b06eeb7f 87 prev_error = error;
vsluiter 8:15c6cb82c725 88 scope.set(1,out_p);
vsluiter 8:15c6cb82c725 89 scope.set(2,out_i);
vsluiter 8:15c6cb82c725 90 scope.set(3,out_d);
Hooglugt 11:4f65e70290ac 91 return out_p + out_i + out_d;
vsluiter 0:c9e647421e54 92 }
Hooglugt 11:4f65e70290ac 93 */
vsluiter 0:c9e647421e54 94