Marijke Abma / Mbed 2 deprecated motor1aansturing201014

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 MODSERIAL mbed-dsp mbed

Fork of motor1aansturing by BMT M9 Groep01

main.cpp@3:864137a5f702, 2014-10-03 (annotated)

Committer:
Jolein
Date:
Fri Oct 03 11:10:46 2014 +0000
Revision:
3:864137a5f702
Parent:
2:ef0fa691e77e
Child:
4:2a3b8c562f4a
added comments;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jolein 0:eb00992c1597 1 #include "mbed.h"
Jolein 0:eb00992c1597 2 #include "encoder.h"
Jolein 0:eb00992c1597 3 #define TSAMP 0.01
Jolein 0:eb00992c1597 4 #define K_P (0.01)
Jolein 0:eb00992c1597 5 #define K_I (0 *TSAMP)
Jolein 2:ef0fa691e77e 6 #define K_D (0 /TSAMP)
Jolein 0:eb00992c1597 7 #define I_LIMIT 1.
Jolein 0:eb00992c1597 8
Jolein 0:eb00992c1597 9 #define POT_AVG 50
Jolein 0:eb00992c1597 10
Jolein 0:eb00992c1597 11 void clamp(float * in, float min, float max);
Jolein 0:eb00992c1597 12 float pid(float setpoint, float measurement);
Jolein 2:ef0fa691e77e 13 //-------------------------------------------------------------------------------------------Input potentiometer-------------------------------
Jolein 0:eb00992c1597 14 AnalogIn potmeter(PTC2);
Jolein 0:eb00992c1597 15 volatile bool looptimerflag;
Jolein 0:eb00992c1597 16 float potsamples[POT_AVG];
Jolein 0:eb00992c1597 17
Jolein 0:eb00992c1597 18
Jolein 0:eb00992c1597 19 void setlooptimerflag(void)
Jolein 0:eb00992c1597 20 {
Jolein 0:eb00992c1597 21 looptimerflag = true;
Jolein 0:eb00992c1597 22 }
Jolein 0:eb00992c1597 23
Jolein 0:eb00992c1597 24 int main()
Jolein 0:eb00992c1597 25 {
Jolein 0:eb00992c1597 26 Encoder motor1(PTD0,PTC9);
Jolein 0:eb00992c1597 27 PwmOut pwm_motor(PTA5);
Jolein 0:eb00992c1597 28 pwm_motor.period_us(100);
Jolein 0:eb00992c1597 29 DigitalOut motordir(PTD1);
Jolein 0:eb00992c1597 30 Ticker looptimer;
Jolein 0:eb00992c1597 31 looptimer.attach(setlooptimerflag,TSAMP);
Jolein 0:eb00992c1597 32 while(1) {
Jolein 0:eb00992c1597 33 float setpoint;
Jolein 0:eb00992c1597 34 float new_pwm;
Jolein 0:eb00992c1597 35 while(!looptimerflag);
Jolein 2:ef0fa691e77e 36 looptimerflag = false;
Jolein 3:864137a5f702 37 //---------------------------------------------------------leest potentiometer (?), schaalt tussen 0 en 1-------------------------------------
Jolein 2:ef0fa691e77e 38 setpoint = (potmeter.read()-.5)*500;
Jolein 3:864137a5f702 39 //----------------------------------------------------------------------new_pwm = getal?.....-------------------------------------------------
Jolein 0:eb00992c1597 40 new_pwm = pid(setpoint, motor1.getPosition());
Jolein 0:eb00992c1597 41 clamp(&new_pwm, -1,1);
Jolein 2:ef0fa691e77e 42 //-------------------------------------------------------------------------------------output motor richting----------------------------------
Jolein 0:eb00992c1597 43 if(new_pwm > 0)
Jolein 0:eb00992c1597 44 motordir = 0;
Jolein 0:eb00992c1597 45 else
Jolein 0:eb00992c1597 46 motordir = 1;
Jolein 2:ef0fa691e77e 47 //-------------------------------------------------------------------------------output motorsnelhied (wij willen 4 distinctive stappen)------
Jolein 0:eb00992c1597 48 pwm_motor.write(abs(new_pwm));
Jolein 0:eb00992c1597 49 }
Jolein 0:eb00992c1597 50 }
Jolein 0:eb00992c1597 51
Jolein 0:eb00992c1597 52 void clamp(float * in, float min, float max)
Jolein 0:eb00992c1597 53 {
Jolein 0:eb00992c1597 54 *in > min ? *in < max? : *in = max: *in = min;
Jolein 0:eb00992c1597 55 }
Jolein 0:eb00992c1597 56
Jolein 0:eb00992c1597 57
Jolein 0:eb00992c1597 58 float pid(float setpoint, float measurement)
Jolein 0:eb00992c1597 59 {
Jolein 0:eb00992c1597 60 float error;
Jolein 0:eb00992c1597 61 static float prev_error = 0;
Jolein 0:eb00992c1597 62 float out_p = 0;
Jolein 0:eb00992c1597 63 static float out_i = 0;
Jolein 0:eb00992c1597 64 float out_d = 0;
Jolein 0:eb00992c1597 65 error = setpoint-measurement;
Jolein 0:eb00992c1597 66 out_p = error*K_P;
Jolein 0:eb00992c1597 67 out_i += error*K_I;
Jolein 0:eb00992c1597 68 out_d = (error-prev_error)*K_D;
Jolein 0:eb00992c1597 69 clamp(&out_i,-I_LIMIT,I_LIMIT);
Jolein 0:eb00992c1597 70 prev_error = error;
Jolein 0:eb00992c1597 71 return out_p + out_i + out_d;
Jolein 0:eb00992c1597 72 }