thijs ruikes / Mbed 2 deprecated PROJECT-MOTORSPEEDCONTROLLER

ding

Dependencies:   Encoder mbed

main.cpp@2:44cc689e14d6, 2014-10-29 (annotated)

Committer:
wiesdat
Date:
Wed Oct 29 13:39:26 2014 +0000
Revision:
2:44cc689e14d6
Parent:
1:5ac17a29fa08 
Motorspeedcontrol: I regelaar, input case

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 2:44cc689e14d6 14 int y1;
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 2:44cc689e14d6 23
wiesdat 1:5ac17a29fa08 24 void clamp(float * in, float min, float max)
wiesdat 1:5ac17a29fa08 25 {
wiesdat 1:5ac17a29fa08 26 *in > min ? *in < max? : *in = max: *in = min;
wiesdat 1:5ac17a29fa08 27 }
wiesdat 1:5ac17a29fa08 28
wiesdat 1:5ac17a29fa08 29
wiesdat 0:e89ba43e3f05 30
wiesdat 0:e89ba43e3f05 31 float pid(float setpoint, float measurement)
wiesdat 0:e89ba43e3f05 32 {
wiesdat 0:e89ba43e3f05 33 float error;
wiesdat 0:e89ba43e3f05 34 static float prev_error = 0;
wiesdat 0:e89ba43e3f05 35 float out_p = 0;
wiesdat 0:e89ba43e3f05 36 static float out_i = 0;
wiesdat 0:e89ba43e3f05 37 float out_d = 0;
wiesdat 0:e89ba43e3f05 38 error = setpoint-measurement;
wiesdat 0:e89ba43e3f05 39 out_p = error*K_P;
wiesdat 0:e89ba43e3f05 40 out_i += error*K_I;
wiesdat 0:e89ba43e3f05 41 out_d = (error-prev_error)*K_D;
wiesdat 0:e89ba43e3f05 42 clamp(&out_i,-I_LIMIT,I_LIMIT);
wiesdat 0:e89ba43e3f05 43 prev_error = error;
wiesdat 1:5ac17a29fa08 44 out = out_i;
wiesdat 1:5ac17a29fa08 45 clamp(&out, -1,1);
wiesdat 1:5ac17a29fa08 46 out = fabs(out);
wiesdat 1:5ac17a29fa08 47 return out;
wiesdat 0:e89ba43e3f05 48 }
wiesdat 1:5ac17a29fa08 49
wiesdat 1:5ac17a29fa08 50 float getv()
wiesdat 1:5ac17a29fa08 51 {
wiesdat 1:5ac17a29fa08 52 while(n<3) {
wiesdat 1:5ac17a29fa08 53 wait(0.2);
wiesdat 1:5ac17a29fa08 54 enca = encoderA.getPosition();
wiesdat 1:5ac17a29fa08 55 enca2 = enca1;
wiesdat 1:5ac17a29fa08 56 enca1 = enca;
wiesdat 1:5ac17a29fa08 57 n++;
wiesdat 1:5ac17a29fa08 58 cout<<n<<endl;
wiesdat 1:5ac17a29fa08 59 }
wiesdat 1:5ac17a29fa08 60
wiesdat 1:5ac17a29fa08 61 n =0 ;
wiesdat 1:5ac17a29fa08 62 counts = (enca1 - enca2)/0.4;
wiesdat 1:5ac17a29fa08 63 cout<<"counts: "<<counts<<endl;
wiesdat 1:5ac17a29fa08 64 v = (counts)*((2*3.14159265359)/1550);
wiesdat 1:5ac17a29fa08 65 return v;
wiesdat 1:5ac17a29fa08 66 }
wiesdat 2:44cc689e14d6 67
wiesdat 2:44cc689e14d6 68 float speedcontrol()
wiesdat 2:44cc689e14d6 69 {
wiesdat 2:44cc689e14d6 70
wiesdat 2:44cc689e14d6 71 v = getv();
wiesdat 2:44cc689e14d6 72 cout<<"v: "<<v<<endl;
wiesdat 2:44cc689e14d6 73 out = pid(v_ref,v);
wiesdat 2:44cc689e14d6 74 cout<<"out: "<<out<<endl;
wiesdat 2:44cc689e14d6 75 pwm = out;
wiesdat 2:44cc689e14d6 76 }
wiesdat 2:44cc689e14d6 77
wiesdat 1:5ac17a29fa08 78 int main()
wiesdat 1:5ac17a29fa08 79 {
wiesdat 1:5ac17a29fa08 80 dir = 1;
wiesdat 2:44cc689e14d6 81 cout<<"typ case: "<<endl;
wiesdat 2:44cc689e14d6 82 cin>>y1;
wiesdat 2:44cc689e14d6 83 cout<<"case: "<<y1<<endl;
wiesdat 2:44cc689e14d6 84
wiesdat 2:44cc689e14d6 85
wiesdat 1:5ac17a29fa08 86 while(1) {
wiesdat 2:44cc689e14d6 87 switch(y1) {
wiesdat 2:44cc689e14d6 88 case 0 :
wiesdat 2:44cc689e14d6 89 v_ref = 1;
wiesdat 2:44cc689e14d6 90 break;
wiesdat 2:44cc689e14d6 91 case 1 :
wiesdat 2:44cc689e14d6 92 v_ref = 2;
wiesdat 2:44cc689e14d6 93 break;
wiesdat 2:44cc689e14d6 94 case 2 :
wiesdat 2:44cc689e14d6 95 v_ref = 3;
wiesdat 2:44cc689e14d6 96 break;
wiesdat 2:44cc689e14d6 97 }
wiesdat 2:44cc689e14d6 98
wiesdat 2:44cc689e14d6 99 speedcontrol();
wiesdat 2:44cc689e14d6 100
wiesdat 1:5ac17a29fa08 101 }
wiesdat 1:5ac17a29fa08 102 }
wiesdat 1:5ac17a29fa08 103
wiesdat 1:5ac17a29fa08 104
wiesdat 0:e89ba43e3f05 105