thijs ruikes / Mbed 2 deprecated PROJECT-MOTORCONTROL

ding

Dependencies:   Encoder HIDScope mbed

main.cpp@5:93ccec91a4ae, 2014-10-30 (annotated)

Committer:
wiesdat
Date:
Thu Oct 30 11:27:04 2014 +0000
Revision:
5:93ccec91a4ae
Parent:
4:055913d9c737
Child:
6:0403c2e276c2
niet werkend

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 5:93ccec91a4ae 5 #define K_I (0.0001)
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 5:93ccec91a4ae 65 float reset(){
wiesdat 5:93ccec91a4ae 66
wiesdat 5:93ccec91a4ae 67 return pwm;
wiesdat 5:93ccec91a4ae 68 }
wiesdat 5:93ccec91a4ae 69
wiesdat 3:8a8dc30fb08e 70
wiesdat 3:8a8dc30fb08e 71
wiesdat 3:8a8dc30fb08e 72 int main()
wiesdat 4:055913d9c737 73 {
wiesdat 4:055913d9c737 74 while(1) {
wiesdat 5:93ccec91a4ae 75 pwm = 1;
wiesdat 5:93ccec91a4ae 76 cout<<"voor de loop"<<endl;
wiesdat 5:93ccec91a4ae 77
wiesdat 5:93ccec91a4ae 78 while(v !=0) {
wiesdat 4:055913d9c737 79
wiesdat 4:055913d9c737 80 dir = 0;
wiesdat 4:055913d9c737 81 pwm.write(0.1);
wiesdat 5:93ccec91a4ae 82 v =getv(0.2);
wiesdat 3:8a8dc30fb08e 83 }
wiesdat 3:8a8dc30fb08e 84 pwm = 0;
wiesdat 3:8a8dc30fb08e 85 dir =1;
wiesdat 3:8a8dc30fb08e 86 encoderA.setPosition(0);
wiesdat 5:93ccec91a4ae 87 zero = encoderA.getPosition();
wiesdat 3:8a8dc30fb08e 88 cout<<"zero encoder: "<<zero<<endl;
wiesdat 4:055913d9c737 89
wiesdat 5:93ccec91a4ae 90
wiesdat 5:93ccec91a4ae 91
wiesdat 5:93ccec91a4ae 92
wiesdat 3:8a8dc30fb08e 93 cout<<"positie in encoder bits: "<<endl;
wiesdat 3:8a8dc30fb08e 94 cin>>pos;
wiesdat 4:055913d9c737 95 cout<<"enc in: "<<pos<<endl;
wiesdat 4:055913d9c737 96
wiesdat 3:8a8dc30fb08e 97 enc = encoderA.getPosition();
wiesdat 4:055913d9c737 98
wiesdat 4:055913d9c737 99 while((pos - enc)>0) {
wiesdat 4:055913d9c737 100 wait(0.1);
wiesdat 4:055913d9c737 101 enc = encoderA.getPosition();
wiesdat 4:055913d9c737 102 out = pid(pos, enc);
wiesdat 4:055913d9c737 103 pwm = out;
wiesdat 4:055913d9c737 104 cout<<"pwm: "<<pwm<<endl;
wiesdat 4:055913d9c737 105
wiesdat 4:055913d9c737 106
wiesdat 3:8a8dc30fb08e 107 }
wiesdat 4:055913d9c737 108
wiesdat 3:8a8dc30fb08e 109 pwm =0;
wiesdat 4:055913d9c737 110 enc = encoderA.getPosition();
wiesdat 4:055913d9c737 111 cout<<"final enc: "<<enc<<endl;
wiesdat 4:055913d9c737 112 cout<<"final error: "<<error<<endl;
wiesdat 4:055913d9c737 113 wait(5);
wiesdat 4:055913d9c737 114 }
wiesdat 3:8a8dc30fb08e 115 }
wiesdat 3:8a8dc30fb08e 116
wiesdat 3:8a8dc30fb08e 117
wiesdat 3:8a8dc30fb08e 118
wiesdat 4:055913d9c737 119
wiesdat 4:055913d9c737 120