thijs ruikes / Mbed 2 deprecated PROJECT-MOTORSPEEDCONTROLLER

ding

Dependencies:   Encoder mbed

main.cpp@0:e89ba43e3f05, 2014-10-29 (annotated)

Committer:
wiesdat
Date:
Wed Oct 29 11:33:37 2014 +0000
Revision:
0:e89ba43e3f05
Child:
1:5ac17a29fa08
dign

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