Marijke Abma / Mbed 2 deprecated motor1aansturing201014

Programma om motor 1 aan te sturen

Dependencies:   Encoder MODSERIAL mbed-dsp mbed

Fork of motor1aansturing by BMT M9 Groep01

main.cpp@0:eb00992c1597, 2014-10-03 (annotated)

Committer:
Jolein
Date:
Fri Oct 03 08:56:01 2014 +0000
Revision:
0:eb00992c1597
Child:
2:ef0fa691e77e
vsluiters originele script

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 #include "HIDScope.h"
Jolein 0:eb00992c1597 4
Jolein 0:eb00992c1597 5 #define TSAMP 0.01
Jolein 0:eb00992c1597 6 #define K_P (0.01)
Jolein 0:eb00992c1597 7 #define K_I (0 *TSAMP)
Jolein 0:eb00992c1597 8 #define K_D (0 /TSAMP)
Jolein 0:eb00992c1597 9 #define I_LIMIT 1.
Jolein 0:eb00992c1597 10
Jolein 0:eb00992c1597 11 #define POT_AVG 50
Jolein 0:eb00992c1597 12
Jolein 0:eb00992c1597 13 void clamp(float * in, float min, float max);
Jolein 0:eb00992c1597 14 float pid(float setpoint, float measurement);
Jolein 0:eb00992c1597 15 AnalogIn potmeter(PTC2);
Jolein 0:eb00992c1597 16 volatile bool looptimerflag;
Jolein 0:eb00992c1597 17 float potsamples[POT_AVG];
Jolein 0:eb00992c1597 18 HIDScope scope(6);
Jolein 0:eb00992c1597 19
Jolein 0:eb00992c1597 20
Jolein 0:eb00992c1597 21 void setlooptimerflag(void)
Jolein 0:eb00992c1597 22 {
Jolein 0:eb00992c1597 23 looptimerflag = true;
Jolein 0:eb00992c1597 24 }
Jolein 0:eb00992c1597 25
Jolein 0:eb00992c1597 26 int main()
Jolein 0:eb00992c1597 27 {
Jolein 0:eb00992c1597 28 //start Encoder
Jolein 0:eb00992c1597 29 Encoder motor1(PTD0,PTC9);
Jolein 0:eb00992c1597 30 /*PwmOut to motor driver*/
Jolein 0:eb00992c1597 31 PwmOut pwm_motor(PTA5);
Jolein 0:eb00992c1597 32 //10kHz PWM frequency
Jolein 0:eb00992c1597 33 pwm_motor.period_us(100);
Jolein 0:eb00992c1597 34 DigitalOut motordir(PTD1);
Jolein 0:eb00992c1597 35 Ticker looptimer;
Jolein 0:eb00992c1597 36 looptimer.attach(setlooptimerflag,TSAMP);
Jolein 0:eb00992c1597 37 while(1) {
Jolein 0:eb00992c1597 38 float setpoint;
Jolein 0:eb00992c1597 39 float new_pwm;
Jolein 0:eb00992c1597 40 /*wait until timer has elapsed*/
Jolein 0:eb00992c1597 41 while(!looptimerflag);
Jolein 0:eb00992c1597 42 looptimerflag = false; //clear flag
Jolein 0:eb00992c1597 43 /*potmeter value: 0-1*/
Jolein 0:eb00992c1597 44 setpoint = (potmeter.read()-.5)*500;
Jolein 0:eb00992c1597 45 /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
Jolein 0:eb00992c1597 46 new_pwm = pid(setpoint, motor1.getPosition());
Jolein 0:eb00992c1597 47 clamp(&new_pwm, -1,1);
Jolein 0:eb00992c1597 48 scope.set(0, setpoint);
Jolein 0:eb00992c1597 49 scope.set(4, new_pwm);
Jolein 0:eb00992c1597 50 scope.set(5, motor1.getPosition());
Jolein 0:eb00992c1597 51 // ch 1, 2 and 3 set in pid controller */
Jolein 0:eb00992c1597 52 scope.send();
Jolein 0:eb00992c1597 53 if(new_pwm > 0)
Jolein 0:eb00992c1597 54 motordir = 0;
Jolein 0:eb00992c1597 55 else
Jolein 0:eb00992c1597 56 motordir = 1;
Jolein 0:eb00992c1597 57 pwm_motor.write(abs(new_pwm));
Jolein 0:eb00992c1597 58 }
Jolein 0:eb00992c1597 59 }
Jolein 0:eb00992c1597 60
Jolein 0:eb00992c1597 61
Jolein 0:eb00992c1597 62 //clamps value 'in' to min or max when exceeding those values
Jolein 0:eb00992c1597 63 //if you'd like to understand the statement below take a google for
Jolein 0:eb00992c1597 64 //'ternary operators'.
Jolein 0:eb00992c1597 65 void clamp(float * in, float min, float max)
Jolein 0:eb00992c1597 66 {
Jolein 0:eb00992c1597 67 *in > min ? *in < max? : *in = max: *in = min;
Jolein 0:eb00992c1597 68 }
Jolein 0:eb00992c1597 69
Jolein 0:eb00992c1597 70
Jolein 0:eb00992c1597 71 float pid(float setpoint, float measurement)
Jolein 0:eb00992c1597 72 {
Jolein 0:eb00992c1597 73 float error;
Jolein 0:eb00992c1597 74 static float prev_error = 0;
Jolein 0:eb00992c1597 75 float out_p = 0;
Jolein 0:eb00992c1597 76 static float out_i = 0;
Jolein 0:eb00992c1597 77 float out_d = 0;
Jolein 0:eb00992c1597 78 error = setpoint-measurement;
Jolein 0:eb00992c1597 79 out_p = error*K_P;
Jolein 0:eb00992c1597 80 out_i += error*K_I;
Jolein 0:eb00992c1597 81 out_d = (error-prev_error)*K_D;
Jolein 0:eb00992c1597 82 clamp(&out_i,-I_LIMIT,I_LIMIT);
Jolein 0:eb00992c1597 83 prev_error = error;
Jolein 0:eb00992c1597 84 scope.set(1,out_p);
Jolein 0:eb00992c1597 85 scope.set(2,out_i);
Jolein 0:eb00992c1597 86 scope.set(3,out_d);
Jolein 0:eb00992c1597 87 return out_p + out_i + out_d;
Jolein 0:eb00992c1597 88 }