thijs ruikes / Mbed 2 deprecated PROJECT-MOTORSPEEDCONTROLLER

ding

Dependencies:   Encoder mbed

main.cpp@1:5ac17a29fa08, 2014-10-29 (annotated)

Committer:
wiesdat
Date:
Wed Oct 29 13:25:18 2014 +0000
Revision:
1:5ac17a29fa08
Parent:
0:e89ba43e3f05
Child:
2:44cc689e14d6
PI regelaar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wiesdat 0:e89ba43e3f05 1 #include "mbed.h"
wiesdat 0:e89ba43e3f05 2 #include "encoder.h"
wiesdat 1:5ac17a29fa08 3 #define K_P (0.1)
wiesdat 1:5ac17a29fa08 4 #define K_I (0.1)
wiesdat 1:5ac17a29fa08 5 #define K_D (0.0005 /TSAMP)
wiesdat 1:5ac17a29fa08 6 #define TSAMP 0.001
wiesdat 1:5ac17a29fa08 7 #define I_LIMIT 1.
wiesdat 0:e89ba43e3f05 8 #include <iostream>
wiesdat 0:e89ba43e3f05 9
wiesdat 0:e89ba43e3f05 10 Encoder encoderA(PTD0,PTD2);
wiesdat 0:e89ba43e3f05 11
wiesdat 0:e89ba43e3f05 12 PwmOut pwm(PTA5);
wiesdat 0:e89ba43e3f05 13 DigitalOut dir(PTA4);
wiesdat 0:e89ba43e3f05 14
wiesdat 0:e89ba43e3f05 15
wiesdat 1:5ac17a29fa08 16
wiesdat 1:5ac17a29fa08 17
wiesdat 0:e89ba43e3f05 18 int32_t enca = 0,enca1 =0,enca2 =0,counts=0, encb;
wiesdat 1:5ac17a29fa08 19 float speed = 0,v=0,v_ref=0;
wiesdat 0:e89ba43e3f05 20 int n=0,a =1 ,b =0;
wiesdat 1:5ac17a29fa08 21 float out;
wiesdat 1:5ac17a29fa08 22
wiesdat 1:5ac17a29fa08 23 //clamps value 'in' to min or max when exceeding those values
wiesdat 1:5ac17a29fa08 24 //if you'd like to understand the statement below take a google for
wiesdat 1:5ac17a29fa08 25 //'ternary operators'.
wiesdat 1:5ac17a29fa08 26 void clamp(float * in, float min, float max)
wiesdat 1:5ac17a29fa08 27 {
wiesdat 1:5ac17a29fa08 28 *in > min ? *in < max? : *in = max: *in = min;
wiesdat 1:5ac17a29fa08 29 }
wiesdat 1:5ac17a29fa08 30
wiesdat 1:5ac17a29fa08 31
wiesdat 0:e89ba43e3f05 32
wiesdat 0:e89ba43e3f05 33 float pid(float setpoint, float measurement)
wiesdat 0:e89ba43e3f05 34 {
wiesdat 0:e89ba43e3f05 35 float error;
wiesdat 0:e89ba43e3f05 36 static float prev_error = 0;
wiesdat 0:e89ba43e3f05 37 float out_p = 0;
wiesdat 0:e89ba43e3f05 38 static float out_i = 0;
wiesdat 0:e89ba43e3f05 39 float out_d = 0;
wiesdat 0:e89ba43e3f05 40 error = setpoint-measurement;
wiesdat 0:e89ba43e3f05 41 out_p = error*K_P;
wiesdat 0:e89ba43e3f05 42 out_i += error*K_I;
wiesdat 0:e89ba43e3f05 43 out_d = (error-prev_error)*K_D;
wiesdat 0:e89ba43e3f05 44 clamp(&out_i,-I_LIMIT,I_LIMIT);
wiesdat 0:e89ba43e3f05 45 prev_error = error;
wiesdat 1:5ac17a29fa08 46 out = out_i;
wiesdat 1:5ac17a29fa08 47 clamp(&out, -1,1);
wiesdat 1:5ac17a29fa08 48 out = fabs(out);
wiesdat 1:5ac17a29fa08 49 return out;
wiesdat 0:e89ba43e3f05 50 }
wiesdat 1:5ac17a29fa08 51
wiesdat 1:5ac17a29fa08 52 float getv()
wiesdat 1:5ac17a29fa08 53 {
wiesdat 1:5ac17a29fa08 54 while(n<3) {
wiesdat 1:5ac17a29fa08 55 wait(0.2);
wiesdat 1:5ac17a29fa08 56 enca = encoderA.getPosition();
wiesdat 1:5ac17a29fa08 57 enca2 = enca1;
wiesdat 1:5ac17a29fa08 58 enca1 = enca;
wiesdat 1:5ac17a29fa08 59 n++;
wiesdat 1:5ac17a29fa08 60 cout<<n<<endl;
wiesdat 1:5ac17a29fa08 61 }
wiesdat 1:5ac17a29fa08 62
wiesdat 1:5ac17a29fa08 63 n =0 ;
wiesdat 1:5ac17a29fa08 64 counts = (enca1 - enca2)/0.4;
wiesdat 1:5ac17a29fa08 65 cout<<"counts: "<<counts<<endl;
wiesdat 1:5ac17a29fa08 66 v = (counts)*((2*3.14159265359)/1550);
wiesdat 1:5ac17a29fa08 67 return v;
wiesdat 1:5ac17a29fa08 68 }
wiesdat 1:5ac17a29fa08 69 int main()
wiesdat 1:5ac17a29fa08 70 {
wiesdat 1:5ac17a29fa08 71 dir = 1;
wiesdat 1:5ac17a29fa08 72 cout<<"typ v_ref: "<<endl;
wiesdat 1:5ac17a29fa08 73 cin>>v_ref;
wiesdat 1:5ac17a29fa08 74 cout<<"v_ref: "<<v_ref<<endl;
wiesdat 1:5ac17a29fa08 75 while(1) {
wiesdat 1:5ac17a29fa08 76 v = getv();
wiesdat 1:5ac17a29fa08 77 cout<<"v: "<<v<<endl;
wiesdat 1:5ac17a29fa08 78 out = pid(v_ref,v);
wiesdat 1:5ac17a29fa08 79 cout<<"out: "<<out<<endl;
wiesdat 1:5ac17a29fa08 80 pwm = out;
wiesdat 1:5ac17a29fa08 81 }
wiesdat 1:5ac17a29fa08 82 }
wiesdat 1:5ac17a29fa08 83
wiesdat 1:5ac17a29fa08 84
wiesdat 0:e89ba43e3f05 85 /*
wiesdat 0:e89ba43e3f05 86 int main()
wiesdat 0:e89ba43e3f05 87 {
wiesdat 0:e89ba43e3f05 88 speed = 0.09;
wiesdat 0:e89ba43e3f05 89 pwm = speed;
wiesdat 0:e89ba43e3f05 90 while(1){
wiesdat 0:e89ba43e3f05 91 wait(0.2);
wiesdat 0:e89ba43e3f05 92 enca = encoderA.getPosition();
wiesdat 0:e89ba43e3f05 93 cout<<enca<<endl;
wiesdat 0:e89ba43e3f05 94 }
wiesdat 0:e89ba43e3f05 95 }
wiesdat 0:e89ba43e3f05 96 */
wiesdat 0:e89ba43e3f05 97
wiesdat 0:e89ba43e3f05 98