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@1:41a9e3340c96, 2015-10-02 (annotated)

Committer:
Gerth
Date:
Fri Oct 02 09:02:08 2015 +0000
Revision:
1:41a9e3340c96
Parent:
0:24628832f21d
Child:
2:91bf9f1765ef
oke, niet opschonen. schietfunctie inbouwen;

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