Wouter ter Veldhuis / Mbed 2 deprecated Burgerboy3000code

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI mbed

Fork of Burgerboy3000code by Timo de Vries

main.cpp@33:db4fc2f642f0, 2016-11-07 (annotated)

Committer:
WterVeldhuis
Date:
Mon Nov 07 12:42:58 2016 +0000
Revision:
33:db4fc2f642f0
Parent:
32:952f3f30a0cd 
really really final code with commentary

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
Frostworks 21:2b55d53e11f6 3 #include "MODSERIAL.h"
Frostworks 23:fdde3e4b9e69 4 #include "QEI.h"
Frostworks 23:fdde3e4b9e69 5
Frostworks 23:fdde3e4b9e69 6 DigitalOut led_g(LED_GREEN);
Frostworks 23:fdde3e4b9e69 7 DigitalOut led_b(LED_BLUE);
Frostworks 23:fdde3e4b9e69 8 DigitalOut led_r(LED_RED);
Frostworks 23:fdde3e4b9e69 9
Frostworks 23:fdde3e4b9e69 10 DigitalOut M1_Rotate(D2); // voltage only base rotation
Frostworks 23:fdde3e4b9e69 11 PwmOut M1_Speed(D3); // voltage only base rotation
Frostworks 23:fdde3e4b9e69 12
Frostworks 23:fdde3e4b9e69 13 MODSERIAL pc(USBTX, USBRX);
Frostworks 23:fdde3e4b9e69 14
Frostworks 23:fdde3e4b9e69 15 //QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
Frostworks 23:fdde3e4b9e69 16 QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
Frostworks 23:fdde3e4b9e69 17 QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);
Frostworks 23:fdde3e4b9e69 18
WterVeldhuis 32:952f3f30a0cd 19 //Motor control outputs
Frostworks 23:fdde3e4b9e69 20 DigitalOut M2_Rotate(D4); // encoder side pot 2 translation
Frostworks 23:fdde3e4b9e69 21 PwmOut M2_Speed(D5); // encoder side pot 2 translation
Frostworks 23:fdde3e4b9e69 22 DigitalOut M3_Rotate(D7); // encoder side pot 1 spatel rotation
Frostworks 23:fdde3e4b9e69 23 PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation
Frostworks 23:fdde3e4b9e69 24
WterVeldhuis 32:952f3f30a0cd 25 //Booleans for simplifying the EMG control
WterVeldhuis 33:db4fc2f642f0 26 bool left;
WterVeldhuis 33:db4fc2f642f0 27 bool right;
Frostworks 23:fdde3e4b9e69 28
WterVeldhuis 32:952f3f30a0cd 29 //EMG related inputs and outputs
Frostworks 30:492595db0fc3 30 HIDScope scope( 2 );
tomlankhorst 19:2bf824669684 31 AnalogIn emg0( A0 );
tomlankhorst 19:2bf824669684 32 AnalogIn emg1( A1 );
Frostworks 30:492595db0fc3 33 DigitalIn buttonCalibrate(SW3);
Frostworks 31:21a112643dc9 34 DigitalIn buttonCalibrateComplete(SW2);
Frostworks 23:fdde3e4b9e69 35
WterVeldhuis 32:952f3f30a0cd 36 //Variables for control
WterVeldhuis 32:952f3f30a0cd 37 bool turnRight;
WterVeldhuis 32:952f3f30a0cd 38 bool turnLeft;
Frostworks 23:fdde3e4b9e69 39 bool turn = 0;
Frostworks 31:21a112643dc9 40 float waiter = 0.12;
WterVeldhuis 32:952f3f30a0cd 41 float translation = 0; //initialise values at 0
Frostworks 23:fdde3e4b9e69 42 float degrees3 = 0;
Frostworks 23:fdde3e4b9e69 43
Frostworks 23:fdde3e4b9e69 44 float Puls_degree = (8400/360);
Frostworks 23:fdde3e4b9e69 45 float wheel1 = 16;
Frostworks 23:fdde3e4b9e69 46 float wheel2 = 31;
Frostworks 23:fdde3e4b9e69 47 float wheel3 = 41;
WterVeldhuis 33:db4fc2f642f0 48 float transmission = ((wheel2/wheel1)*(wheel3/wheel1));
Frostworks 23:fdde3e4b9e69 49 float pi = 3.14159265359;
Frostworks 21:2b55d53e11f6 50
WterVeldhuis 32:952f3f30a0cd 51 //Global filter variables
Frostworks 21:2b55d53e11f6 52 volatile float x;
Frostworks 21:2b55d53e11f6 53 volatile float x_prev =0;
Frostworks 21:2b55d53e11f6 54 volatile float b; // filtered 'output' of ReadAnalogInAndFilter
Frostworks 21:2b55d53e11f6 55 const double a1 = -1.6475;
Frostworks 21:2b55d53e11f6 56 const double a2 = 0.7009;
Frostworks 21:2b55d53e11f6 57 const double b0 = 0.8371;
Frostworks 21:2b55d53e11f6 58 const double b1 = -1.6742;
Frostworks 21:2b55d53e11f6 59 const double b2 = 0.8371;
Frostworks 21:2b55d53e11f6 60 const double c1 = -1.9645;
Frostworks 21:2b55d53e11f6 61 const double c2 = 0.9651;
Frostworks 21:2b55d53e11f6 62 const double d0 = 0.0001551;
Frostworks 21:2b55d53e11f6 63 const double d1 = 0.0003103;
Frostworks 21:2b55d53e11f6 64 const double d2 = 0.0001551;
Frostworks 30:492595db0fc3 65 double v1_HR = 0;
Frostworks 30:492595db0fc3 66 double v2_HR = 0;
Frostworks 30:492595db0fc3 67 double v1_LR = 0;
Frostworks 30:492595db0fc3 68 double v2_LR = 0;
Frostworks 30:492595db0fc3 69 double v1_HL = 0;
Frostworks 30:492595db0fc3 70 double v2_HL = 0;
Frostworks 30:492595db0fc3 71 double v1_LL = 0;
Frostworks 30:492595db0fc3 72 double v2_LL = 0;
Frostworks 22:ad85b8acf8b5 73 double highpassFilterLeft = 0;
Frostworks 22:ad85b8acf8b5 74 double lowpassFilterLeft = 0;
Frostworks 22:ad85b8acf8b5 75 double highpassFilterRight = 0;
Frostworks 22:ad85b8acf8b5 76 double lowpassFilterRight = 0;
vsluiter 2:e314bb3b2d99 77
WterVeldhuis 32:952f3f30a0cd 78 //calibration
WterVeldhuis 32:952f3f30a0cd 79 bool calibrate = false;
WterVeldhuis 32:952f3f30a0cd 80 bool calibrate_complete = false;
WterVeldhuis 32:952f3f30a0cd 81 double threshold_Left = 0;
WterVeldhuis 32:952f3f30a0cd 82 double threshold_Right= 0;
WterVeldhuis 32:952f3f30a0cd 83
WterVeldhuis 32:952f3f30a0cd 84
WterVeldhuis 32:952f3f30a0cd 85 //Tickers
WterVeldhuis 32:952f3f30a0cd 86 Ticker sample_timer;
WterVeldhuis 32:952f3f30a0cd 87 Ticker printinfo;
WterVeldhuis 32:952f3f30a0cd 88 Ticker checkSetpointTranslation;
WterVeldhuis 32:952f3f30a0cd 89 Ticker checkSetpointRotation;
WterVeldhuis 32:952f3f30a0cd 90
WterVeldhuis 32:952f3f30a0cd 91 //LED for testing the code
WterVeldhuis 32:952f3f30a0cd 92 DigitalOut led(LED1);
WterVeldhuis 32:952f3f30a0cd 93
WterVeldhuis 32:952f3f30a0cd 94
Frostworks 26:c640851fa1e7 95 //setpoints
Frostworks 29:b6d7bcb26f47 96 volatile float setpointRotation;
Frostworks 29:b6d7bcb26f47 97 volatile float setpointTranslation;
WterVeldhuis 33:db4fc2f642f0 98 const double Setpoint_Translation = -300;
Frostworks 27:16327d1337cf 99 const double Setpoint_Back = 0;
Frostworks 27:16327d1337cf 100 const double Setpoint_Rotation = pi;
Frostworks 26:c640851fa1e7 101 double M3_ControlSpeed = 0;
Frostworks 26:c640851fa1e7 102 double M2_ControlSpeed = 0;
Frostworks 27:16327d1337cf 103 double SetpointError_Translation = 0;
Frostworks 27:16327d1337cf 104 double SetpointError_Rotation = 0;
Frostworks 29:b6d7bcb26f47 105 double theta_translation;
Frostworks 29:b6d7bcb26f47 106 double theta_rotation;
Frostworks 31:21a112643dc9 107
WterVeldhuis 32:952f3f30a0cd 108 //Variables for storing previous setpoints (for calculation average setpoint over 6 previously measured values)
Frostworks 31:21a112643dc9 109 int counter = 0;
Frostworks 31:21a112643dc9 110 double Setpoint1 = 0;
Frostworks 31:21a112643dc9 111 double Setpoint2 = 0;
Frostworks 31:21a112643dc9 112 double Setpoint3 = 0;
Frostworks 31:21a112643dc9 113 double Setpoint4 = 0;
Frostworks 31:21a112643dc9 114 double Setpoint5 = 0;
Frostworks 31:21a112643dc9 115 double SetpointAvg = 0;
WterVeldhuis 32:952f3f30a0cd 116
WterVeldhuis 32:952f3f30a0cd 117 //booleans acting as 'Go-Flags'
Frostworks 28:d265c64d2bca 118 bool booltranslate = false;
Frostworks 28:d265c64d2bca 119 bool boolrotate = false;
WterVeldhuis 32:952f3f30a0cd 120
Frostworks 26:c640851fa1e7 121 //Arm PID
Frostworks 26:c640851fa1e7 122 const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
Frostworks 28:d265c64d2bca 123 const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
Frostworks 26:c640851fa1e7 124 double Translation_error = 0;
Frostworks 26:c640851fa1e7 125 double Translation_e_prev = 0;
Frostworks 24:bdd74b91abbb 126
Frostworks 26:c640851fa1e7 127 //Spatel PID
Frostworks 31:21a112643dc9 128 const double Rotation_Kp = 0.23, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
Frostworks 26:c640851fa1e7 129 double Rotation_error = 0;
Frostworks 26:c640851fa1e7 130 double Rotation_e_prev = 0;
Frostworks 26:c640851fa1e7 131
WterVeldhuis 32:952f3f30a0cd 132 //Pid calculation (reusable)
Frostworks 26:c640851fa1e7 133 double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
Frostworks 24:bdd74b91abbb 134 {
Frostworks 26:c640851fa1e7 135 double e_der = (error - e_prev) / Ts;
Frostworks 26:c640851fa1e7 136 e_prev = error;
Frostworks 26:c640851fa1e7 137 e_int = e_int + (Ts * error);
Frostworks 26:c640851fa1e7 138
Frostworks 26:c640851fa1e7 139 return kp*error + ki + e_int + kd + e_der;
Frostworks 24:bdd74b91abbb 140 }
Frostworks 24:bdd74b91abbb 141
WterVeldhuis 32:952f3f30a0cd 142 //biquad calculation (reusable)
Frostworks 30:492595db0fc3 143 double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
Frostworks 30:492595db0fc3 144 const double b1, const double b2)
Frostworks 21:2b55d53e11f6 145 {
Frostworks 21:2b55d53e11f6 146 double v = u - a1*v1 - a2*v2;
Frostworks 21:2b55d53e11f6 147 double y = b0*v + b1*v1 + b2*v2;
Frostworks 21:2b55d53e11f6 148 v2 = v1;
Frostworks 21:2b55d53e11f6 149 v1 = v;
Frostworks 21:2b55d53e11f6 150 return y;
Frostworks 21:2b55d53e11f6 151 }
Frostworks 24:bdd74b91abbb 152
WterVeldhuis 32:952f3f30a0cd 153 //sample function, samples and processes through a highpassfilter, rectifier and lowpassfilter.
Frostworks 30:492595db0fc3 154 void filterSample()
Frostworks 21:2b55d53e11f6 155 {
Frostworks 30:492595db0fc3 156 highpassFilterLeft = fabs(biquad(emg0.read(), v1_HL, v2_HL, a1, a2, b0, b1, b2));
Frostworks 30:492595db0fc3 157 lowpassFilterLeft = biquad(highpassFilterLeft, v1_LL, v2_LL, c1, c2, d0, d1, d2);
Frostworks 30:492595db0fc3 158 //pc.printf("%f \n \r ", lowpassFilter);
Frostworks 30:492595db0fc3 159 highpassFilterRight = fabs(biquad(emg1.read(), v1_HR, v2_HR, a1, a2, b0, b1, b2));
Frostworks 30:492595db0fc3 160 lowpassFilterRight = biquad(highpassFilterRight, v1_LR, v2_LR, c1, c2, d0, d1, d2);
Frostworks 22:ad85b8acf8b5 161 scope.set(0, lowpassFilterLeft );
Frostworks 22:ad85b8acf8b5 162 scope.set(1, lowpassFilterRight );
Frostworks 22:ad85b8acf8b5 163 scope.send();
Frostworks 22:ad85b8acf8b5 164 //pc.printf("%f \n \r ", lowpassFilter);
Frostworks 22:ad85b8acf8b5 165 }
Frostworks 24:bdd74b91abbb 166
WterVeldhuis 32:952f3f30a0cd 167 //Getting the positions of moving parts by processing the motor angles
Frostworks 23:fdde3e4b9e69 168 float GetPositionM2()
Frostworks 23:fdde3e4b9e69 169 {
Frostworks 23:fdde3e4b9e69 170 float pulses2 = motor2.getPulses();
Frostworks 23:fdde3e4b9e69 171 float degrees2 = (pulses2/Puls_degree);
Frostworks 23:fdde3e4b9e69 172 float radians2 = (degrees2/360)*2*pi;
WterVeldhuis 33:db4fc2f642f0 173 float translation = ((radians2/transmission)*32.25);
Frostworks 23:fdde3e4b9e69 174
Frostworks 23:fdde3e4b9e69 175 return translation;
Frostworks 23:fdde3e4b9e69 176 }
Frostworks 23:fdde3e4b9e69 177 float GetRotationM3()
Frostworks 23:fdde3e4b9e69 178 {
Frostworks 23:fdde3e4b9e69 179 float pulses3 = motor3.getPulses();
Frostworks 23:fdde3e4b9e69 180 float degrees3 = (pulses3/Puls_degree);
Frostworks 23:fdde3e4b9e69 181 float radians3 = (degrees3/360)*2*pi;
Frostworks 23:fdde3e4b9e69 182
Frostworks 27:16327d1337cf 183 return radians3;
Frostworks 27:16327d1337cf 184 }
WterVeldhuis 32:952f3f30a0cd 185
WterVeldhuis 32:952f3f30a0cd 186 // check the error at current sample, and every 50 samples sample for an average (to be used in the controllers)
Frostworks 30:492595db0fc3 187 void CheckErrorRotation()
Frostworks 30:492595db0fc3 188 {
Frostworks 31:21a112643dc9 189 counter++;
Frostworks 31:21a112643dc9 190 if (counter > 50) {
Frostworks 31:21a112643dc9 191 theta_rotation = GetRotationM3();
Frostworks 31:21a112643dc9 192 Setpoint5 = Setpoint4;
Frostworks 31:21a112643dc9 193 Setpoint4 = Setpoint3;
Frostworks 31:21a112643dc9 194 Setpoint3 = Setpoint2;
Frostworks 31:21a112643dc9 195 Setpoint2 = Setpoint1;
Frostworks 31:21a112643dc9 196 Setpoint1 = SetpointError_Rotation;
Frostworks 31:21a112643dc9 197 counter = 0;
Frostworks 31:21a112643dc9 198 }
Frostworks 29:b6d7bcb26f47 199 SetpointError_Rotation = setpointRotation -theta_rotation;
Frostworks 31:21a112643dc9 200
Frostworks 31:21a112643dc9 201 SetpointAvg = ((SetpointError_Rotation + Setpoint1 + Setpoint2 + Setpoint3 + Setpoint4 + Setpoint5)/6);
Frostworks 31:21a112643dc9 202
Frostworks 29:b6d7bcb26f47 203 }
WterVeldhuis 32:952f3f30a0cd 204 // check the error at current sample (Translation does well enough without averaging
Frostworks 30:492595db0fc3 205 void CheckErrorTranslation()
Frostworks 30:492595db0fc3 206 {
Frostworks 29:b6d7bcb26f47 207 theta_translation = GetPositionM2();
Frostworks 29:b6d7bcb26f47 208 SetpointError_Translation = setpointTranslation -theta_translation;
Frostworks 29:b6d7bcb26f47 209 }
WterVeldhuis 32:952f3f30a0cd 210
WterVeldhuis 32:952f3f30a0cd 211 //Controller function for the rotation of the spatula
Frostworks 29:b6d7bcb26f47 212 void motorRotation()
Frostworks 27:16327d1337cf 213 {
Frostworks 30:492595db0fc3 214 printf("setpoint = %f \n\r", setpointRotation);
Frostworks 27:16327d1337cf 215 //set direction
WterVeldhuis 32:952f3f30a0cd 216 if (SetpointError_Rotation > 0) { //if rotation exceeds the setpoint, turn in the other direction
Frostworks 27:16327d1337cf 217 M3_Rotate = 0;
Frostworks 27:16327d1337cf 218 } else {
Frostworks 27:16327d1337cf 219 M3_Rotate = 1;
Frostworks 27:16327d1337cf 220
Frostworks 27:16327d1337cf 221 }
Frostworks 31:21a112643dc9 222 double speedfactor = 1;
WterVeldhuis 32:952f3f30a0cd 223 //when on the way back (Setpoint is not pi, but 0) go at a lower speed for higher accuracy
Frostworks 31:21a112643dc9 224 if (setpointRotation != Setpoint_Rotation) {
WterVeldhuis 32:952f3f30a0cd 225 speedfactor = 0.3;
Frostworks 31:21a112643dc9 226 }
WterVeldhuis 32:952f3f30a0cd 227 //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)
Frostworks 31:21a112643dc9 228 double tolerance = 0.1;
Frostworks 31:21a112643dc9 229 if (setpointRotation == Setpoint_Rotation){
Frostworks 31:21a112643dc9 230 tolerance = 1;
WterVeldhuis 32:952f3f30a0cd 231 }
WterVeldhuis 32:952f3f30a0cd 232
WterVeldhuis 32:952f3f30a0cd 233 //control action with 'speedfactor'
Frostworks 31:21a112643dc9 234 M3_ControlSpeed = speedfactor * Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
WterVeldhuis 32:952f3f30a0cd 235 if (fabs(SetpointAvg) < 0.1) { //when average error over the last samples is low enough, stop the motor
Frostworks 27:16327d1337cf 236 M3_ControlSpeed = 0;
Frostworks 27:16327d1337cf 237 }
WterVeldhuis 32:952f3f30a0cd 238 if (theta_rotation > tolerance) //if the angle is within tolerance, give the correct Go-Flag.
Frostworks 28:d265c64d2bca 239 boolrotate = true;
Frostworks 31:21a112643dc9 240 if ((fabs(theta_rotation) < tolerance ) && (M3_ControlSpeed == 0))
Frostworks 28:d265c64d2bca 241 boolrotate = false;
Frostworks 30:492595db0fc3 242 M3_Speed = M3_ControlSpeed;
Frostworks 27:16327d1337cf 243 }
WterVeldhuis 32:952f3f30a0cd 244
WterVeldhuis 32:952f3f30a0cd 245 //controller for the translation, similar to the rotation but easier because it does not use speedfactors and variable tolerances et cetera
Frostworks 29:b6d7bcb26f47 246 void motorTranslation()
Frostworks 27:16327d1337cf 247 {
Frostworks 29:b6d7bcb26f47 248 theta_translation = GetPositionM2();
Frostworks 29:b6d7bcb26f47 249 SetpointError_Translation = setpointTranslation - theta_translation;
Frostworks 27:16327d1337cf 250
Frostworks 27:16327d1337cf 251 //set direction
Frostworks 27:16327d1337cf 252 if (SetpointError_Translation < 0) {
Frostworks 27:16327d1337cf 253 M2_Rotate = 0;
Frostworks 27:16327d1337cf 254 } else {
Frostworks 27:16327d1337cf 255 M2_Rotate = 1;
Frostworks 27:16327d1337cf 256 }
Frostworks 27:16327d1337cf 257 M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
Frostworks 31:21a112643dc9 258 if (fabs(SetpointError_Translation) < 8) {
Frostworks 27:16327d1337cf 259 M2_ControlSpeed = 0;
Frostworks 30:492595db0fc3 260
Frostworks 27:16327d1337cf 261 }
WterVeldhuis 33:db4fc2f642f0 262 if ((theta_translation < -292) && (M2_ControlSpeed == 0))
Frostworks 28:d265c64d2bca 263 booltranslate = true;
Frostworks 31:21a112643dc9 264 if ((theta_translation > -8) && (M2_ControlSpeed == 0))
Frostworks 28:d265c64d2bca 265 booltranslate = false;
Frostworks 27:16327d1337cf 266 M2_Speed = M2_ControlSpeed;
Frostworks 30:492595db0fc3 267
Frostworks 23:fdde3e4b9e69 268 }
WterVeldhuis 32:952f3f30a0cd 269 //from now on the position of motors can be easily controlled by calling the previous two functions and changing the setpoints.
WterVeldhuis 32:952f3f30a0cd 270
WterVeldhuis 32:952f3f30a0cd 271
WterVeldhuis 32:952f3f30a0cd 272
WterVeldhuis 32:952f3f30a0cd 273 //go back, so the setpoint will change to Setpoint_Back which is 0
Frostworks 23:fdde3e4b9e69 274 void GoBack()
Frostworks 23:fdde3e4b9e69 275 {
WterVeldhuis 32:952f3f30a0cd 276 setpointTranslation = Setpoint_Back; //setting the setpoint
WterVeldhuis 32:952f3f30a0cd 277 motorTranslation(); //executing the action
Frostworks 28:d265c64d2bca 278 if (booltranslate == false) {
Frostworks 29:b6d7bcb26f47 279 setpointRotation = Setpoint_Back;
Frostworks 29:b6d7bcb26f47 280 motorRotation();
Frostworks 28:d265c64d2bca 281 }
Frostworks 28:d265c64d2bca 282 if (boolrotate == false) {
Frostworks 28:d265c64d2bca 283 turn = 0;
Frostworks 28:d265c64d2bca 284 }
Frostworks 27:16327d1337cf 285 led_r = 1;
Frostworks 27:16327d1337cf 286 led_b = 0;
Frostworks 24:bdd74b91abbb 287 }
Frostworks 23:fdde3e4b9e69 288
WterVeldhuis 32:952f3f30a0cd 289 //Same as GoBack but forward
Frostworks 23:fdde3e4b9e69 290 void Burgerflip()
Frostworks 23:fdde3e4b9e69 291 {
Frostworks 27:16327d1337cf 292 led_r = 0;
Frostworks 27:16327d1337cf 293 led_b = 1;
Frostworks 29:b6d7bcb26f47 294 setpointTranslation = Setpoint_Translation;
Frostworks 29:b6d7bcb26f47 295 motorTranslation();
WterVeldhuis 32:952f3f30a0cd 296 if (booltranslate == true) { //this is a Go-Flag, for when the translation is complete. When this is the case the spatula can rotate.
WterVeldhuis 32:952f3f30a0cd 297 setpointRotation = Setpoint_Rotation; //setting the setpoint
WterVeldhuis 32:952f3f30a0cd 298 motorRotation(); //executing the action
Frostworks 28:d265c64d2bca 299 }
Frostworks 27:16327d1337cf 300 }
WterVeldhuis 32:952f3f30a0cd 301 void BurgerflipActie() //a simple function which calls on the previous two, two perform the 2 actions after each other
Frostworks 28:d265c64d2bca 302 {
Frostworks 28:d265c64d2bca 303 Burgerflip();
Frostworks 28:d265c64d2bca 304 if (boolrotate == true) {
Frostworks 28:d265c64d2bca 305 GoBack();
Frostworks 28:d265c64d2bca 306 }
Frostworks 28:d265c64d2bca 307 }
WterVeldhuis 32:952f3f30a0cd 308 void print() //a print function which can be attached to a ticker to read out the positions
Frostworks 27:16327d1337cf 309 {
Frostworks 27:16327d1337cf 310 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
Frostworks 23:fdde3e4b9e69 311 }
WterVeldhuis 32:952f3f30a0cd 312
WterVeldhuis 32:952f3f30a0cd 313
WterVeldhuis 32:952f3f30a0cd 314 //Getting the directions. This means reading out the input and setting the correct booleans which are used to control the motors
Frostworks 31:21a112643dc9 315 void GetDirections()
Frostworks 31:21a112643dc9 316 {
WterVeldhuis 32:952f3f30a0cd 317
WterVeldhuis 32:952f3f30a0cd 318 //booleans based on EMG input
Frostworks 31:21a112643dc9 319 if (lowpassFilterRight < threshold_Right)
WterVeldhuis 33:db4fc2f642f0 320 right = 0;
Frostworks 31:21a112643dc9 321 if (lowpassFilterRight > threshold_Right)
WterVeldhuis 33:db4fc2f642f0 322 right = 1;
Frostworks 31:21a112643dc9 323 if (lowpassFilterLeft < threshold_Left)
WterVeldhuis 33:db4fc2f642f0 324 left = 0;
Frostworks 31:21a112643dc9 325 if (lowpassFilterLeft > threshold_Left)
WterVeldhuis 33:db4fc2f642f0 326 left = 1;
Frostworks 31:21a112643dc9 327 pc.baud(115200);
WterVeldhuis 32:952f3f30a0cd 328
WterVeldhuis 32:952f3f30a0cd 329 //based on the EMG inputs and the boolean 'turn' (which is a go flag for the burger flip action) turnLeft and turnRight are set.
WterVeldhuis 32:952f3f30a0cd 330 //turnLeft and turnRight control the base motor which rotates the entire robot.
WterVeldhuis 33:db4fc2f642f0 331 if ((right == 1) && (left == 1) && (turn == 0)) {
WterVeldhuis 32:952f3f30a0cd 332 turnLeft = 0;
WterVeldhuis 32:952f3f30a0cd 333 turnRight = 0;
Frostworks 31:21a112643dc9 334 turn = 1;
WterVeldhuis 33:db4fc2f642f0 335 pc.printf("start action \n \r ");
Frostworks 31:21a112643dc9 336 wait(waiter);
Frostworks 31:21a112643dc9 337
WterVeldhuis 33:db4fc2f642f0 338 } else if ((right == 1) && (left == 1) && (turn == 1)) {
WterVeldhuis 32:952f3f30a0cd 339 turnLeft = 0;
WterVeldhuis 32:952f3f30a0cd 340 turnRight = 0;
Frostworks 31:21a112643dc9 341 turn = 0;
WterVeldhuis 33:db4fc2f642f0 342 pc.printf("cancel action \n \r ");
Frostworks 31:21a112643dc9 343 GoBack();
Frostworks 31:21a112643dc9 344 wait(waiter);
WterVeldhuis 33:db4fc2f642f0 345 } else if ((right == 0) && (left == 0)&& (turn == 0)) {
Frostworks 31:21a112643dc9 346
WterVeldhuis 33:db4fc2f642f0 347 } else if ((right == 1) && (turnLeft == 0)&& (turn == 0)) {
Frostworks 31:21a112643dc9 348 /* if the right button is pressed and the motor isn't rotating to the left,
Frostworks 31:21a112643dc9 349 then start rotating to the right etc*/
WterVeldhuis 32:952f3f30a0cd 350 turnRight = !turnRight;
WterVeldhuis 33:db4fc2f642f0 351 pc.printf("turn right \n \r ");
Frostworks 31:21a112643dc9 352 wait(waiter);
WterVeldhuis 33:db4fc2f642f0 353 } else if ((right == 1) && (turnLeft == 1)&& (turn == 0)) {
WterVeldhuis 32:952f3f30a0cd 354 turnLeft = 0;
WterVeldhuis 32:952f3f30a0cd 355 turnRight = !turnRight;
WterVeldhuis 33:db4fc2f642f0 356 pc.printf("turn right after left \n \r ");
Frostworks 31:21a112643dc9 357 wait(waiter);
WterVeldhuis 33:db4fc2f642f0 358 } else if ((left == 1) && (turnRight == 0)&& (turn == 0)) {
WterVeldhuis 32:952f3f30a0cd 359 turnLeft = !turnLeft;
WterVeldhuis 33:db4fc2f642f0 360 pc.printf("turn left \n \r ");
Frostworks 31:21a112643dc9 361 wait(waiter);
WterVeldhuis 33:db4fc2f642f0 362 } else if ((left == 1) && (turnRight == 1) && (turn == 0)) {
WterVeldhuis 32:952f3f30a0cd 363 turnRight = 0;
WterVeldhuis 32:952f3f30a0cd 364 turnLeft = !turnLeft;
WterVeldhuis 33:db4fc2f642f0 365 pc.printf("turn left after right \n \r ");
Frostworks 31:21a112643dc9 366 wait(waiter);
Frostworks 31:21a112643dc9 367 }
Frostworks 31:21a112643dc9 368 wait(2*waiter);
Frostworks 31:21a112643dc9 369 }
vsluiter 0:32bb76391d89 370 int main()
Frostworks 21:2b55d53e11f6 371 {
WterVeldhuis 32:952f3f30a0cd 372 //setting leds
Frostworks 23:fdde3e4b9e69 373 led_g = 1;
Frostworks 23:fdde3e4b9e69 374 led_b = 1;
Frostworks 23:fdde3e4b9e69 375 led_r = 1;
Frostworks 23:fdde3e4b9e69 376
tomlankhorst 14:f83354387756 377 /**Attach the 'sample' function to the timer 'sample_timer'.
WterVeldhuis 32:952f3f30a0cd 378 * this ensures that 'sample' is executed every... 0.001953125 seconds = 512 Hz
vsluiter 4:8b298dfada81 379 */
Frostworks 22:ad85b8acf8b5 380 //sample_timer.attach(&sample, 0.001953125);
WterVeldhuis 32:952f3f30a0cd 381 sample_timer.attach(&filterSample, Ts);
Frostworks 29:b6d7bcb26f47 382 checkSetpointTranslation.attach(&CheckErrorTranslation,Ts);
Frostworks 29:b6d7bcb26f47 383 checkSetpointRotation.attach(&CheckErrorRotation,Ts);
Frostworks 31:21a112643dc9 384
Frostworks 27:16327d1337cf 385 //printinfo.attach(&print, Ts);
Frostworks 21:2b55d53e11f6 386 pc.baud(115200);
Frostworks 22:ad85b8acf8b5 387 pc.printf("please push the button to calibrate \n \r");
Frostworks 22:ad85b8acf8b5 388 while (1) {
WterVeldhuis 32:952f3f30a0cd 389
WterVeldhuis 32:952f3f30a0cd 390 //The main function starts with some calibration steps
Frostworks 22:ad85b8acf8b5 391 if (buttonCalibrate == 0) {
Frostworks 22:ad85b8acf8b5 392 calibrate = true;
WterVeldhuis 32:952f3f30a0cd 393 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.
Frostworks 30:492595db0fc3 394 threshold_Right = lowpassFilterRight*0.9;
Frostworks 31:21a112643dc9 395 pc.printf("calibration complete, press to continue \n \r");
Frostworks 21:2b55d53e11f6 396 }
Frostworks 31:21a112643dc9 397 if ((buttonCalibrateComplete == 0) && (calibrate == true)) {
Frostworks 31:21a112643dc9 398 calibrate_complete = true;
Frostworks 31:21a112643dc9 399 }
Frostworks 31:21a112643dc9 400 if (calibrate_complete == true) {
WterVeldhuis 32:952f3f30a0cd 401
WterVeldhuis 32:952f3f30a0cd 402 //After calibrating, the booleans turn, turnRight and turnLeft are evaluated and based on that the motors are controlled.
Frostworks 30:492595db0fc3 403
Frostworks 31:21a112643dc9 404 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);
Frostworks 23:fdde3e4b9e69 405 GetDirections();
WterVeldhuis 32:952f3f30a0cd 406 if (turnRight == true) {
WterVeldhuis 32:952f3f30a0cd 407 M1_Speed = 0.1; //turn to the right
Frostworks 31:21a112643dc9 408 M1_Rotate = 1;
WterVeldhuis 32:952f3f30a0cd 409 } else if (turnLeft == true) {
WterVeldhuis 32:952f3f30a0cd 410 M1_Speed = 0.1; //turn to the left
Frostworks 31:21a112643dc9 411 M1_Rotate = 0;
Frostworks 23:fdde3e4b9e69 412 } else if (turn == 1) {
WterVeldhuis 32:952f3f30a0cd 413 BurgerflipActie(); //flip the burger
Frostworks 23:fdde3e4b9e69 414 } else if (turn == 0) {
WterVeldhuis 32:952f3f30a0cd 415 M2_Speed = 0; //do not flip the burger
Frostworks 23:fdde3e4b9e69 416 M3_Speed = 0;
Frostworks 23:fdde3e4b9e69 417 }
WterVeldhuis 32:952f3f30a0cd 418 if ((turnLeft == false) && (turnRight == false)) {
WterVeldhuis 32:952f3f30a0cd 419 M1_Speed = 0; //do nothing
Frostworks 30:492595db0fc3 420
Frostworks 23:fdde3e4b9e69 421 }
Frostworks 30:492595db0fc3 422
Frostworks 22:ad85b8acf8b5 423 }
Frostworks 22:ad85b8acf8b5 424 }
vsluiter 0:32bb76391d89 425 }