Jasper Gerth / Mbed 2 deprecated P-control

P controller to make the motor follow pot1

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@6:b3fd4c6a292e, 2015-09-24 (annotated)

Committer:
Gerth
Date:
Thu Sep 24 10:46:13 2015 +0000
Revision:
6:b3fd4c6a292e
Parent:
5:efa3157df167
Child:
7:1d276d663e1e
working;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:3df25d5b2946 1 #include "mbed.h"
Gerth 1:f367ab17bc18 2 #include "MODSERIAL.h"
Gerth 1:f367ab17bc18 3 #include "QEI.h"
Gerth 1:f367ab17bc18 4 #include "HIDScope.h"
Gerth 1:f367ab17bc18 5
Gerth 2:04ad19deeb2c 6 //info uit
Gerth 5:efa3157df167 7 HIDScope scope(3);
Gerth 1:f367ab17bc18 8 MODSERIAL pc(USBTX, USBRX);
Gerth 2:04ad19deeb2c 9
Gerth 2:04ad19deeb2c 10 //encoders
Gerth 4:339514e618e0 11 QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
Gerth 1:f367ab17bc18 12
Gerth 1:f367ab17bc18 13 //ingangen
Gerth 1:f367ab17bc18 14 AnalogIn pot1(A0);
Gerth 1:f367ab17bc18 15
Gerth 2:04ad19deeb2c 16 //uitgangen
Gerth 4:339514e618e0 17 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 18 PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
Gerth 2:04ad19deeb2c 19 //PwmOut motor2_speed_control(D5);
Gerth 2:04ad19deeb2c 20 //DigitalOut motor2_direction(D4);
Gerth 4:339514e618e0 21 const int CW=1; //clockwise
Gerth 4:339514e618e0 22 const int CCW=0; //counterclockwise
Gerth 2:04ad19deeb2c 23
Gerth 1:f367ab17bc18 24 //frequencies
Gerth 4:339514e618e0 25 //const float pwm_frequency=1000;
Gerth 1:f367ab17bc18 26 const float hidscope_frequency=100;
Gerth 2:04ad19deeb2c 27 const float p_control_frequency=5;
Gerth 1:f367ab17bc18 28
Gerth 1:f367ab17bc18 29 //tickers
Gerth 1:f367ab17bc18 30 Ticker hidscope_ticker;
Gerth 1:f367ab17bc18 31 Ticker p_control_ticker;
Gerth 0:3df25d5b2946 32
Gerth 1:f367ab17bc18 33 //constants
Gerth 4:339514e618e0 34 const int cpr=32*131;
Gerth 1:f367ab17bc18 35 const float PI=3.1415;
Gerth 1:f367ab17bc18 36 const float counttorad=((2*PI)/cpr);
Gerth 4:339514e618e0 37 const float motor1_p_kp=0.5;
Gerth 1:f367ab17bc18 38
Gerth 1:f367ab17bc18 39 //go flags
Gerth 1:f367ab17bc18 40 volatile bool scopedata_go=false, p_control_go=false;
Gerth 1:f367ab17bc18 41
Gerth 1:f367ab17bc18 42 //acvitator functions
Gerth 1:f367ab17bc18 43
Gerth 1:f367ab17bc18 44 void scopedata_activate()
Gerth 1:f367ab17bc18 45 {
Gerth 1:f367ab17bc18 46 scopedata_go=true;
Gerth 1:f367ab17bc18 47 }
Gerth 2:04ad19deeb2c 48 void p_control_activate()
Gerth 2:04ad19deeb2c 49 {
Gerth 1:f367ab17bc18 50 p_control_go=true;
Gerth 2:04ad19deeb2c 51 }
Gerth 1:f367ab17bc18 52 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
Gerth 1:f367ab17bc18 53
Gerth 1:f367ab17bc18 54 //scopedata
Gerth 1:f367ab17bc18 55 void scopedata()
Gerth 1:f367ab17bc18 56 {
Gerth 3:c39510de9443 57 scope.set(0,2*PI*pot1.read());
Gerth 1:f367ab17bc18 58 scope.set(1,counttorad*encoder1.getPulses());
Gerth 3:c39510de9443 59 scope.set(2,motor1_speed_control.read());
Gerth 1:f367ab17bc18 60 scope.send();
Gerth 1:f367ab17bc18 61 }
Gerth 2:04ad19deeb2c 62
Gerth 2:04ad19deeb2c 63 //re usable P controller
Gerth 3:c39510de9443 64 double p_control(float error,float kp)
Gerth 2:04ad19deeb2c 65 {
Gerth 2:04ad19deeb2c 66 return (error*kp);
Gerth 2:04ad19deeb2c 67 }
Gerth 0:3df25d5b2946 68
Gerth 0:3df25d5b2946 69 int main()
Gerth 0:3df25d5b2946 70 {
Gerth 1:f367ab17bc18 71 //set initial shizzle
Gerth 4:339514e618e0 72 //motor1_speed_control.period(1.0/pwm_frequency);
Gerth 3:c39510de9443 73 motor1_speed_control.write(0);
Gerth 1:f367ab17bc18 74
Gerth 2:04ad19deeb2c 75 //tickers
Gerth 1:f367ab17bc18 76 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 1:f367ab17bc18 77 p_control_ticker.attach(&p_control_activate,1.0/p_control_frequency);
Gerth 1:f367ab17bc18 78
Gerth 1:f367ab17bc18 79 while(1) {
Gerth 2:04ad19deeb2c 80 //control motor 1 with a p controller
Gerth 1:f367ab17bc18 81 if (p_control_go==true) {
Gerth 4:339514e618e0 82 float signal_motor1=p_control(2*PI*pot1.read()-counttorad*encoder1.getPulses(),motor1_p_kp);//send error to p controller
Gerth 2:04ad19deeb2c 83 if (signal_motor1>=0) {//determine CW or CCW rotation
Gerth 2:04ad19deeb2c 84 motor1_direction.write(CW);
Gerth 2:04ad19deeb2c 85 } else {
Gerth 2:04ad19deeb2c 86 motor1_direction.write(CCW);
Gerth 2:04ad19deeb2c 87 }
Gerth 4:339514e618e0 88
Gerth 3:c39510de9443 89 if (fabs(signal_motor1)>=1) { //check if signal is <1
Gerth 6:b3fd4c6a292e 90 signal_motor1=1;//if signal >1 make it 1 to not damage motor
Gerth 3:c39510de9443 91 } else {
Gerth 6:b3fd4c6a292e 92 signal_motor1=fabs(signal_motor1);// if signal<1 use signal
Gerth 2:04ad19deeb2c 93 }
Gerth 4:339514e618e0 94
Gerth 2:04ad19deeb2c 95 motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
Gerth 1:f367ab17bc18 96 p_control_go=false;
Gerth 1:f367ab17bc18 97 }
Gerth 6:b3fd4c6a292e 98 //call scopedata
Gerth 1:f367ab17bc18 99 if (scopedata_go==true) {
Gerth 1:f367ab17bc18 100 scopedata();
Gerth 1:f367ab17bc18 101 scopedata_go=false;
Gerth 1:f367ab17bc18 102 }
Gerth 0:3df25d5b2946 103 }
Gerth 6:b3fd4c6a292e 104 return 0;
Gerth 1:f367ab17bc18 105 }