thijs ruikes / Mbed 2 deprecated PROJECT-MOTORCONTROL

ding

Dependencies:   Encoder HIDScope mbed

main.cpp@4:055913d9c737, 2014-10-29 (annotated)

Committer:
wiesdat
Date:
Wed Oct 29 16:45:46 2014 +0000
Revision:
4:055913d9c737
Parent:
3:8a8dc30fb08e
Child:
5:93ccec91a4ae
Werkende? positie regelaar, positie in encoder bits

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wiesdat 0:7e81becf02c6 1 #include "mbed.h"
wiesdat 0:7e81becf02c6 2 #include "encoder.h"
wiesdat 2:ca2573919cb5 3
wiesdat 3:8a8dc30fb08e 4 #define K_P (0.1)
wiesdat 4:055913d9c737 5 #define K_I (0.00001)
wiesdat 3:8a8dc30fb08e 6 #define K_D (0.0005 /TSAMP)
wiesdat 3:8a8dc30fb08e 7 #define TSAMP 0.001
wiesdat 3:8a8dc30fb08e 8 #define I_LIMIT 1.
wiesdat 1:1709bd849df0 9 #include <iostream>
wiesdat 0:7e81becf02c6 10
wiesdat 1:1709bd849df0 11 Encoder encoderA(PTD0,PTD2);
wiesdat 1:1709bd849df0 12
wiesdat 0:7e81becf02c6 13 PwmOut pwm(PTA5);
wiesdat 0:7e81becf02c6 14 DigitalOut dir(PTA4);
wiesdat 2:ca2573919cb5 15
wiesdat 2:ca2573919cb5 16
wiesdat 3:8a8dc30fb08e 17 int32_t enc = 0,enca2 =0,enca1=0, encp=0, counts =0;
wiesdat 3:8a8dc30fb08e 18 float speed = 0.1, out =0;
wiesdat 3:8a8dc30fb08e 19 int pos =0,zero =0, fout;
wiesdat 2:ca2573919cb5 20
wiesdat 2:ca2573919cb5 21
wiesdat 3:8a8dc30fb08e 22 void clamp(float * in, float min, float max)
wiesdat 3:8a8dc30fb08e 23 {
wiesdat 3:8a8dc30fb08e 24 *in > min ? *in < max? : *in = max: *in = min;
wiesdat 0:7e81becf02c6 25 }
wiesdat 0:7e81becf02c6 26
wiesdat 0:7e81becf02c6 27
wiesdat 2:ca2573919cb5 28
wiesdat 3:8a8dc30fb08e 29 float pid(float setpoint, float measurement)
wiesdat 3:8a8dc30fb08e 30 {
wiesdat 3:8a8dc30fb08e 31 float error;
wiesdat 3:8a8dc30fb08e 32 static float prev_error = 0;
wiesdat 3:8a8dc30fb08e 33 float out_p = 0;
wiesdat 3:8a8dc30fb08e 34 static float out_i = 0;
wiesdat 3:8a8dc30fb08e 35 float out_d = 0;
wiesdat 3:8a8dc30fb08e 36 error = setpoint-measurement;
wiesdat 3:8a8dc30fb08e 37 out_p = error*K_P;
wiesdat 3:8a8dc30fb08e 38 out_i += error*K_I;
wiesdat 3:8a8dc30fb08e 39 out_d = (error-prev_error)*K_D;
wiesdat 3:8a8dc30fb08e 40 clamp(&out_i,-I_LIMIT,I_LIMIT);
wiesdat 3:8a8dc30fb08e 41 prev_error = error;
wiesdat 3:8a8dc30fb08e 42 out = out_i;
wiesdat 4:055913d9c737 43
wiesdat 3:8a8dc30fb08e 44 return out;
wiesdat 3:8a8dc30fb08e 45 }
wiesdat 3:8a8dc30fb08e 46
wiesdat 3:8a8dc30fb08e 47 float getv(float delta_t)
wiesdat 3:8a8dc30fb08e 48 {
wiesdat 3:8a8dc30fb08e 49 float v;
wiesdat 3:8a8dc30fb08e 50 int n =0 ;
wiesdat 3:8a8dc30fb08e 51 while(n<3) {
wiesdat 3:8a8dc30fb08e 52 wait(delta_t);
wiesdat 3:8a8dc30fb08e 53 enc = encoderA.getPosition();
wiesdat 3:8a8dc30fb08e 54 enca2 = enca1;
wiesdat 3:8a8dc30fb08e 55 enca1 = enc;
wiesdat 3:8a8dc30fb08e 56 n++;
wiesdat 4:055913d9c737 57
wiesdat 3:8a8dc30fb08e 58 }
wiesdat 4:055913d9c737 59
wiesdat 3:8a8dc30fb08e 60 counts = (enca1 - enca2)/delta_t;
wiesdat 3:8a8dc30fb08e 61 v = (counts)*((2*3.14159265359)/1550);
wiesdat 3:8a8dc30fb08e 62 return v;
wiesdat 3:8a8dc30fb08e 63 }
wiesdat 3:8a8dc30fb08e 64
wiesdat 3:8a8dc30fb08e 65
wiesdat 3:8a8dc30fb08e 66
wiesdat 3:8a8dc30fb08e 67 int main()
wiesdat 4:055913d9c737 68 {
wiesdat 4:055913d9c737 69 while(1) {
wiesdat 3:8a8dc30fb08e 70 float velocity =1;
wiesdat 4:055913d9c737 71 while(velocity !=0) {
wiesdat 4:055913d9c737 72
wiesdat 4:055913d9c737 73 dir = 0;
wiesdat 4:055913d9c737 74 pwm.write(0.1);
wiesdat 4:055913d9c737 75 velocity =getv(0.2);
wiesdat 3:8a8dc30fb08e 76 }
wiesdat 3:8a8dc30fb08e 77 pwm = 0;
wiesdat 3:8a8dc30fb08e 78 dir =1;
wiesdat 4:055913d9c737 79
wiesdat 3:8a8dc30fb08e 80 encoderA.setPosition(0);
wiesdat 3:8a8dc30fb08e 81 zero = encoderA.getPosition();
wiesdat 3:8a8dc30fb08e 82 cout<<"zero encoder: "<<zero<<endl;
wiesdat 4:055913d9c737 83
wiesdat 3:8a8dc30fb08e 84 cout<<"positie in encoder bits: "<<endl;
wiesdat 3:8a8dc30fb08e 85 cin>>pos;
wiesdat 4:055913d9c737 86 cout<<"enc in: "<<pos<<endl;
wiesdat 4:055913d9c737 87
wiesdat 3:8a8dc30fb08e 88 enc = encoderA.getPosition();
wiesdat 4:055913d9c737 89
wiesdat 4:055913d9c737 90 while((pos - enc)>0) {
wiesdat 4:055913d9c737 91 wait(0.1);
wiesdat 4:055913d9c737 92 enc = encoderA.getPosition();
wiesdat 4:055913d9c737 93 out = pid(pos, enc);
wiesdat 4:055913d9c737 94 pwm = out;
wiesdat 4:055913d9c737 95 cout<<"pwm: "<<pwm<<endl;
wiesdat 4:055913d9c737 96
wiesdat 4:055913d9c737 97
wiesdat 3:8a8dc30fb08e 98 }
wiesdat 4:055913d9c737 99
wiesdat 3:8a8dc30fb08e 100 pwm =0;
wiesdat 4:055913d9c737 101 enc = encoderA.getPosition();
wiesdat 4:055913d9c737 102 cout<<"final enc: "<<enc<<endl;
wiesdat 4:055913d9c737 103 cout<<"final error: "<<error<<endl;
wiesdat 4:055913d9c737 104 wait(5);
wiesdat 4:055913d9c737 105 }
wiesdat 3:8a8dc30fb08e 106 }
wiesdat 3:8a8dc30fb08e 107
wiesdat 3:8a8dc30fb08e 108
wiesdat 3:8a8dc30fb08e 109
wiesdat 4:055913d9c737 110
wiesdat 4:055913d9c737 111