thijs ruikes / Mbed 2 deprecated PROJECT-MOTORCONTROL

ding

Dependencies:   Encoder HIDScope mbed

main.cpp@6:0403c2e276c2, 2014-10-30 (annotated)

Committer:
wiesdat
Date:
Thu Oct 30 12:38:09 2014 +0000
Revision:
6:0403c2e276c2
Parent:
5:93ccec91a4ae 
positie regelaar

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 6:0403c2e276c2 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 5:93ccec91a4ae 20 float v=0;
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 5:93ccec91a4ae 29
wiesdat 3:8a8dc30fb08e 30 float pid(float setpoint, float measurement)
wiesdat 3:8a8dc30fb08e 31 {
wiesdat 3:8a8dc30fb08e 32 float error;
wiesdat 3:8a8dc30fb08e 33 static float prev_error = 0;
wiesdat 3:8a8dc30fb08e 34 float out_p = 0;
wiesdat 3:8a8dc30fb08e 35 static float out_i = 0;
wiesdat 3:8a8dc30fb08e 36 float out_d = 0;
wiesdat 3:8a8dc30fb08e 37 error = setpoint-measurement;
wiesdat 3:8a8dc30fb08e 38 out_p = error*K_P;
wiesdat 3:8a8dc30fb08e 39 out_i += error*K_I;
wiesdat 3:8a8dc30fb08e 40 out_d = (error-prev_error)*K_D;
wiesdat 3:8a8dc30fb08e 41 clamp(&out_i,-I_LIMIT,I_LIMIT);
wiesdat 3:8a8dc30fb08e 42 prev_error = error;
wiesdat 3:8a8dc30fb08e 43 out = out_i;
wiesdat 4:055913d9c737 44
wiesdat 3:8a8dc30fb08e 45 return out;
wiesdat 3:8a8dc30fb08e 46 }
wiesdat 3:8a8dc30fb08e 47
wiesdat 3:8a8dc30fb08e 48 float getv(float delta_t)
wiesdat 3:8a8dc30fb08e 49 {
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 6:0403c2e276c2 65 float reset()
wiesdat 6:0403c2e276c2 66 {
wiesdat 6:0403c2e276c2 67 v = 1;
wiesdat 6:0403c2e276c2 68 while(v !=0) {
wiesdat 6:0403c2e276c2 69
wiesdat 6:0403c2e276c2 70 dir = 0;
wiesdat 6:0403c2e276c2 71 pwm.write(0.1);
wiesdat 6:0403c2e276c2 72 v =getv(0.1);
wiesdat 6:0403c2e276c2 73 }
wiesdat 6:0403c2e276c2 74 pwm = 0;
wiesdat 6:0403c2e276c2 75 dir =1;
wiesdat 6:0403c2e276c2 76 encoderA.setPosition(0);
wiesdat 6:0403c2e276c2 77 zero = encoderA.getPosition();
wiesdat 6:0403c2e276c2 78 cout<<"zero encoder: "<<zero<<endl;
wiesdat 6:0403c2e276c2 79 return pwm;
wiesdat 6:0403c2e276c2 80 }
wiesdat 5:93ccec91a4ae 81
wiesdat 6:0403c2e276c2 82 float gotopos(float pos)
wiesdat 6:0403c2e276c2 83 {
wiesdat 6:0403c2e276c2 84 enc = encoderA.getPosition();
wiesdat 3:8a8dc30fb08e 85
wiesdat 6:0403c2e276c2 86 while((pos - enc)>0) {
wiesdat 6:0403c2e276c2 87 wait(0.1);
wiesdat 6:0403c2e276c2 88 enc = encoderA.getPosition();
wiesdat 6:0403c2e276c2 89 out = pid(pos, enc);
wiesdat 6:0403c2e276c2 90 pwm = out;
wiesdat 6:0403c2e276c2 91 }
wiesdat 6:0403c2e276c2 92 pwm =0;
wiesdat 6:0403c2e276c2 93 enc = encoderA.getPosition();
wiesdat 6:0403c2e276c2 94 cout<<"final enc: "<<enc<<endl;
wiesdat 6:0403c2e276c2 95 cout<<"final error: "<<error<<endl;
wiesdat 6:0403c2e276c2 96 return pwm;
wiesdat 6:0403c2e276c2 97 }
wiesdat 3:8a8dc30fb08e 98
wiesdat 3:8a8dc30fb08e 99 int main()
wiesdat 4:055913d9c737 100 {
wiesdat 4:055913d9c737 101 while(1) {
wiesdat 6:0403c2e276c2 102 reset();
wiesdat 6:0403c2e276c2 103 cout<<"typ pos: "<<endl;
wiesdat 3:8a8dc30fb08e 104 cin>>pos;
wiesdat 6:0403c2e276c2 105 gotopos(pos);
wiesdat 4:055913d9c737 106 wait(5);
wiesdat 4:055913d9c737 107 }
wiesdat 3:8a8dc30fb08e 108 }
wiesdat 3:8a8dc30fb08e 109
wiesdat 3:8a8dc30fb08e 110
wiesdat 3:8a8dc30fb08e 111
wiesdat 4:055913d9c737 112
wiesdat 4:055913d9c737 113