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@12:02abb60385c8, 2014-10-16 (annotated)

Committer:
Hooglugt
Date:
Thu Oct 16 10:41:21 2014 +0000
Revision:
12:02abb60385c8
Parent:
11:4f65e70290ac 
stukje code toegevoegd voor het uitprinten van de derivative van de motor

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"
Hooglugt 12:02abb60385c8 4 #include "MODSERIAL.h" //voor print realterm
vsluiter 6:0832c6c6bcba 5
vsluiter 5:06381e54b94a 6 #define TSAMP 0.01
vsluiter 10:6f8af13cc3f4 7 #define K_P (0.01)
vsluiter 10:6f8af13cc3f4 8 #define K_I (0 *TSAMP)
vsluiter 9:e09d81850a05 9 #define K_D (0 /TSAMP)
vsluiter 8:15c6cb82c725 10 #define I_LIMIT 1.
vsluiter 1:5ac85aad9da4 11
vsluiter 4:1a53b06eeb7f 12 #define POT_AVG 50
vsluiter 6:0832c6c6bcba 13
vsluiter 8:15c6cb82c725 14 void clamp(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];
Hooglugt 11:4f65e70290ac 19 HIDScope scope(2);
vsluiter 6:0832c6c6bcba 20
Hooglugt 12:02abb60385c8 21 MODSERIAL pc(USBTX,USBRX); //voor print realterm
Hooglugt 12:02abb60385c8 22
vsluiter 0:c9e647421e54 23
vsluiter 0:c9e647421e54 24 void setlooptimerflag(void)
vsluiter 0:c9e647421e54 25 {
vsluiter 0:c9e647421e54 26 looptimerflag = true;
vsluiter 0:c9e647421e54 27 }
vsluiter 0:c9e647421e54 28
vsluiter 1:5ac85aad9da4 29
vsluiter 1:5ac85aad9da4 30
vsluiter 4:1a53b06eeb7f 31 int main()
vsluiter 4:1a53b06eeb7f 32 {
Hooglugt 12:02abb60385c8 33 pc.baud(115200); //baudrate instellen voor realterm print
vsluiter 6:0832c6c6bcba 34 //start Encoder
vsluiter 0:c9e647421e54 35 Encoder motor1(PTD0,PTC9);
Hooglugt 11:4f65e70290ac 36 // @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 37 // @param int_b second encoder pin, used as DigitalIn. Can be any DigitalIn pin, not necessarily on InterruptIn location
vsluiter 7:37b06688b449 38 /*PwmOut to motor driver*/
vsluiter 7:37b06688b449 39 PwmOut pwm_motor(PTA5);
vsluiter 6:0832c6c6bcba 40 //10kHz PWM frequency
vsluiter 6:0832c6c6bcba 41 pwm_motor.period_us(100);
vsluiter 7:37b06688b449 42 DigitalOut motordir(PTD1);
vsluiter 0:c9e647421e54 43 Ticker looptimer;
vsluiter 5:06381e54b94a 44 looptimer.attach(setlooptimerflag,TSAMP);
Hooglugt 12:02abb60385c8 45 float der, prevpos, stop;
Hooglugt 12:02abb60385c8 46 der = 0; //derivative
Hooglugt 12:02abb60385c8 47 prevpos = 0; //previous result of motor1.getPosition()
Hooglugt 12:02abb60385c8 48 stop = 0; //after x amount of whiles while loop stops
Hooglugt 12:02abb60385c8 49 wait(20);
vsluiter 0:c9e647421e54 50 while(1) {
vsluiter 1:5ac85aad9da4 51 float setpoint;
Hooglugt 11:4f65e70290ac 52 float new_pwm;
vsluiter 0:c9e647421e54 53 while(!looptimerflag);
vsluiter 8:15c6cb82c725 54 looptimerflag = false; //clear flag
Hooglugt 11:4f65e70290ac 55 //potmeter value: 0-1
Hooglugt 11:4f65e70290ac 56 //setpoint = (potmeter.read()-.5)*500;
Hooglugt 11:4f65e70290ac 57 //new_pwm = (setpoint - motor1.getPosition())*.001; -> P action
Hooglugt 12:02abb60385c8 58 new_pwm = -1; // DIT IS PUUR VOOR DE STEPFUNCTION
Hooglugt 11:4f65e70290ac 59 //new_pwm = pid(setpoint, motor1.getPosition());
vsluiter 8:15c6cb82c725 60 clamp(&new_pwm, -1,1);
Hooglugt 11:4f65e70290ac 61 //scope.set(0, setpoint);
Hooglugt 12:02abb60385c8 62 scope.set(0, new_pwm);
Hooglugt 12:02abb60385c8 63 scope.set(1, motor1.getPosition());
Hooglugt 11:4f65e70290ac 64 // ch 1, 2 and 3 set in pid controller
vsluiter 6:0832c6c6bcba 65 scope.send();
vsluiter 0:c9e647421e54 66 if(new_pwm > 0)
Hooglugt 12:02abb60385c8 67 motordir = 0; // links 25D motor
vsluiter 7:37b06688b449 68 else
Hooglugt 12:02abb60385c8 69 motordir = 1; // rechts 25D motor
vsluiter 0:c9e647421e54 70 pwm_motor.write(abs(new_pwm));
Hooglugt 12:02abb60385c8 71
Hooglugt 12:02abb60385c8 72 der = (motor1.getPosition() - prevpos)/TSAMP;
Hooglugt 12:02abb60385c8 73 pc.printf("%f \n", der);
Hooglugt 12:02abb60385c8 74 prevpos = motor1.getPosition();
Hooglugt 12:02abb60385c8 75 stop++;
Hooglugt 12:02abb60385c8 76 if (stop>500) {
Hooglugt 12:02abb60385c8 77 break;
Hooglugt 12:02abb60385c8 78 }
vsluiter 0:c9e647421e54 79 }
vsluiter 0:c9e647421e54 80 }
vsluiter 0:c9e647421e54 81
vsluiter 1:5ac85aad9da4 82
vsluiter 8:15c6cb82c725 83 //clamps value 'in' to min or max when exceeding those values
vsluiter 1:5ac85aad9da4 84 //if you'd like to understand the statement below take a google for
vsluiter 1:5ac85aad9da4 85 //'ternary operators'.
vsluiter 8:15c6cb82c725 86 void clamp(float * in, float min, float max)
vsluiter 0:c9e647421e54 87 {
vsluiter 6:0832c6c6bcba 88 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:c9e647421e54 89 }
vsluiter 0:c9e647421e54 90
Hooglugt 11:4f65e70290ac 91 /*
vsluiter 0:c9e647421e54 92 float pid(float setpoint, float measurement)
vsluiter 0:c9e647421e54 93 {
vsluiter 4:1a53b06eeb7f 94 float error;
vsluiter 4:1a53b06eeb7f 95 static float prev_error = 0;
vsluiter 4:1a53b06eeb7f 96 float out_p = 0;
vsluiter 4:1a53b06eeb7f 97 static float out_i = 0;
vsluiter 4:1a53b06eeb7f 98 float out_d = 0;
vsluiter 4:1a53b06eeb7f 99 error = setpoint-measurement;
vsluiter 4:1a53b06eeb7f 100 out_p = error*K_P;
vsluiter 4:1a53b06eeb7f 101 out_i += error*K_I;
vsluiter 4:1a53b06eeb7f 102 out_d = (error-prev_error)*K_D;
vsluiter 8:15c6cb82c725 103 clamp(&out_i,-I_LIMIT,I_LIMIT);
vsluiter 4:1a53b06eeb7f 104 prev_error = error;
vsluiter 8:15c6cb82c725 105 scope.set(1,out_p);
vsluiter 8:15c6cb82c725 106 scope.set(2,out_i);
vsluiter 8:15c6cb82c725 107 scope.set(3,out_d);
Hooglugt 11:4f65e70290ac 108 return out_p + out_i + out_d;
vsluiter 0:c9e647421e54 109 }
Hooglugt 11:4f65e70290ac 110 */
vsluiter 0:c9e647421e54 111