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@3:bb7b6034bb7c, 2015-10-08 (annotated)

Committer:
Gerth
Date:
Thu Oct 08 14:04:55 2015 +0000
Revision:
3:bb7b6034bb7c
Parent:
2:91bf9f1765ef
Child:
4:5243e66b1fea
made the shoot function wait so the motors don't crash;

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 2:91bf9f1765ef 4 #include "controlandadjust.h"
Gerth 0:24628832f21d 5
Gerth 0:24628832f21d 6 //////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
Gerth 0:24628832f21d 7 //info uit
Gerth 0:24628832f21d 8 HIDScope scope(5);
Gerth 1:41a9e3340c96 9 Serial pc(USBTX, USBRX);
Gerth 0:24628832f21d 10
Gerth 0:24628832f21d 11 //encoders
Gerth 0:24628832f21d 12 QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
Gerth 0:24628832f21d 13 QEI encoder2 (D10,D11,NC,32);
Gerth 0:24628832f21d 14
Gerth 0:24628832f21d 15 //ingangen
Gerth 0:24628832f21d 16 DigitalIn butR(D2);
Gerth 0:24628832f21d 17 DigitalIn butL(D3);
Gerth 2:91bf9f1765ef 18 InterruptIn changecontrollervaluesbutton(PTA4);//button to adjus controllervalues
Gerth 0:24628832f21d 19
Gerth 0:24628832f21d 20 //frequencies
Gerth 0:24628832f21d 21 const double hidscope_frequency=100;//frequentie waarop data naar HIDScope wordt gestuurd
Gerth 2:91bf9f1765ef 22 const double control_frequency=25;//frequentie waarop er een control actie wordt uitgevoerd. kan hoger, voorzichtig bij ruis!
Gerth 0:24628832f21d 23 const double read_but_frequency=25;
Gerth 0:24628832f21d 24
Gerth 0:24628832f21d 25 //tickers
Gerth 0:24628832f21d 26 Ticker hidscope_ticker;
Gerth 2:91bf9f1765ef 27 Ticker control_ticker;
Gerth 0:24628832f21d 28 Ticker read_but_ticker;
Gerth 2:91bf9f1765ef 29 Timeout schieten_timeout;
Gerth 2:91bf9f1765ef 30
Gerth 2:91bf9f1765ef 31 // controller values and errors
Gerth 2:91bf9f1765ef 32 controlandadjust mycontroller;
Gerth 2:91bf9f1765ef 33 const float Kp=0.5;
Gerth 2:91bf9f1765ef 34 const float Ki=0.01;
Gerth 2:91bf9f1765ef 35 const float Kd=0.001;
Gerth 2:91bf9f1765ef 36 const float Ts_control=1.0/control_frequency;
Gerth 2:91bf9f1765ef 37 float error1=0;
Gerth 2:91bf9f1765ef 38 float error2=0;
Gerth 2:91bf9f1765ef 39 float error1_int=0;
Gerth 2:91bf9f1765ef 40 float error2_int=0;
Gerth 2:91bf9f1765ef 41 float error1_prev=0;
Gerth 2:91bf9f1765ef 42 float error2_prec=0;
Gerth 0:24628832f21d 43
Gerth 0:24628832f21d 44 //constants
Gerth 0:24628832f21d 45 const float cpr=32*131;
Gerth 0:24628832f21d 46 const float PI=3.1415;
Gerth 0:24628832f21d 47 const float counttorad=((2*PI)/cpr);
Gerth 0:24628832f21d 48 const float radpersecbut=(PI/read_but_frequency);
Gerth 2:91bf9f1765ef 49 const float schiethoek=0.5*PI;
Gerth 2:91bf9f1765ef 50 const float schiettijd=0.5;
Gerth 0:24628832f21d 51
Gerth 2:91bf9f1765ef 52 //angle for motor
Gerth 2:91bf9f1765ef 53 float desiredangle[]= {0,0};
Gerth 2:91bf9f1765ef 54 float desiredposition=0;
Gerth 0:24628832f21d 55 ////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
Gerth 0:24628832f21d 56 //go flags
Gerth 2:91bf9f1765ef 57 volatile bool scopedata_go=false, control_go=false,read_but_go=false;
Gerth 0:24628832f21d 58
Gerth 0:24628832f21d 59 //acvitator functions
Gerth 0:24628832f21d 60
Gerth 0:24628832f21d 61 void scopedata_activate()
Gerth 0:24628832f21d 62 {
Gerth 0:24628832f21d 63 scopedata_go=true;
Gerth 0:24628832f21d 64 }
Gerth 2:91bf9f1765ef 65 void control_activate()
Gerth 0:24628832f21d 66 {
Gerth 2:91bf9f1765ef 67 control_go=true;
Gerth 0:24628832f21d 68 }
Gerth 0:24628832f21d 69 void read_but_activate()
Gerth 0:24628832f21d 70 {
Gerth 0:24628832f21d 71 read_but_go=true;
Gerth 0:24628832f21d 72 }
Gerth 0:24628832f21d 73 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
Gerth 0:24628832f21d 74
Gerth 0:24628832f21d 75 //scopedata
Gerth 0:24628832f21d 76 void scopedata()
Gerth 0:24628832f21d 77 {
Gerth 0:24628832f21d 78 scope.set(0,desiredposition);//gewenste hoek van arm
Gerth 2:91bf9f1765ef 79 scope.set(1,counttorad*encoder1.getPulses());//hoek in rad van outputshaft
Gerth 2:91bf9f1765ef 80 scope.set(2,mycontroller.motor1pwm());//pwm signaal naar motor toe
Gerth 2:91bf9f1765ef 81 scope.set(3,counttorad*encoder2.getPulses());//hoek in rad van outputshaft
Gerth 2:91bf9f1765ef 82 scope.set(4,mycontroller.motor2pwm());//pwm signaal naar motor toe
Gerth 0:24628832f21d 83 scope.send();
Gerth 0:24628832f21d 84 }
Gerth 2:91bf9f1765ef 85
Gerth 2:91bf9f1765ef 86
Gerth 2:91bf9f1765ef 87
Gerth 2:91bf9f1765ef 88 void schieten()
Gerth 2:91bf9f1765ef 89 {
Gerth 2:91bf9f1765ef 90 pc.printf("SHOOT\n");
Gerth 2:91bf9f1765ef 91 //hoeken groter maken
Gerth 2:91bf9f1765ef 92 desiredangle[0]-=schiethoek;
Gerth 2:91bf9f1765ef 93 desiredangle[1]+=schiethoek;
Gerth 0:24628832f21d 94
Gerth 2:91bf9f1765ef 95 Timer schiettimer;
Gerth 2:91bf9f1765ef 96 schiettimer.reset();
Gerth 2:91bf9f1765ef 97 schiettimer.start();
Gerth 3:bb7b6034bb7c 98 float pass=0;
Gerth 3:bb7b6034bb7c 99 while(schiettimer.read()<=schiettijd) {
Gerth 3:bb7b6034bb7c 100 // errors berekenen en naar de controller passen
Gerth 2:91bf9f1765ef 101 error1=(desiredangle[0]-counttorad*encoder1.getPulses());
Gerth 2:91bf9f1765ef 102 error2=(desiredangle[1]-counttorad*encoder2.getPulses());
Gerth 2:91bf9f1765ef 103 mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int, error2_int);
Gerth 3:bb7b6034bb7c 104
Gerth 3:bb7b6034bb7c 105 wait (Ts_control-(schiettimer.read()-Ts_control*pass)); // even wachten anders wordt de while loop te snel doorlopen en gaan de motoren wak
Gerth 3:bb7b6034bb7c 106 pass++;
Gerth 2:91bf9f1765ef 107 }
Gerth 2:91bf9f1765ef 108 schiettimer.stop();
Gerth 0:24628832f21d 109
Gerth 2:91bf9f1765ef 110 //terug na schieten
Gerth 2:91bf9f1765ef 111 desiredangle[0]+=schiethoek;
Gerth 2:91bf9f1765ef 112 desiredangle[1]-=schiethoek;
Gerth 0:24628832f21d 113 }
Gerth 0:24628832f21d 114
Gerth 0:24628832f21d 115 //////////////////////////////////////////////////MAIN///////////////////////////////////
Gerth 0:24628832f21d 116 int main()
Gerth 0:24628832f21d 117 {
Gerth 3:bb7b6034bb7c 118 pc.baud(115200);
Gerth 0:24628832f21d 119 //tickers
Gerth 0:24628832f21d 120 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 2:91bf9f1765ef 121 control_ticker.attach(&control_activate,1.0/control_frequency);
Gerth 0:24628832f21d 122 read_but_ticker.attach(&read_but_activate,1.0/read_but_frequency);
Gerth 0:24628832f21d 123
Gerth 0:24628832f21d 124 while(1) {
Gerth 2:91bf9f1765ef 125 //read buttons and adjust angles
Gerth 0:24628832f21d 126 if (read_but_go==true) {
Gerth 0:24628832f21d 127 float buttonR=butR.read(), buttonL=butL.read();
Gerth 0:24628832f21d 128 if (buttonR==0 && buttonL ==1) {
Gerth 0:24628832f21d 129 desiredposition += (radpersecbut);
Gerth 0:24628832f21d 130 read_but_go=false;
Gerth 0:24628832f21d 131 }
Gerth 0:24628832f21d 132 if (buttonR==1 && buttonL==0) {
Gerth 0:24628832f21d 133 desiredposition -= (radpersecbut);
Gerth 0:24628832f21d 134 read_but_go=false;
Gerth 2:91bf9f1765ef 135 }
Gerth 2:91bf9f1765ef 136 //schieten
Gerth 2:91bf9f1765ef 137 if (buttonR==0 && buttonL==0) {
Gerth 2:91bf9f1765ef 138 schieten();
Gerth 2:91bf9f1765ef 139 read_but_go==false;
Gerth 0:24628832f21d 140 } else {
Gerth 0:24628832f21d 141 desiredposition=desiredposition;
Gerth 0:24628832f21d 142 }
Gerth 0:24628832f21d 143 desiredangle[0]=desiredposition;
Gerth 0:24628832f21d 144 desiredangle[1]=desiredposition;
Gerth 2:91bf9f1765ef 145
Gerth 0:24628832f21d 146 read_but_go=false;
Gerth 0:24628832f21d 147 }
Gerth 2:91bf9f1765ef 148 //control motors with pi controller
Gerth 2:91bf9f1765ef 149 if (control_go==true) {
Gerth 2:91bf9f1765ef 150 error1=(desiredangle[0]-counttorad*encoder1.getPulses());
Gerth 2:91bf9f1765ef 151 error2=(desiredangle[1]-counttorad*encoder2.getPulses());
Gerth 2:91bf9f1765ef 152 mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int,error2_int);
Gerth 2:91bf9f1765ef 153 control_go=false;
Gerth 0:24628832f21d 154 }
Gerth 0:24628832f21d 155 //call scopedata
Gerth 0:24628832f21d 156 if (scopedata_go==true) {
Gerth 0:24628832f21d 157 scopedata();
Gerth 0:24628832f21d 158 scopedata_go=false;
Gerth 0:24628832f21d 159 }
Gerth 1:41a9e3340c96 160 //unit om controllervalues aan te passen
Gerth 2:91bf9f1765ef 161 if (changecontrollervaluesbutton.read()==0) {
Gerth 2:91bf9f1765ef 162 mycontroller.STOP();
Gerth 2:91bf9f1765ef 163 pc.printf("KP is now %f, enter new value\n",Kp);
Gerth 2:91bf9f1765ef 164 pc.scanf("%f", &Kp);
Gerth 1:41a9e3340c96 165
Gerth 2:91bf9f1765ef 166 pc.printf("KI is now %f, enter new value\n",Ki);
Gerth 2:91bf9f1765ef 167 pc.scanf("%f", &Ki);
Gerth 2:91bf9f1765ef 168
Gerth 2:91bf9f1765ef 169 pc.printf("KD is now %f, enter new value\n",Kd);
Gerth 2:91bf9f1765ef 170 pc.scanf("%f", &Kd);
Gerth 1:41a9e3340c96 171 }
Gerth 0:24628832f21d 172 }
Gerth 0:24628832f21d 173 return 0;
Gerth 0:24628832f21d 174 }