BioRobotics / Mbed 2 deprecated PI_control_pot

PI controller to make the motor follow pot1

Dependencies:   HIDScope MODSERIAL QEI mbed

Fork of PI_control_pot by Jasper Gerth

main.cpp@1:b75c4b9f1c98, 2015-09-24 (annotated)

Committer:
Gerth
Date:
Thu Sep 24 15:58:55 2015 +0000
Revision:
1:b75c4b9f1c98
Parent:
0:dbe7715dcfe9
Child:
2:d0076e9d0a7f
PI controller, only have to adjust values for the controller; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:dbe7715dcfe9 1 #include "mbed.h"
Gerth 0:dbe7715dcfe9 2 #include "QEI.h"
Gerth 0:dbe7715dcfe9 3 #include "HIDScope.h"
Gerth 0:dbe7715dcfe9 4
Gerth 1:b75c4b9f1c98 5 //////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
Gerth 0:dbe7715dcfe9 6 //info uit
Gerth 0:dbe7715dcfe9 7 HIDScope scope(3);
Gerth 0:dbe7715dcfe9 8
Gerth 0:dbe7715dcfe9 9 //encoders
Gerth 0:dbe7715dcfe9 10 QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
Gerth 0:dbe7715dcfe9 11
Gerth 0:dbe7715dcfe9 12 //ingangen
Gerth 0:dbe7715dcfe9 13 AnalogIn pot1(A2);
Gerth 0:dbe7715dcfe9 14
Gerth 0:dbe7715dcfe9 15 //uitgangen
Gerth 0:dbe7715dcfe9 16 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 0:dbe7715dcfe9 17 PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
Gerth 0:dbe7715dcfe9 18 //PwmOut motor2_speed_control(D5);
Gerth 0:dbe7715dcfe9 19 //DigitalOut motor2_direction(D4);
Gerth 0:dbe7715dcfe9 20 const int CW=1; //clockwise
Gerth 0:dbe7715dcfe9 21 const int CCW=0; //counterclockwise
Gerth 0:dbe7715dcfe9 22
Gerth 0:dbe7715dcfe9 23 //frequencies
Gerth 0:dbe7715dcfe9 24 //const float pwm_frequency=1000;
Gerth 0:dbe7715dcfe9 25 const double hidscope_frequency=100;
Gerth 1:b75c4b9f1c98 26 const double pi_control_frequency=5;
Gerth 0:dbe7715dcfe9 27
Gerth 0:dbe7715dcfe9 28 //tickers
Gerth 0:dbe7715dcfe9 29 Ticker hidscope_ticker;
Gerth 1:b75c4b9f1c98 30 Ticker pi_control_ticker;
Gerth 0:dbe7715dcfe9 31
Gerth 0:dbe7715dcfe9 32 //constants
Gerth 0:dbe7715dcfe9 33 const int cpr=32*131;
Gerth 0:dbe7715dcfe9 34 const float PI=3.1415;
Gerth 0:dbe7715dcfe9 35 const float counttorad=((2*PI)/cpr);
Gerth 1:b75c4b9f1c98 36
Gerth 1:b75c4b9f1c98 37 ///////////////////////////////////////////////////CONTROLLER CONSTANTS////////////////////////////////
Gerth 0:dbe7715dcfe9 38
Gerth 1:b75c4b9f1c98 39 //DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!
Gerth 1:b75c4b9f1c98 40 const float motor1_pi_kp=0.5;
Gerth 1:b75c4b9f1c98 41 const double motor1_pi_ki=0.01;
Gerth 1:b75c4b9f1c98 42 double motor1_error_int=0;
Gerth 1:b75c4b9f1c98 43 ////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
Gerth 0:dbe7715dcfe9 44 //go flags
Gerth 1:b75c4b9f1c98 45 volatile bool scopedata_go=false, pi_control_go=false;
Gerth 0:dbe7715dcfe9 46
Gerth 0:dbe7715dcfe9 47 //acvitator functions
Gerth 0:dbe7715dcfe9 48
Gerth 0:dbe7715dcfe9 49 void scopedata_activate()
Gerth 0:dbe7715dcfe9 50 {
Gerth 0:dbe7715dcfe9 51 scopedata_go=true;
Gerth 0:dbe7715dcfe9 52 }
Gerth 1:b75c4b9f1c98 53 void pi_control_activate()
Gerth 0:dbe7715dcfe9 54 {
Gerth 1:b75c4b9f1c98 55 pi_control_go=true;
Gerth 0:dbe7715dcfe9 56 }
Gerth 0:dbe7715dcfe9 57 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
Gerth 0:dbe7715dcfe9 58
Gerth 0:dbe7715dcfe9 59 //scopedata
Gerth 0:dbe7715dcfe9 60 void scopedata()
Gerth 0:dbe7715dcfe9 61 {
Gerth 0:dbe7715dcfe9 62 scope.set(0,2*PI*pot1.read());//gewenste hoek in rad van potmeter
Gerth 0:dbe7715dcfe9 63 scope.set(1,counttorad*encoder1.getPulses());//hoek in rad van outputshaft
Gerth 0:dbe7715dcfe9 64 scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe
Gerth 0:dbe7715dcfe9 65 scope.send();
Gerth 0:dbe7715dcfe9 66 }
Gerth 1:b75c4b9f1c98 67 //////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
Gerth 1:b75c4b9f1c98 68 // Reusable PI controller
Gerth 1:b75c4b9f1c98 69 double pi_control( double e, const double Kp, const double Ki, double Ts, double& motor1_error_int)
Gerth 0:dbe7715dcfe9 70 {
Gerth 1:b75c4b9f1c98 71 motor1_error_int = motor1_error_int + Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
Gerth 1:b75c4b9f1c98 72 return Kp*e+Ki*motor1_error_int;
Gerth 0:dbe7715dcfe9 73 }
Gerth 0:dbe7715dcfe9 74
Gerth 1:b75c4b9f1c98 75 //////////////////////////////////////////////////MAIN///////////////////////////////////
Gerth 0:dbe7715dcfe9 76 int main()
Gerth 0:dbe7715dcfe9 77 {
Gerth 0:dbe7715dcfe9 78 //set initial shizzle
Gerth 0:dbe7715dcfe9 79 //motor1_speed_control.period(1.0/pwm_frequency);
Gerth 0:dbe7715dcfe9 80 motor1_speed_control.write(0);
Gerth 0:dbe7715dcfe9 81
Gerth 0:dbe7715dcfe9 82 //tickers
Gerth 0:dbe7715dcfe9 83 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 1:b75c4b9f1c98 84 pi_control_ticker.attach(&pi_control_activate,1.0/pi_control_frequency);
Gerth 0:dbe7715dcfe9 85
Gerth 0:dbe7715dcfe9 86 while(1) {
Gerth 0:dbe7715dcfe9 87 //control motor 1 with a p controller
Gerth 1:b75c4b9f1c98 88 if (pi_control_go==true) {
Gerth 1:b75c4b9f1c98 89 double error=2*PI*pot1.read()-counttorad*encoder1.getPulses();
Gerth 1:b75c4b9f1c98 90 float signal_motor1=pi_control(error,motor1_pi_kp,motor1_pi_ki,1/pi_control_frequency,motor1_error_int);//send error to p controller
Gerth 0:dbe7715dcfe9 91 if (signal_motor1>=0) {//determine CW or CCW rotation
Gerth 0:dbe7715dcfe9 92 motor1_direction.write(CW);
Gerth 0:dbe7715dcfe9 93 } else {
Gerth 0:dbe7715dcfe9 94 motor1_direction.write(CCW);
Gerth 0:dbe7715dcfe9 95 }
Gerth 0:dbe7715dcfe9 96
Gerth 0:dbe7715dcfe9 97 if (fabs(signal_motor1)>=1) { //check if signal is <1
Gerth 0:dbe7715dcfe9 98 signal_motor1=1;//if signal >1 make it 1 to not damage motor
Gerth 0:dbe7715dcfe9 99 } else {
Gerth 0:dbe7715dcfe9 100 signal_motor1=fabs(signal_motor1);// if signal<1 use signal
Gerth 0:dbe7715dcfe9 101 }
Gerth 0:dbe7715dcfe9 102
Gerth 0:dbe7715dcfe9 103 motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
Gerth 1:b75c4b9f1c98 104 pi_control_go=false;
Gerth 0:dbe7715dcfe9 105 }
Gerth 0:dbe7715dcfe9 106 //call scopedata
Gerth 0:dbe7715dcfe9 107 if (scopedata_go==true) {
Gerth 0:dbe7715dcfe9 108 scopedata();
Gerth 0:dbe7715dcfe9 109 scopedata_go=false;
Gerth 0:dbe7715dcfe9 110 }
Gerth 0:dbe7715dcfe9 111 }
Gerth 0:dbe7715dcfe9 112 return 0;
Gerth 0:dbe7715dcfe9 113 }