Complete burgerboy3000 code with commentary
Dependencies: HIDScope MODSERIAL QEI mbed
Fork of Burgerboy3000code by
main.cpp
00001 #include "mbed.h" 00002 #include "HIDScope.h" 00003 #include "MODSERIAL.h" 00004 #include "QEI.h" 00005 00006 DigitalOut led_g(LED_GREEN); 00007 DigitalOut led_b(LED_BLUE); 00008 DigitalOut led_r(LED_RED); 00009 00010 DigitalOut M1_Rotate(D2); // voltage only base rotation 00011 PwmOut M1_Speed(D3); // voltage only base rotation 00012 00013 MODSERIAL pc(USBTX, USBRX); 00014 00015 //QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING) 00016 QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING); 00017 QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING); 00018 00019 //Motor control outputs 00020 DigitalOut M2_Rotate(D4); // encoder side pot 2 translation 00021 PwmOut M2_Speed(D5); // encoder side pot 2 translation 00022 DigitalOut M3_Rotate(D7); // encoder side pot 1 spatel rotation 00023 PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation 00024 00025 //Booleans for simplifying the EMG control 00026 bool left; 00027 bool right; 00028 00029 //EMG related inputs and outputs 00030 HIDScope scope( 2 ); 00031 AnalogIn emg0( A0 ); 00032 AnalogIn emg1( A1 ); 00033 DigitalIn buttonCalibrate(SW3); 00034 DigitalIn buttonCalibrateComplete(SW2); 00035 00036 //Variables for control 00037 bool turnRight; 00038 bool turnLeft; 00039 bool turn = 0; 00040 float waiter = 0.12; 00041 float translation = 0; //initialise values at 0 00042 float degrees3 = 0; 00043 00044 float Puls_degree = (8400/360); 00045 float wheel1 = 16; 00046 float wheel2 = 31; 00047 float wheel3 = 41; 00048 float transmission = ((wheel2/wheel1)*(wheel3/wheel1)); 00049 float pi = 3.14159265359; 00050 00051 //Global filter variables 00052 volatile float x; 00053 volatile float x_prev =0; 00054 volatile float b; // filtered 'output' of ReadAnalogInAndFilter 00055 const double a1 = -1.6475; 00056 const double a2 = 0.7009; 00057 const double b0 = 0.8371; 00058 const double b1 = -1.6742; 00059 const double b2 = 0.8371; 00060 const double c1 = -1.9645; 00061 const double c2 = 0.9651; 00062 const double d0 = 0.0001551; 00063 const double d1 = 0.0003103; 00064 const double d2 = 0.0001551; 00065 double v1_HR = 0; 00066 double v2_HR = 0; 00067 double v1_LR = 0; 00068 double v2_LR = 0; 00069 double v1_HL = 0; 00070 double v2_HL = 0; 00071 double v1_LL = 0; 00072 double v2_LL = 0; 00073 double highpassFilterLeft = 0; 00074 double lowpassFilterLeft = 0; 00075 double highpassFilterRight = 0; 00076 double lowpassFilterRight = 0; 00077 00078 //calibration 00079 bool calibrate = false; 00080 bool calibrate_complete = false; 00081 double threshold_Left = 0; 00082 double threshold_Right= 0; 00083 00084 00085 //Tickers 00086 Ticker sample_timer; 00087 Ticker printinfo; 00088 Ticker checkSetpointTranslation; 00089 Ticker checkSetpointRotation; 00090 00091 //LED for testing the code 00092 DigitalOut led(LED1); 00093 00094 00095 //setpoints 00096 volatile float setpointRotation; 00097 volatile float setpointTranslation; 00098 const double Setpoint_Translation = -300; 00099 const double Setpoint_Back = 0; 00100 const double Setpoint_Rotation = pi; 00101 double M3_ControlSpeed = 0; 00102 double M2_ControlSpeed = 0; 00103 double SetpointError_Translation = 0; 00104 double SetpointError_Rotation = 0; 00105 double theta_translation; 00106 double theta_rotation; 00107 00108 //Variables for storing previous setpoints (for calculation average setpoint over 6 previously measured values) 00109 int counter = 0; 00110 double Setpoint1 = 0; 00111 double Setpoint2 = 0; 00112 double Setpoint3 = 0; 00113 double Setpoint4 = 0; 00114 double Setpoint5 = 0; 00115 double SetpointAvg = 0; 00116 00117 //booleans acting as 'Go-Flags' 00118 bool booltranslate = false; 00119 bool boolrotate = false; 00120 00121 //Arm PID 00122 const double Ts = 0.001953125; //Ts=1/fs (sample frequency) 00123 const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4; 00124 double Translation_error = 0; 00125 double Translation_e_prev = 0; 00126 00127 //Spatel PID 00128 const double Rotation_Kp = 0.23, Rotation_Ki = 0.0429 , Rotation_Kd = 2; 00129 double Rotation_error = 0; 00130 double Rotation_e_prev = 0; 00131 00132 //Pid calculation (reusable) 00133 double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev) 00134 { 00135 double e_der = (error - e_prev) / Ts; 00136 e_prev = error; 00137 e_int = e_int + (Ts * error); 00138 00139 return kp*error + ki + e_int + kd + e_der; 00140 } 00141 00142 //biquad calculation (reusable) 00143 double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0, 00144 const double b1, const double b2) 00145 { 00146 double v = u - a1*v1 - a2*v2; 00147 double y = b0*v + b1*v1 + b2*v2; 00148 v2 = v1; 00149 v1 = v; 00150 return y; 00151 } 00152 00153 //sample function, samples and processes through a highpassfilter, rectifier and lowpassfilter. 00154 void filterSample() 00155 { 00156 highpassFilterLeft = fabs(biquad(emg0.read(), v1_HL, v2_HL, a1, a2, b0, b1, b2)); 00157 lowpassFilterLeft = biquad(highpassFilterLeft, v1_LL, v2_LL, c1, c2, d0, d1, d2); 00158 //pc.printf("%f \n \r ", lowpassFilter); 00159 highpassFilterRight = fabs(biquad(emg1.read(), v1_HR, v2_HR, a1, a2, b0, b1, b2)); 00160 lowpassFilterRight = biquad(highpassFilterRight, v1_LR, v2_LR, c1, c2, d0, d1, d2); 00161 scope.set(0, lowpassFilterLeft ); 00162 scope.set(1, lowpassFilterRight ); 00163 scope.send(); 00164 //pc.printf("%f \n \r ", lowpassFilter); 00165 } 00166 00167 //Getting the positions of moving parts by processing the motor angles 00168 float GetPositionM2() 00169 { 00170 float pulses2 = motor2.getPulses(); 00171 float degrees2 = (pulses2/Puls_degree); 00172 float radians2 = (degrees2/360)*2*pi; 00173 float translation = ((radians2/transmission)*32.25); 00174 00175 return translation; 00176 } 00177 float GetRotationM3() 00178 { 00179 float pulses3 = motor3.getPulses(); 00180 float degrees3 = (pulses3/Puls_degree); 00181 float radians3 = (degrees3/360)*2*pi; 00182 00183 return radians3; 00184 } 00185 00186 // check the error at current sample, and every 50 samples sample for an average (to be used in the controllers) 00187 void CheckErrorRotation() 00188 { 00189 counter++; 00190 if (counter > 50) { 00191 theta_rotation = GetRotationM3(); 00192 Setpoint5 = Setpoint4; 00193 Setpoint4 = Setpoint3; 00194 Setpoint3 = Setpoint2; 00195 Setpoint2 = Setpoint1; 00196 Setpoint1 = SetpointError_Rotation; 00197 counter = 0; 00198 } 00199 SetpointError_Rotation = setpointRotation -theta_rotation; 00200 00201 SetpointAvg = ((SetpointError_Rotation + Setpoint1 + Setpoint2 + Setpoint3 + Setpoint4 + Setpoint5)/6); 00202 00203 } 00204 // check the error at current sample (Translation does well enough without averaging 00205 void CheckErrorTranslation() 00206 { 00207 theta_translation = GetPositionM2(); 00208 SetpointError_Translation = setpointTranslation -theta_translation; 00209 } 00210 00211 //Controller function for the rotation of the spatula 00212 void motorRotation() 00213 { 00214 printf("setpoint = %f \n\r", setpointRotation); 00215 //set direction 00216 if (SetpointError_Rotation > 0) { //if rotation exceeds the setpoint, turn in the other direction 00217 M3_Rotate = 0; 00218 } else { 00219 M3_Rotate = 1; 00220 00221 } 00222 double speedfactor = 1; 00223 //when on the way back (Setpoint is not pi, but 0) go at a lower speed for higher accuracy 00224 if (setpointRotation != Setpoint_Rotation) { 00225 speedfactor = 0.3; 00226 } 00227 //the way back has to be more precise. On the way up it doesn't really matter at which point the spatula stops. (as long as it's somewhere high) 00228 double tolerance = 0.1; 00229 if (setpointRotation == Setpoint_Rotation){ 00230 tolerance = 1; 00231 } 00232 00233 //control action with 'speedfactor' 00234 M3_ControlSpeed = speedfactor * Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev)); 00235 if (fabs(SetpointAvg) < 0.1) { //when average error over the last samples is low enough, stop the motor 00236 M3_ControlSpeed = 0; 00237 } 00238 if (theta_rotation > tolerance) //if the angle is within tolerance, give the correct Go-Flag. 00239 boolrotate = true; 00240 if ((fabs(theta_rotation) < tolerance ) && (M3_ControlSpeed == 0)) 00241 boolrotate = false; 00242 M3_Speed = M3_ControlSpeed; 00243 } 00244 00245 //controller for the translation, similar to the rotation but easier because it does not use speedfactors and variable tolerances et cetera 00246 void motorTranslation() 00247 { 00248 theta_translation = GetPositionM2(); 00249 SetpointError_Translation = setpointTranslation - theta_translation; 00250 00251 //set direction 00252 if (SetpointError_Translation < 0) { 00253 M2_Rotate = 0; 00254 } else { 00255 M2_Rotate = 1; 00256 } 00257 M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev)); 00258 if (fabs(SetpointError_Translation) < 8) { 00259 M2_ControlSpeed = 0; 00260 00261 } 00262 if ((theta_translation < -292) && (M2_ControlSpeed == 0)) 00263 booltranslate = true; 00264 if ((theta_translation > -8) && (M2_ControlSpeed == 0)) 00265 booltranslate = false; 00266 M2_Speed = M2_ControlSpeed; 00267 00268 } 00269 //from now on the position of motors can be easily controlled by calling the previous two functions and changing the setpoints. 00270 00271 00272 00273 //go back, so the setpoint will change to Setpoint_Back which is 0 00274 void GoBack() 00275 { 00276 setpointTranslation = Setpoint_Back; //setting the setpoint 00277 motorTranslation(); //executing the action 00278 if (booltranslate == false) { 00279 setpointRotation = Setpoint_Back; 00280 motorRotation(); 00281 } 00282 if (boolrotate == false) { 00283 turn = 0; 00284 } 00285 led_r = 1; 00286 led_b = 0; 00287 } 00288 00289 //Same as GoBack but forward 00290 void Burgerflip() 00291 { 00292 led_r = 0; 00293 led_b = 1; 00294 setpointTranslation = Setpoint_Translation; 00295 motorTranslation(); 00296 if (booltranslate == true) { //this is a Go-Flag, for when the translation is complete. When this is the case the spatula can rotate. 00297 setpointRotation = Setpoint_Rotation; //setting the setpoint 00298 motorRotation(); //executing the action 00299 } 00300 } 00301 void BurgerflipActie() //a simple function which calls on the previous two, two perform the 2 actions after each other 00302 { 00303 Burgerflip(); 00304 if (boolrotate == true) { 00305 GoBack(); 00306 } 00307 } 00308 void print() //a print function which can be attached to a ticker to read out the positions 00309 { 00310 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); 00311 } 00312 00313 00314 //Getting the directions. This means reading out the input and setting the correct booleans which are used to control the motors 00315 void GetDirections() 00316 { 00317 00318 //booleans based on EMG input 00319 if (lowpassFilterRight < threshold_Right) 00320 right = 0; 00321 if (lowpassFilterRight > threshold_Right) 00322 right = 1; 00323 if (lowpassFilterLeft < threshold_Left) 00324 left = 0; 00325 if (lowpassFilterLeft > threshold_Left) 00326 left = 1; 00327 pc.baud(115200); 00328 00329 //based on the EMG inputs and the boolean 'turn' (which is a go flag for the burger flip action) turnLeft and turnRight are set. 00330 //turnLeft and turnRight control the base motor which rotates the entire robot. 00331 if ((right == 1) && (left == 1) && (turn == 0)) { 00332 turnLeft = 0; 00333 turnRight = 0; 00334 turn = 1; 00335 pc.printf("start action \n \r "); 00336 wait(waiter); 00337 00338 } else if ((right == 1) && (left == 1) && (turn == 1)) { 00339 turnLeft = 0; 00340 turnRight = 0; 00341 turn = 0; 00342 pc.printf("cancel action \n \r "); 00343 GoBack(); 00344 wait(waiter); 00345 } else if ((right == 0) && (left == 0)&& (turn == 0)) { 00346 00347 } else if ((right == 1) && (turnLeft == 0)&& (turn == 0)) { 00348 /* if the right button is pressed and the motor isn't rotating to the left, 00349 then start rotating to the right etc*/ 00350 turnRight = !turnRight; 00351 pc.printf("turn right \n \r "); 00352 wait(waiter); 00353 } else if ((right == 1) && (turnLeft == 1)&& (turn == 0)) { 00354 turnLeft = 0; 00355 turnRight = !turnRight; 00356 pc.printf("turn right after left \n \r "); 00357 wait(waiter); 00358 } else if ((left == 1) && (turnRight == 0)&& (turn == 0)) { 00359 turnLeft = !turnLeft; 00360 pc.printf("turn left \n \r "); 00361 wait(waiter); 00362 } else if ((left == 1) && (turnRight == 1) && (turn == 0)) { 00363 turnRight = 0; 00364 turnLeft = !turnLeft; 00365 pc.printf("turn left after right \n \r "); 00366 wait(waiter); 00367 } 00368 wait(2*waiter); 00369 } 00370 int main() 00371 { 00372 //setting leds 00373 led_g = 1; 00374 led_b = 1; 00375 led_r = 1; 00376 00377 /**Attach the 'sample' function to the timer 'sample_timer'. 00378 * this ensures that 'sample' is executed every... 0.001953125 seconds = 512 Hz 00379 */ 00380 //sample_timer.attach(&sample, 0.001953125); 00381 sample_timer.attach(&filterSample, Ts); 00382 checkSetpointTranslation.attach(&CheckErrorTranslation,Ts); 00383 checkSetpointRotation.attach(&CheckErrorRotation,Ts); 00384 00385 //printinfo.attach(&print, Ts); 00386 pc.baud(115200); 00387 pc.printf("please push the button to calibrate \n \r"); 00388 while (1) { 00389 00390 //The main function starts with some calibration steps 00391 if (buttonCalibrate == 0) { 00392 calibrate = true; 00393 threshold_Left = lowpassFilterLeft*0.9; //at the moment of the button press, the current EMG values are stored and multiplied by 0.9 as a threshold value. 00394 threshold_Right = lowpassFilterRight*0.9; 00395 pc.printf("calibration complete, press to continue \n \r"); 00396 } 00397 if ((buttonCalibrateComplete == 0) && (calibrate == true)) { 00398 calibrate_complete = true; 00399 } 00400 if (calibrate_complete == true) { 00401 00402 //After calibrating, the booleans turn, turnRight and turnLeft are evaluated and based on that the motors are controlled. 00403 00404 pc.printf("rotation is %f, setpoint %f, error = %f en translation = %f en de error %f \n \r", GetRotationM3(), Setpoint_Back, SetpointError_Rotation, GetPositionM2(), SetpointError_Translation); 00405 GetDirections(); 00406 if (turnRight == true) { 00407 M1_Speed = 0.1; //turn to the right 00408 M1_Rotate = 1; 00409 } else if (turnLeft == true) { 00410 M1_Speed = 0.1; //turn to the left 00411 M1_Rotate = 0; 00412 } else if (turn == 1) { 00413 BurgerflipActie(); //flip the burger 00414 } else if (turn == 0) { 00415 M2_Speed = 0; //do not flip the burger 00416 M3_Speed = 0; 00417 } 00418 if ((turnLeft == false) && (turnRight == false)) { 00419 M1_Speed = 0; //do nothing 00420 00421 } 00422 00423 } 00424 } 00425 }
Generated on Tue Aug 9 2022 12:41:28 by 1.7.2