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@0:dbe7715dcfe9, 2015-09-24 (annotated)

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