BioRobotics / Mbed 2 deprecated buttoncontrol

With this program the buttons on the biorobotics shield can be used to move the arms. both buttons pressed means shoot. Also SW3 can be pressed to adjust controller constants via Serial connection

Dependencies:   HIDScope QEI controlandadjust mbed

main.cpp@0:24628832f21d, 2015-10-01 (annotated)

Committer:
Gerth
Date:
Thu Oct 01 14:33:20 2015 +0000
Revision:
0:24628832f21d
Child:
1:41a9e3340c96
working, direct control of angle with 2 motors. ; now trying to clean up code (less global varables)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:24628832f21d 1 #include "mbed.h"
Gerth 0:24628832f21d 2 #include "QEI.h"
Gerth 0:24628832f21d 3 #include "HIDScope.h"
Gerth 0:24628832f21d 4
Gerth 0:24628832f21d 5 //////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
Gerth 0:24628832f21d 6 //info uit
Gerth 0:24628832f21d 7 HIDScope scope(5);
Gerth 0:24628832f21d 8
Gerth 0:24628832f21d 9 //encoders
Gerth 0:24628832f21d 10 QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
Gerth 0:24628832f21d 11 QEI encoder2 (D10,D11,NC,32);
Gerth 0:24628832f21d 12
Gerth 0:24628832f21d 13 //ingangen
Gerth 0:24628832f21d 14 DigitalIn butR(D2);
Gerth 0:24628832f21d 15 DigitalIn butL(D3);
Gerth 0:24628832f21d 16
Gerth 0:24628832f21d 17 //uitgangen
Gerth 0:24628832f21d 18 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:24628832f21d 19 PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
Gerth 0:24628832f21d 20 PwmOut motor2_speed_control(D5);
Gerth 0:24628832f21d 21 DigitalOut motor2_direction(D4);
Gerth 0:24628832f21d 22 const int CW=1; //clockwise
Gerth 0:24628832f21d 23 const int CCW=0; //counterclockwise
Gerth 0:24628832f21d 24
Gerth 0:24628832f21d 25 //frequencies
Gerth 0:24628832f21d 26 const double hidscope_frequency=100;//frequentie waarop data naar HIDScope wordt gestuurd
Gerth 0:24628832f21d 27 const double pi_control_frequency=25;//frequentie waarop er een control actie wordt uitgevoerd. kan hoger, voorzichtig bij ruis!
Gerth 0:24628832f21d 28 const double read_but_frequency=25;
Gerth 0:24628832f21d 29
Gerth 0:24628832f21d 30 //tickers
Gerth 0:24628832f21d 31 Ticker hidscope_ticker;
Gerth 0:24628832f21d 32 Ticker pi_control_ticker;
Gerth 0:24628832f21d 33 Ticker read_but_ticker;
Gerth 0:24628832f21d 34
Gerth 0:24628832f21d 35 //constants
Gerth 0:24628832f21d 36 const float cpr=32*131;
Gerth 0:24628832f21d 37 const float PI=3.1415;
Gerth 0:24628832f21d 38 const float counttorad=((2*PI)/cpr);
Gerth 0:24628832f21d 39 const float counttodeg=(360/cpr);
Gerth 0:24628832f21d 40 const float mmpersecbut=(30/(read_but_frequency));
Gerth 0:24628832f21d 41 const float radpersecbut=(PI/read_but_frequency);
Gerth 0:24628832f21d 42 const float armlength=400;//length "upper arm" in mm
Gerth 0:24628832f21d 43
Gerth 0:24628832f21d 44 ///////////////////////////////////////////////////CONTROLLER CONSTANTS////////////////////////////////
Gerth 0:24628832f21d 45
Gerth 0:24628832f21d 46 //DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!
Gerth 0:24628832f21d 47
Gerth 0:24628832f21d 48
Gerth 0:24628832f21d 49 double desiredangle[2]= {0,0};
Gerth 0:24628832f21d 50 double desiredposition=0;
Gerth 0:24628832f21d 51 double signal_motor[2]= {0,0};
Gerth 0:24628832f21d 52 ////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
Gerth 0:24628832f21d 53 //go flags
Gerth 0:24628832f21d 54 volatile bool scopedata_go=false, pi_control_go=false,read_but_go=false;
Gerth 0:24628832f21d 55
Gerth 0:24628832f21d 56 //acvitator functions
Gerth 0:24628832f21d 57
Gerth 0:24628832f21d 58 void scopedata_activate()
Gerth 0:24628832f21d 59 {
Gerth 0:24628832f21d 60 scopedata_go=true;
Gerth 0:24628832f21d 61 }
Gerth 0:24628832f21d 62 void pi_control_activate()
Gerth 0:24628832f21d 63 {
Gerth 0:24628832f21d 64 pi_control_go=true;
Gerth 0:24628832f21d 65 }
Gerth 0:24628832f21d 66 void read_but_activate()
Gerth 0:24628832f21d 67 {
Gerth 0:24628832f21d 68 read_but_go=true;
Gerth 0:24628832f21d 69 }
Gerth 0:24628832f21d 70 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
Gerth 0:24628832f21d 71
Gerth 0:24628832f21d 72 //scopedata
Gerth 0:24628832f21d 73 void scopedata()
Gerth 0:24628832f21d 74 {
Gerth 0:24628832f21d 75 scope.set(0,desiredposition);//gewenste hoek van arm
Gerth 0:24628832f21d 76 scope.set(1,counttodeg*encoder1.getPulses());//hoek in rad van outputshaft
Gerth 0:24628832f21d 77 scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe
Gerth 0:24628832f21d 78 scope.set(3,counttodeg*encoder2.getPulses());//hoek in rad van outputshaft
Gerth 0:24628832f21d 79 scope.set(4,motor2_speed_control.read());//pwm signaal naar motor toe
Gerth 0:24628832f21d 80 scope.send();
Gerth 0:24628832f21d 81 }
Gerth 0:24628832f21d 82 //////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
Gerth 0:24628832f21d 83 float pi_kp=0.5;
Gerth 0:24628832f21d 84 float pi_ki=0.01;
Gerth 0:24628832f21d 85 float Ts=1.0/pi_control_frequency;
Gerth 0:24628832f21d 86 double e[2]= {0,0};
Gerth 0:24628832f21d 87 double motor_error_int[2]= {0,0};
Gerth 0:24628832f21d 88
Gerth 0:24628832f21d 89 // Reusable PI controller
Gerth 0:24628832f21d 90 void pi_control()
Gerth 0:24628832f21d 91 {
Gerth 0:24628832f21d 92 motor_error_int[0] = motor_error_int[0] + Ts * e[0]; // e_int is changed globally because it’s ’by reference’ (&)
Gerth 0:24628832f21d 93 signal_motor[0]=pi_kp*e[0]+pi_ki*motor_error_int[0];
Gerth 0:24628832f21d 94
Gerth 0:24628832f21d 95 motor_error_int[1] = motor_error_int[1] + Ts * e[1]; // e_int is changed globally because it’s ’by reference’ (&)
Gerth 0:24628832f21d 96 signal_motor[1]=pi_kp*e[1]+pi_ki*motor_error_int[1];
Gerth 0:24628832f21d 97 }
Gerth 0:24628832f21d 98
Gerth 0:24628832f21d 99 //////////////////////////////////////////////////MAIN///////////////////////////////////
Gerth 0:24628832f21d 100 int main()
Gerth 0:24628832f21d 101 {
Gerth 0:24628832f21d 102 //set initial shizzle
Gerth 0:24628832f21d 103 motor1_speed_control.write(0);
Gerth 0:24628832f21d 104 motor2_speed_control.write(0);
Gerth 0:24628832f21d 105
Gerth 0:24628832f21d 106 //tickers
Gerth 0:24628832f21d 107 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 0:24628832f21d 108 pi_control_ticker.attach(&pi_control_activate,1.0/pi_control_frequency);
Gerth 0:24628832f21d 109 read_but_ticker.attach(&read_but_activate,1.0/read_but_frequency);
Gerth 0:24628832f21d 110
Gerth 0:24628832f21d 111 while(1) {
Gerth 0:24628832f21d 112 //control motor 1 with a pi controller
Gerth 0:24628832f21d 113
Gerth 0:24628832f21d 114 if (read_but_go==true) {
Gerth 0:24628832f21d 115 float buttonR=butR.read(), buttonL=butL.read();
Gerth 0:24628832f21d 116 if (buttonR==0 && buttonL ==1) {
Gerth 0:24628832f21d 117 desiredposition += (radpersecbut);
Gerth 0:24628832f21d 118 read_but_go=false;
Gerth 0:24628832f21d 119 }
Gerth 0:24628832f21d 120 if (buttonR==1 && buttonL==0) {
Gerth 0:24628832f21d 121 desiredposition -= (radpersecbut);
Gerth 0:24628832f21d 122 read_but_go=false;
Gerth 0:24628832f21d 123 } else {
Gerth 0:24628832f21d 124 desiredposition=desiredposition;
Gerth 0:24628832f21d 125 }
Gerth 0:24628832f21d 126 desiredangle[0]=desiredposition;
Gerth 0:24628832f21d 127 desiredangle[1]=desiredposition;
Gerth 0:24628832f21d 128 read_but_go=false;
Gerth 0:24628832f21d 129 }
Gerth 0:24628832f21d 130
Gerth 0:24628832f21d 131 if (pi_control_go==true) {
Gerth 0:24628832f21d 132 e[0]=desiredangle[0]-(counttorad*encoder1.getPulses());
Gerth 0:24628832f21d 133 e[1]=desiredangle[1]-(counttorad*encoder2.getPulses());
Gerth 0:24628832f21d 134 pi_control();
Gerth 0:24628832f21d 135 //motor1
Gerth 0:24628832f21d 136 if (signal_motor[0]>=0) {//determine CW or CCW rotation
Gerth 0:24628832f21d 137 motor1_direction.write(CW);
Gerth 0:24628832f21d 138 } else {
Gerth 0:24628832f21d 139 motor1_direction.write(CCW);
Gerth 0:24628832f21d 140 }
Gerth 0:24628832f21d 141
Gerth 0:24628832f21d 142 if (fabs(signal_motor[0])>=1) { //check if signal is <1
Gerth 0:24628832f21d 143 signal_motor[0]=1;//if signal >1 make it 1 to not damage motor
Gerth 0:24628832f21d 144 } else {
Gerth 0:24628832f21d 145 signal_motor[0]=fabs(signal_motor[0]);// if signal<1 use signal
Gerth 0:24628832f21d 146 }
Gerth 0:24628832f21d 147
Gerth 0:24628832f21d 148 //motor 2
Gerth 0:24628832f21d 149 if (signal_motor[1]>=0) {//determine CW or CCW rotation
Gerth 0:24628832f21d 150 motor2_direction.write(CW);
Gerth 0:24628832f21d 151 } else {
Gerth 0:24628832f21d 152 motor2_direction.write(CCW);
Gerth 0:24628832f21d 153 }
Gerth 0:24628832f21d 154
Gerth 0:24628832f21d 155 if (fabs(signal_motor[1])>=1) { //check if signal is <1
Gerth 0:24628832f21d 156 signal_motor[1]=1;//if signal >1 make it 1 to not damage motor
Gerth 0:24628832f21d 157 } else {
Gerth 0:24628832f21d 158 signal_motor[1]=fabs(signal_motor[1]);// if signal<1 use signal
Gerth 0:24628832f21d 159 }
Gerth 0:24628832f21d 160
Gerth 0:24628832f21d 161
Gerth 0:24628832f21d 162
Gerth 0:24628832f21d 163 motor1_speed_control.write(fabs(signal_motor[0]));//write signal to motor
Gerth 0:24628832f21d 164 motor2_speed_control.write(fabs(signal_motor[1]));//write signal to motor
Gerth 0:24628832f21d 165
Gerth 0:24628832f21d 166 pi_control_go=false;
Gerth 0:24628832f21d 167 }
Gerth 0:24628832f21d 168 //call scopedata
Gerth 0:24628832f21d 169 if (scopedata_go==true) {
Gerth 0:24628832f21d 170 scopedata();
Gerth 0:24628832f21d 171 scopedata_go=false;
Gerth 0:24628832f21d 172 }
Gerth 0:24628832f21d 173 }
Gerth 0:24628832f21d 174 return 0;
Gerth 0:24628832f21d 175 }