P controller to make the motor follow pot1
Dependencies: HIDScope MODSERIAL QEI mbed
main.cpp@8:b30e7b255e97, 2015-09-24 (annotated)
- Committer:
- Gerth
- Date:
- Thu Sep 24 16:28:56 2015 +0000
- Revision:
- 8:b30e7b255e97
- Parent:
- 7:1d276d663e1e
works; controller values need to be tuned;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Gerth | 0:3df25d5b2946 | 1 | #include "mbed.h" |
Gerth | 1:f367ab17bc18 | 2 | #include "QEI.h" |
Gerth | 1:f367ab17bc18 | 3 | #include "HIDScope.h" |
Gerth | 1:f367ab17bc18 | 4 | |
Gerth | 2:04ad19deeb2c | 5 | //info uit |
Gerth | 5:efa3157df167 | 6 | HIDScope scope(3); |
Gerth | 2:04ad19deeb2c | 7 | |
Gerth | 2:04ad19deeb2c | 8 | //encoders |
Gerth | 4:339514e618e0 | 9 | QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise + |
Gerth | 1:f367ab17bc18 | 10 | |
Gerth | 1:f367ab17bc18 | 11 | //ingangen |
Gerth | 8:b30e7b255e97 | 12 | AnalogIn pot1(A2); |
Gerth | 1:f367ab17bc18 | 13 | |
Gerth | 2:04ad19deeb2c | 14 | //uitgangen |
Gerth | 4:339514e618e0 | 15 | DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt) |
Gerth | 2:04ad19deeb2c | 16 | PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1 |
Gerth | 2:04ad19deeb2c | 17 | //PwmOut motor2_speed_control(D5); |
Gerth | 2:04ad19deeb2c | 18 | //DigitalOut motor2_direction(D4); |
Gerth | 4:339514e618e0 | 19 | const int CW=1; //clockwise |
Gerth | 4:339514e618e0 | 20 | const int CCW=0; //counterclockwise |
Gerth | 2:04ad19deeb2c | 21 | |
Gerth | 1:f367ab17bc18 | 22 | //frequencies |
Gerth | 4:339514e618e0 | 23 | //const float pwm_frequency=1000; |
Gerth | 1:f367ab17bc18 | 24 | const float hidscope_frequency=100; |
Gerth | 2:04ad19deeb2c | 25 | const float p_control_frequency=5; |
Gerth | 1:f367ab17bc18 | 26 | |
Gerth | 1:f367ab17bc18 | 27 | //tickers |
Gerth | 1:f367ab17bc18 | 28 | Ticker hidscope_ticker; |
Gerth | 1:f367ab17bc18 | 29 | Ticker p_control_ticker; |
Gerth | 0:3df25d5b2946 | 30 | |
Gerth | 1:f367ab17bc18 | 31 | //constants |
Gerth | 4:339514e618e0 | 32 | const int cpr=32*131; |
Gerth | 1:f367ab17bc18 | 33 | const float PI=3.1415; |
Gerth | 1:f367ab17bc18 | 34 | const float counttorad=((2*PI)/cpr); |
Gerth | 4:339514e618e0 | 35 | const float motor1_p_kp=0.5; |
Gerth | 1:f367ab17bc18 | 36 | |
Gerth | 1:f367ab17bc18 | 37 | //go flags |
Gerth | 1:f367ab17bc18 | 38 | volatile bool scopedata_go=false, p_control_go=false; |
Gerth | 1:f367ab17bc18 | 39 | |
Gerth | 1:f367ab17bc18 | 40 | //acvitator functions |
Gerth | 1:f367ab17bc18 | 41 | |
Gerth | 1:f367ab17bc18 | 42 | void scopedata_activate() |
Gerth | 1:f367ab17bc18 | 43 | { |
Gerth | 1:f367ab17bc18 | 44 | scopedata_go=true; |
Gerth | 1:f367ab17bc18 | 45 | } |
Gerth | 2:04ad19deeb2c | 46 | void p_control_activate() |
Gerth | 2:04ad19deeb2c | 47 | { |
Gerth | 1:f367ab17bc18 | 48 | p_control_go=true; |
Gerth | 2:04ad19deeb2c | 49 | } |
Gerth | 1:f367ab17bc18 | 50 | ///////////////////////////////////////////////////////FUNCTIONS////////////////////////////////////////////////////////////////////////// |
Gerth | 1:f367ab17bc18 | 51 | |
Gerth | 1:f367ab17bc18 | 52 | //scopedata |
Gerth | 1:f367ab17bc18 | 53 | void scopedata() |
Gerth | 1:f367ab17bc18 | 54 | { |
Gerth | 8:b30e7b255e97 | 55 | scope.set(0,2*PI*pot1.read());//gewenste hoek in rad van potmeter |
Gerth | 8:b30e7b255e97 | 56 | scope.set(1,counttorad*encoder1.getPulses());//hoek in rad van outputshaft |
Gerth | 8:b30e7b255e97 | 57 | scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe |
Gerth | 1:f367ab17bc18 | 58 | scope.send(); |
Gerth | 1:f367ab17bc18 | 59 | } |
Gerth | 2:04ad19deeb2c | 60 | |
Gerth | 2:04ad19deeb2c | 61 | //re usable P controller |
Gerth | 3:c39510de9443 | 62 | double p_control(float error,float kp) |
Gerth | 2:04ad19deeb2c | 63 | { |
Gerth | 2:04ad19deeb2c | 64 | return (error*kp); |
Gerth | 2:04ad19deeb2c | 65 | } |
Gerth | 0:3df25d5b2946 | 66 | |
Gerth | 0:3df25d5b2946 | 67 | int main() |
Gerth | 0:3df25d5b2946 | 68 | { |
Gerth | 1:f367ab17bc18 | 69 | //set initial shizzle |
Gerth | 4:339514e618e0 | 70 | //motor1_speed_control.period(1.0/pwm_frequency); |
Gerth | 3:c39510de9443 | 71 | motor1_speed_control.write(0); |
Gerth | 1:f367ab17bc18 | 72 | |
Gerth | 2:04ad19deeb2c | 73 | //tickers |
Gerth | 1:f367ab17bc18 | 74 | hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency); |
Gerth | 1:f367ab17bc18 | 75 | p_control_ticker.attach(&p_control_activate,1.0/p_control_frequency); |
Gerth | 1:f367ab17bc18 | 76 | |
Gerth | 1:f367ab17bc18 | 77 | while(1) { |
Gerth | 2:04ad19deeb2c | 78 | //control motor 1 with a p controller |
Gerth | 1:f367ab17bc18 | 79 | if (p_control_go==true) { |
Gerth | 4:339514e618e0 | 80 | float signal_motor1=p_control(2*PI*pot1.read()-counttorad*encoder1.getPulses(),motor1_p_kp);//send error to p controller |
Gerth | 2:04ad19deeb2c | 81 | if (signal_motor1>=0) {//determine CW or CCW rotation |
Gerth | 2:04ad19deeb2c | 82 | motor1_direction.write(CW); |
Gerth | 2:04ad19deeb2c | 83 | } else { |
Gerth | 2:04ad19deeb2c | 84 | motor1_direction.write(CCW); |
Gerth | 2:04ad19deeb2c | 85 | } |
Gerth | 4:339514e618e0 | 86 | |
Gerth | 3:c39510de9443 | 87 | if (fabs(signal_motor1)>=1) { //check if signal is <1 |
Gerth | 6:b3fd4c6a292e | 88 | signal_motor1=1;//if signal >1 make it 1 to not damage motor |
Gerth | 3:c39510de9443 | 89 | } else { |
Gerth | 6:b3fd4c6a292e | 90 | signal_motor1=fabs(signal_motor1);// if signal<1 use signal |
Gerth | 2:04ad19deeb2c | 91 | } |
Gerth | 4:339514e618e0 | 92 | |
Gerth | 2:04ad19deeb2c | 93 | motor1_speed_control.write(fabs(signal_motor1));//write signal to motor |
Gerth | 1:f367ab17bc18 | 94 | p_control_go=false; |
Gerth | 1:f367ab17bc18 | 95 | } |
Gerth | 6:b3fd4c6a292e | 96 | //call scopedata |
Gerth | 1:f367ab17bc18 | 97 | if (scopedata_go==true) { |
Gerth | 1:f367ab17bc18 | 98 | scopedata(); |
Gerth | 1:f367ab17bc18 | 99 | scopedata_go=false; |
Gerth | 1:f367ab17bc18 | 100 | } |
Gerth | 0:3df25d5b2946 | 101 | } |
Gerth | 6:b3fd4c6a292e | 102 | return 0; |
Gerth | 1:f367ab17bc18 | 103 | } |