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@4:5243e66b1fea, 2015-10-14 (annotated)

Committer:
Gerth
Date:
Wed Oct 14 15:22:49 2015 +0000
Revision:
4:5243e66b1fea
Parent:
3:bb7b6034bb7c
Child:
5:be9f9dcbd9b0
working, slightly higher pwm;

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