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@5:be9f9dcbd9b0, 2015-10-15 (annotated)

Committer:
Gerth
Date:
Thu Oct 15 09:51:08 2015 +0000
Revision:
5:be9f9dcbd9b0
Parent:
4:5243e66b1fea
Child:
6:2d67144f217b
Child:
7:8482ea098a37
change controllervalues with serial working;

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 4:5243e66b1fea 32 controlandadjust mycontroller;//the value between () is the maximum value of the PWM signal send to the motors
Gerth 5:be9f9dcbd9b0 33 float Kp=0.5;
Gerth 5:be9f9dcbd9b0 34 float Ki=0.01;
Gerth 5:be9f9dcbd9b0 35 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 4:5243e66b1fea 48 const float radpersecbut=((0.1*PI)/read_but_frequency);
Gerth 4:5243e66b1fea 49 const float schiethoek=0.35*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 5:be9f9dcbd9b0 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 4:5243e66b1fea 104 scopedata();
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 5:be9f9dcbd9b0 115 void changecontrollervalues()
Gerth 5:be9f9dcbd9b0 116 {
Gerth 5:be9f9dcbd9b0 117 mycontroller.STOP();
Gerth 5:be9f9dcbd9b0 118
Gerth 5:be9f9dcbd9b0 119 pc.printf("KP is now %f, enter new value\n",Kp);
Gerth 5:be9f9dcbd9b0 120 pc.scanf("%f", &Kp);
Gerth 5:be9f9dcbd9b0 121
Gerth 5:be9f9dcbd9b0 122 pc.printf("KI is now %f, enter new value\n",Ki);
Gerth 5:be9f9dcbd9b0 123 pc.scanf("%f", &Ki);
Gerth 5:be9f9dcbd9b0 124
Gerth 5:be9f9dcbd9b0 125 pc.printf("KD is now %f, enter new value\n",Kd);
Gerth 5:be9f9dcbd9b0 126 pc.scanf("%f", &Kd);
Gerth 5:be9f9dcbd9b0 127 }
Gerth 0:24628832f21d 128 //////////////////////////////////////////////////MAIN///////////////////////////////////
Gerth 0:24628832f21d 129 int main()
Gerth 0:24628832f21d 130 {
Gerth 4:5243e66b1fea 131 mycontroller.cutoff(0.5);
Gerth 3:bb7b6034bb7c 132 pc.baud(115200);
Gerth 0:24628832f21d 133 //tickers
Gerth 0:24628832f21d 134 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 2:91bf9f1765ef 135 control_ticker.attach(&control_activate,1.0/control_frequency);
Gerth 0:24628832f21d 136 read_but_ticker.attach(&read_but_activate,1.0/read_but_frequency);
Gerth 0:24628832f21d 137
Gerth 0:24628832f21d 138 while(1) {
Gerth 2:91bf9f1765ef 139 //read buttons and adjust angles
Gerth 0:24628832f21d 140 if (read_but_go==true) {
Gerth 0:24628832f21d 141 float buttonR=butR.read(), buttonL=butL.read();
Gerth 0:24628832f21d 142 if (buttonR==0 && buttonL ==1) {
Gerth 0:24628832f21d 143 desiredposition += (radpersecbut);
Gerth 0:24628832f21d 144 read_but_go=false;
Gerth 0:24628832f21d 145 }
Gerth 0:24628832f21d 146 if (buttonR==1 && buttonL==0) {
Gerth 0:24628832f21d 147 desiredposition -= (radpersecbut);
Gerth 0:24628832f21d 148 read_but_go=false;
Gerth 2:91bf9f1765ef 149 }
Gerth 2:91bf9f1765ef 150 //schieten
Gerth 2:91bf9f1765ef 151 if (buttonR==0 && buttonL==0) {
Gerth 2:91bf9f1765ef 152 schieten();
Gerth 2:91bf9f1765ef 153 read_but_go==false;
Gerth 0:24628832f21d 154 } else {
Gerth 0:24628832f21d 155 desiredposition=desiredposition;
Gerth 0:24628832f21d 156 }
Gerth 0:24628832f21d 157 desiredangle[0]=desiredposition;
Gerth 0:24628832f21d 158 desiredangle[1]=desiredposition;
Gerth 2:91bf9f1765ef 159
Gerth 0:24628832f21d 160 read_but_go=false;
Gerth 0:24628832f21d 161 }
Gerth 2:91bf9f1765ef 162 //control motors with pi controller
Gerth 2:91bf9f1765ef 163 if (control_go==true) {
Gerth 2:91bf9f1765ef 164 error1=(desiredangle[0]-counttorad*encoder1.getPulses());
Gerth 2:91bf9f1765ef 165 error2=(desiredangle[1]-counttorad*encoder2.getPulses());
Gerth 2:91bf9f1765ef 166 mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int,error2_int);
Gerth 2:91bf9f1765ef 167 control_go=false;
Gerth 0:24628832f21d 168 }
Gerth 0:24628832f21d 169 //call scopedata
Gerth 0:24628832f21d 170 if (scopedata_go==true) {
Gerth 0:24628832f21d 171 scopedata();
Gerth 0:24628832f21d 172 scopedata_go=false;
Gerth 0:24628832f21d 173 }
Gerth 1:41a9e3340c96 174 //unit om controllervalues aan te passen
Gerth 5:be9f9dcbd9b0 175 changecontrollervaluesbutton.fall(&changecontrollervalues);
Gerth 0:24628832f21d 176 }
Gerth 0:24628832f21d 177 return 0;
Gerth 0:24628832f21d 178 }