Timo de Vries / Mbed 2 deprecated Burgerboy3000code

This is the final code

Dependencies:   HIDScope MODSERIAL QEI mbed

Fork of WearealltogetherEMGboi by Timo de Vries

main.cpp@31:21a112643dc9, 2016-11-07 (annotated)

Committer:
Frostworks
Date:
Mon Nov 07 11:33:59 2016 +0000
Revision:
31:21a112643dc9
Parent:
30:492595db0fc3 
Endwork

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
Frostworks 23:fdde3e4b9e69 19 DigitalOut M2_Rotate(D4); // encoder side pot 2 translation
Frostworks 23:fdde3e4b9e69 20 PwmOut M2_Speed(D5); // encoder side pot 2 translation
Frostworks 23:fdde3e4b9e69 21
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
Frostworks 30:492595db0fc3 25 bool links;
Frostworks 30:492595db0fc3 26 bool rechts;
Frostworks 23:fdde3e4b9e69 27
Frostworks 23:fdde3e4b9e69 28 AnalogIn pot1(A4); // pot 1 motor 1
Frostworks 23:fdde3e4b9e69 29 AnalogIn pot2(A3); // pot 2 motor 3
vsluiter 0:32bb76391d89 30
vsluiter 4:8b298dfada81 31 //Define objects
Frostworks 30:492595db0fc3 32 HIDScope scope( 2 );
tomlankhorst 19:2bf824669684 33 AnalogIn emg0( A0 );
tomlankhorst 19:2bf824669684 34 AnalogIn emg1( A1 );
Frostworks 30:492595db0fc3 35 DigitalIn buttonCalibrate(SW3);
Frostworks 31:21a112643dc9 36 DigitalIn buttonCalibrateComplete(SW2);
Frostworks 23:fdde3e4b9e69 37
Frostworks 23:fdde3e4b9e69 38 bool draairechts;
Frostworks 23:fdde3e4b9e69 39 bool draailinks;
Frostworks 23:fdde3e4b9e69 40 bool turn = 0;
Frostworks 31:21a112643dc9 41 float waiter = 0.12;
Frostworks 23:fdde3e4b9e69 42 float translation = 0;
Frostworks 23:fdde3e4b9e69 43 float degrees3 = 0;
Frostworks 23:fdde3e4b9e69 44
Frostworks 23:fdde3e4b9e69 45 float Puls_degree = (8400/360);
Frostworks 23:fdde3e4b9e69 46 float wheel1 = 16;
Frostworks 23:fdde3e4b9e69 47 float wheel2 = 31;
Frostworks 23:fdde3e4b9e69 48 float wheel3 = 41;
Frostworks 23:fdde3e4b9e69 49 float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
Frostworks 23:fdde3e4b9e69 50 float pi = 3.14159265359;
Frostworks 21:2b55d53e11f6 51
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
Frostworks 22:ad85b8acf8b5 56 bool calibrate = false;
Frostworks 31:21a112643dc9 57 bool calibrate_complete = false;
Frostworks 22:ad85b8acf8b5 58 double threshold_Left = 0;
Frostworks 22:ad85b8acf8b5 59 double threshold_Right= 0;
tomlankhorst 14:f83354387756 60 Ticker sample_timer;
Frostworks 27:16327d1337cf 61 Ticker printinfo;
Frostworks 29:b6d7bcb26f47 62 Ticker checkSetpointTranslation;
Frostworks 29:b6d7bcb26f47 63 Ticker checkSetpointRotation;
tomlankhorst 18:21d8e7a81cf5 64 DigitalOut led(LED1);
Frostworks 21:2b55d53e11f6 65 const double a1 = -1.6475;
Frostworks 21:2b55d53e11f6 66 const double a2 = 0.7009;
Frostworks 21:2b55d53e11f6 67 const double b0 = 0.8371;
Frostworks 21:2b55d53e11f6 68 const double b1 = -1.6742;
Frostworks 21:2b55d53e11f6 69 const double b2 = 0.8371;
Frostworks 21:2b55d53e11f6 70 const double c1 = -1.9645;
Frostworks 21:2b55d53e11f6 71 const double c2 = 0.9651;
Frostworks 21:2b55d53e11f6 72 const double d0 = 0.0001551;
Frostworks 21:2b55d53e11f6 73 const double d1 = 0.0003103;
Frostworks 21:2b55d53e11f6 74 const double d2 = 0.0001551;
Frostworks 30:492595db0fc3 75 double v1_HR = 0;
Frostworks 30:492595db0fc3 76 double v2_HR = 0;
Frostworks 30:492595db0fc3 77 double v1_LR = 0;
Frostworks 30:492595db0fc3 78 double v2_LR = 0;
Frostworks 30:492595db0fc3 79 double v1_HL = 0;
Frostworks 30:492595db0fc3 80 double v2_HL = 0;
Frostworks 30:492595db0fc3 81 double v1_LL = 0;
Frostworks 30:492595db0fc3 82 double v2_LL = 0;
Frostworks 22:ad85b8acf8b5 83 double highpassFilterLeft = 0;
Frostworks 22:ad85b8acf8b5 84 double lowpassFilterLeft = 0;
Frostworks 22:ad85b8acf8b5 85 double highpassFilterRight = 0;
Frostworks 22:ad85b8acf8b5 86 double lowpassFilterRight = 0;
vsluiter 2:e314bb3b2d99 87
Frostworks 26:c640851fa1e7 88 //setpoints
Frostworks 29:b6d7bcb26f47 89 volatile float setpointRotation;
Frostworks 29:b6d7bcb26f47 90 volatile float setpointTranslation;
Frostworks 27:16327d1337cf 91 const double Setpoint_Translation = -200;
Frostworks 27:16327d1337cf 92 const double Setpoint_Back = 0;
Frostworks 27:16327d1337cf 93 const double Setpoint_Rotation = pi;
Frostworks 26:c640851fa1e7 94 double M3_ControlSpeed = 0;
Frostworks 26:c640851fa1e7 95 double M2_ControlSpeed = 0;
Frostworks 27:16327d1337cf 96 double SetpointError_Translation = 0;
Frostworks 27:16327d1337cf 97 double SetpointError_Rotation = 0;
Frostworks 29:b6d7bcb26f47 98 double theta_translation;
Frostworks 29:b6d7bcb26f47 99 double theta_rotation;
Frostworks 31:21a112643dc9 100
Frostworks 31:21a112643dc9 101 //gemiddelde
Frostworks 31:21a112643dc9 102 int counter = 0;
Frostworks 31:21a112643dc9 103 double Setpoint1 = 0;
Frostworks 31:21a112643dc9 104 double Setpoint2 = 0;
Frostworks 31:21a112643dc9 105 double Setpoint3 = 0;
Frostworks 31:21a112643dc9 106 double Setpoint4 = 0;
Frostworks 31:21a112643dc9 107 double Setpoint5 = 0;
Frostworks 31:21a112643dc9 108 double SetpointAvg = 0;
Frostworks 28:d265c64d2bca 109 //booleans for control
Frostworks 28:d265c64d2bca 110 bool booltranslate = false;
Frostworks 28:d265c64d2bca 111 bool boolrotate = false;
Frostworks 26:c640851fa1e7 112 //copied from slides
Frostworks 26:c640851fa1e7 113 //Arm PID
Frostworks 26:c640851fa1e7 114 const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
Frostworks 28:d265c64d2bca 115 const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
Frostworks 26:c640851fa1e7 116 double Translation_error = 0;
Frostworks 26:c640851fa1e7 117 double Translation_e_prev = 0;
Frostworks 24:bdd74b91abbb 118
Frostworks 26:c640851fa1e7 119 //Spatel PID
Frostworks 31:21a112643dc9 120 const double Rotation_Kp = 0.23, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
Frostworks 31:21a112643dc9 121 //const double Rotation_Kp = 0.3, Rotation_Ki = 0.0429 , Rotation_Kd = 2; is best
Frostworks 31:21a112643dc9 122 //const double Rotation_Kp = 0.10, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
Frostworks 31:21a112643dc9 123 //const double Rotation_Kp = 0.05, Rotation_Ki = 0.0429 , Rotation_Kd = 0.4;
Frostworks 26:c640851fa1e7 124 double Rotation_error = 0;
Frostworks 26:c640851fa1e7 125 double Rotation_e_prev = 0;
Frostworks 26:c640851fa1e7 126
Frostworks 26:c640851fa1e7 127 double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
Frostworks 24:bdd74b91abbb 128 {
Frostworks 26:c640851fa1e7 129 double e_der = (error - e_prev) / Ts;
Frostworks 26:c640851fa1e7 130 e_prev = error;
Frostworks 26:c640851fa1e7 131 e_int = e_int + (Ts * error);
Frostworks 26:c640851fa1e7 132
Frostworks 26:c640851fa1e7 133 return kp*error + ki + e_int + kd + e_der;
Frostworks 24:bdd74b91abbb 134 }
Frostworks 24:bdd74b91abbb 135
Frostworks 30:492595db0fc3 136 double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
Frostworks 30:492595db0fc3 137 const double b1, const double b2)
Frostworks 21:2b55d53e11f6 138 {
Frostworks 21:2b55d53e11f6 139 double v = u - a1*v1 - a2*v2;
Frostworks 21:2b55d53e11f6 140 double y = b0*v + b1*v1 + b2*v2;
Frostworks 21:2b55d53e11f6 141 v2 = v1;
Frostworks 21:2b55d53e11f6 142 v1 = v;
Frostworks 21:2b55d53e11f6 143 return y;
Frostworks 21:2b55d53e11f6 144 }
Frostworks 24:bdd74b91abbb 145
tomlankhorst 14:f83354387756 146 /** Sample function
tomlankhorst 14:f83354387756 147 * this function samples the emg and sends it to HIDScope
tomlankhorst 14:f83354387756 148 **/
Frostworks 21:2b55d53e11f6 149
Frostworks 30:492595db0fc3 150 void filterSample()
Frostworks 21:2b55d53e11f6 151 {
Frostworks 30:492595db0fc3 152 highpassFilterLeft = fabs(biquad(emg0.read(), v1_HL, v2_HL, a1, a2, b0, b1, b2));
Frostworks 30:492595db0fc3 153 lowpassFilterLeft = biquad(highpassFilterLeft, v1_LL, v2_LL, c1, c2, d0, d1, d2);
Frostworks 30:492595db0fc3 154 //pc.printf("%f \n \r ", lowpassFilter);
Frostworks 30:492595db0fc3 155 highpassFilterRight = fabs(biquad(emg1.read(), v1_HR, v2_HR, a1, a2, b0, b1, b2));
Frostworks 30:492595db0fc3 156 lowpassFilterRight = biquad(highpassFilterRight, v1_LR, v2_LR, c1, c2, d0, d1, d2);
Frostworks 22:ad85b8acf8b5 157 scope.set(0, lowpassFilterLeft );
Frostworks 22:ad85b8acf8b5 158 scope.set(1, lowpassFilterRight );
Frostworks 22:ad85b8acf8b5 159 scope.send();
Frostworks 22:ad85b8acf8b5 160 //pc.printf("%f \n \r ", lowpassFilter);
Frostworks 22:ad85b8acf8b5 161 }
Frostworks 24:bdd74b91abbb 162
Frostworks 30:492595db0fc3 163 /*void sample()
vsluiter 2:e314bb3b2d99 164 {
Frostworks 24:bdd74b91abbb 165 // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
tomlankhorst 19:2bf824669684 166 scope.set(0, emg0.read() );
tomlankhorst 19:2bf824669684 167 scope.set(1, emg1.read() );
Frostworks 31:21a112643dc9 168 * Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
tomlankhorst 19:2bf824669684 169 * Ensure that enough channels are available (HIDScope scope( 2 ))
Frostworks 30:492595db0fc3 170 * Finally, send all channels to the PC at once
Frostworks 21:2b55d53e11f6 171
Frostworks 21:2b55d53e11f6 172 x = emg0; // Capture data scope.set(0, x); // store data in first element of scope memory
Frostworks 21:2b55d53e11f6 173 b = (x_prev + x)/2.0; // averaging filter
Frostworks 21:2b55d53e11f6 174 x_prev = x; // Prepare for next round
Frostworks 21:2b55d53e11f6 175
vsluiter 11:ce72ec658a95 176 scope.send();
Frostworks 24:bdd74b91abbb 177 // To indicate that the function is working, the LED is toggled
tomlankhorst 18:21d8e7a81cf5 178 led = !led;
Frostworks 21:2b55d53e11f6 179 pc.printf("%f, %f \n \r ", x, b);
Frostworks 30:492595db0fc3 180 } \*/
Frostworks 24:bdd74b91abbb 181
Frostworks 23:fdde3e4b9e69 182 float GetPositionM2()
Frostworks 23:fdde3e4b9e69 183 {
Frostworks 23:fdde3e4b9e69 184 float pulses2 = motor2.getPulses();
Frostworks 23:fdde3e4b9e69 185 float degrees2 = (pulses2/Puls_degree);
Frostworks 23:fdde3e4b9e69 186 float radians2 = (degrees2/360)*2*pi;
Frostworks 23:fdde3e4b9e69 187 float translation = ((radians2/overbrenging)*32.25);
Frostworks 23:fdde3e4b9e69 188
Frostworks 23:fdde3e4b9e69 189 return translation;
Frostworks 23:fdde3e4b9e69 190 }
Frostworks 23:fdde3e4b9e69 191 float GetRotationM3()
Frostworks 23:fdde3e4b9e69 192 {
Frostworks 23:fdde3e4b9e69 193 float pulses3 = motor3.getPulses();
Frostworks 23:fdde3e4b9e69 194 float degrees3 = (pulses3/Puls_degree);
Frostworks 23:fdde3e4b9e69 195 float radians3 = (degrees3/360)*2*pi;
Frostworks 23:fdde3e4b9e69 196
Frostworks 27:16327d1337cf 197 return radians3;
Frostworks 27:16327d1337cf 198 }
Frostworks 30:492595db0fc3 199 void CheckErrorRotation()
Frostworks 30:492595db0fc3 200 {
Frostworks 31:21a112643dc9 201 counter++;
Frostworks 31:21a112643dc9 202 if (counter > 50) {
Frostworks 31:21a112643dc9 203 theta_rotation = GetRotationM3();
Frostworks 31:21a112643dc9 204 Setpoint5 = Setpoint4;
Frostworks 31:21a112643dc9 205 Setpoint4 = Setpoint3;
Frostworks 31:21a112643dc9 206 Setpoint3 = Setpoint2;
Frostworks 31:21a112643dc9 207 Setpoint2 = Setpoint1;
Frostworks 31:21a112643dc9 208 Setpoint1 = SetpointError_Rotation;
Frostworks 31:21a112643dc9 209 counter = 0;
Frostworks 31:21a112643dc9 210 }
Frostworks 29:b6d7bcb26f47 211 SetpointError_Rotation = setpointRotation -theta_rotation;
Frostworks 31:21a112643dc9 212
Frostworks 31:21a112643dc9 213 SetpointAvg = ((SetpointError_Rotation + Setpoint1 + Setpoint2 + Setpoint3 + Setpoint4 + Setpoint5)/6);
Frostworks 31:21a112643dc9 214
Frostworks 29:b6d7bcb26f47 215 }
Frostworks 30:492595db0fc3 216 void CheckErrorTranslation()
Frostworks 30:492595db0fc3 217 {
Frostworks 29:b6d7bcb26f47 218 theta_translation = GetPositionM2();
Frostworks 29:b6d7bcb26f47 219 SetpointError_Translation = setpointTranslation -theta_translation;
Frostworks 29:b6d7bcb26f47 220 }
Frostworks 29:b6d7bcb26f47 221 void motorRotation()
Frostworks 27:16327d1337cf 222 {
Frostworks 30:492595db0fc3 223 printf("setpoint = %f \n\r", setpointRotation);
Frostworks 27:16327d1337cf 224 //set direction
Frostworks 27:16327d1337cf 225 if (SetpointError_Rotation > 0) {
Frostworks 27:16327d1337cf 226 M3_Rotate = 0;
Frostworks 27:16327d1337cf 227 } else {
Frostworks 27:16327d1337cf 228 M3_Rotate = 1;
Frostworks 27:16327d1337cf 229
Frostworks 27:16327d1337cf 230 }
Frostworks 31:21a112643dc9 231 double speedfactor = 1;
Frostworks 31:21a112643dc9 232 if (setpointRotation != Setpoint_Rotation) {
Frostworks 31:21a112643dc9 233 speedfactor = 0.3;
Frostworks 31:21a112643dc9 234 }
Frostworks 31:21a112643dc9 235 double tolerance = 0.1;
Frostworks 31:21a112643dc9 236 if (setpointRotation == Setpoint_Rotation){
Frostworks 31:21a112643dc9 237 tolerance = 1;
Frostworks 31:21a112643dc9 238 }
Frostworks 31:21a112643dc9 239 M3_ControlSpeed = speedfactor * Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
Frostworks 31:21a112643dc9 240 if (fabs(SetpointAvg) < 0.1) {
Frostworks 27:16327d1337cf 241 M3_ControlSpeed = 0;
Frostworks 27:16327d1337cf 242 }
Frostworks 31:21a112643dc9 243 if (theta_rotation > tolerance)
Frostworks 28:d265c64d2bca 244 boolrotate = true;
Frostworks 31:21a112643dc9 245 if ((fabs(theta_rotation) < tolerance ) && (M3_ControlSpeed == 0))
Frostworks 28:d265c64d2bca 246 boolrotate = false;
Frostworks 30:492595db0fc3 247 M3_Speed = M3_ControlSpeed;
Frostworks 27:16327d1337cf 248 }
Frostworks 29:b6d7bcb26f47 249 void motorTranslation()
Frostworks 27:16327d1337cf 250 {
Frostworks 29:b6d7bcb26f47 251 theta_translation = GetPositionM2();
Frostworks 29:b6d7bcb26f47 252 SetpointError_Translation = setpointTranslation - theta_translation;
Frostworks 27:16327d1337cf 253
Frostworks 27:16327d1337cf 254 //set direction
Frostworks 27:16327d1337cf 255 if (SetpointError_Translation < 0) {
Frostworks 27:16327d1337cf 256 M2_Rotate = 0;
Frostworks 27:16327d1337cf 257 } else {
Frostworks 27:16327d1337cf 258 M2_Rotate = 1;
Frostworks 27:16327d1337cf 259 }
Frostworks 27:16327d1337cf 260 M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
Frostworks 31:21a112643dc9 261 if (fabs(SetpointError_Translation) < 8) {
Frostworks 27:16327d1337cf 262 M2_ControlSpeed = 0;
Frostworks 30:492595db0fc3 263
Frostworks 27:16327d1337cf 264 }
Frostworks 31:21a112643dc9 265 if ((theta_translation < -192) && (M2_ControlSpeed == 0))
Frostworks 28:d265c64d2bca 266 booltranslate = true;
Frostworks 31:21a112643dc9 267 if ((theta_translation > -8) && (M2_ControlSpeed == 0))
Frostworks 28:d265c64d2bca 268 booltranslate = false;
Frostworks 27:16327d1337cf 269 M2_Speed = M2_ControlSpeed;
Frostworks 30:492595db0fc3 270
Frostworks 23:fdde3e4b9e69 271 }
Frostworks 23:fdde3e4b9e69 272 void GoBack()
Frostworks 23:fdde3e4b9e69 273 {
Frostworks 29:b6d7bcb26f47 274 setpointTranslation = Setpoint_Back;
Frostworks 29:b6d7bcb26f47 275 motorTranslation();
Frostworks 28:d265c64d2bca 276 if (booltranslate == false) {
Frostworks 29:b6d7bcb26f47 277 setpointRotation = Setpoint_Back;
Frostworks 29:b6d7bcb26f47 278 motorRotation();
Frostworks 28:d265c64d2bca 279 }
Frostworks 28:d265c64d2bca 280 if (boolrotate == false) {
Frostworks 28:d265c64d2bca 281 turn = 0;
Frostworks 28:d265c64d2bca 282 }
Frostworks 27:16327d1337cf 283 led_r = 1;
Frostworks 27:16327d1337cf 284 led_b = 0;
Frostworks 24:bdd74b91abbb 285 }
Frostworks 23:fdde3e4b9e69 286
Frostworks 23:fdde3e4b9e69 287 void Burgerflip()
Frostworks 23:fdde3e4b9e69 288 {
Frostworks 27:16327d1337cf 289 led_r = 0;
Frostworks 27:16327d1337cf 290 led_b = 1;
Frostworks 29:b6d7bcb26f47 291 setpointTranslation = Setpoint_Translation;
Frostworks 29:b6d7bcb26f47 292 motorTranslation();
Frostworks 28:d265c64d2bca 293 if (booltranslate == true) {
Frostworks 29:b6d7bcb26f47 294 setpointRotation = Setpoint_Rotation;
Frostworks 29:b6d7bcb26f47 295 motorRotation();
Frostworks 28:d265c64d2bca 296 }
Frostworks 27:16327d1337cf 297 }
Frostworks 28:d265c64d2bca 298 void BurgerflipActie()
Frostworks 28:d265c64d2bca 299 {
Frostworks 28:d265c64d2bca 300 Burgerflip();
Frostworks 28:d265c64d2bca 301 if (boolrotate == true) {
Frostworks 28:d265c64d2bca 302 GoBack();
Frostworks 28:d265c64d2bca 303 }
Frostworks 28:d265c64d2bca 304 }
Frostworks 27:16327d1337cf 305 void print()
Frostworks 27:16327d1337cf 306 {
Frostworks 27:16327d1337cf 307 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
Frostworks 23:fdde3e4b9e69 308 }
Frostworks 31:21a112643dc9 309 void GetDirections()
Frostworks 31:21a112643dc9 310 {
Frostworks 31:21a112643dc9 311 if (lowpassFilterRight < threshold_Right)
Frostworks 31:21a112643dc9 312 rechts = 0;
Frostworks 31:21a112643dc9 313 if (lowpassFilterRight > threshold_Right)
Frostworks 31:21a112643dc9 314 rechts = 1;
Frostworks 31:21a112643dc9 315 if (lowpassFilterLeft < threshold_Left)
Frostworks 31:21a112643dc9 316 links = 0;
Frostworks 31:21a112643dc9 317 if (lowpassFilterLeft > threshold_Left)
Frostworks 31:21a112643dc9 318 links = 1;
Frostworks 31:21a112643dc9 319
Frostworks 31:21a112643dc9 320 pc.baud(115200);
Frostworks 31:21a112643dc9 321 if ((rechts == 1) && (links == 1) && (turn == 0)) {
Frostworks 31:21a112643dc9 322 draailinks = 0;
Frostworks 31:21a112643dc9 323 draairechts = 0;
Frostworks 31:21a112643dc9 324 turn = 1;
Frostworks 31:21a112643dc9 325 pc.printf("begin de actie \n \r ");
Frostworks 31:21a112643dc9 326 wait(waiter);
Frostworks 31:21a112643dc9 327
Frostworks 31:21a112643dc9 328 } else if ((rechts == 1) && (links == 1) && (turn == 1)) {
Frostworks 31:21a112643dc9 329 draailinks = 0;
Frostworks 31:21a112643dc9 330 draairechts = 0;
Frostworks 31:21a112643dc9 331 turn = 0;
Frostworks 31:21a112643dc9 332 pc.printf("breek de actie af \n \r ");
Frostworks 31:21a112643dc9 333 GoBack();
Frostworks 31:21a112643dc9 334 wait(waiter);
Frostworks 31:21a112643dc9 335 } else if ((rechts == 0) && (links == 0)&& (turn == 0)) {
Frostworks 31:21a112643dc9 336
Frostworks 31:21a112643dc9 337 } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
Frostworks 31:21a112643dc9 338 /* if the right button is pressed and the motor isn't rotating to the left,
Frostworks 31:21a112643dc9 339 then start rotating to the right etc*/
Frostworks 31:21a112643dc9 340 draairechts = !draairechts;
Frostworks 31:21a112643dc9 341 pc.printf("draai naar rechts \n \r ");
Frostworks 31:21a112643dc9 342 wait(waiter);
Frostworks 31:21a112643dc9 343 } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
Frostworks 31:21a112643dc9 344 draailinks = 0;
Frostworks 31:21a112643dc9 345 draairechts = !draairechts;
Frostworks 31:21a112643dc9 346 pc.printf("draai naar rechts na links \n \r ");
Frostworks 31:21a112643dc9 347 wait(waiter);
Frostworks 31:21a112643dc9 348 } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
Frostworks 31:21a112643dc9 349 draailinks = !draailinks;
Frostworks 31:21a112643dc9 350 pc.printf("draai naar links \n \r ");
Frostworks 31:21a112643dc9 351 wait(waiter);
Frostworks 31:21a112643dc9 352 } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
Frostworks 31:21a112643dc9 353 draairechts = 0;
Frostworks 31:21a112643dc9 354 draailinks = !draailinks;
Frostworks 31:21a112643dc9 355 pc.printf("draai naar links na rechts \n \r ");
Frostworks 31:21a112643dc9 356 wait(waiter);
Frostworks 31:21a112643dc9 357 }
Frostworks 31:21a112643dc9 358 wait(2*waiter);
Frostworks 31:21a112643dc9 359 }
vsluiter 0:32bb76391d89 360 int main()
Frostworks 21:2b55d53e11f6 361 {
Frostworks 24:bdd74b91abbb 362 //Leds
Frostworks 23:fdde3e4b9e69 363 led_g = 1;
Frostworks 23:fdde3e4b9e69 364 led_b = 1;
Frostworks 23:fdde3e4b9e69 365 led_r = 1;
Frostworks 23:fdde3e4b9e69 366
tomlankhorst 14:f83354387756 367 /**Attach the 'sample' function to the timer 'sample_timer'.
tomlankhorst 19:2bf824669684 368 * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
vsluiter 4:8b298dfada81 369 */
Frostworks 22:ad85b8acf8b5 370 //sample_timer.attach(&sample, 0.001953125);
Frostworks 30:492595db0fc3 371 sample_timer.attach(&filterSample, Ts); //512 Hz
Frostworks 29:b6d7bcb26f47 372 checkSetpointTranslation.attach(&CheckErrorTranslation,Ts);
Frostworks 29:b6d7bcb26f47 373 checkSetpointRotation.attach(&CheckErrorRotation,Ts);
Frostworks 31:21a112643dc9 374
Frostworks 27:16327d1337cf 375 //printinfo.attach(&print, Ts);
Frostworks 21:2b55d53e11f6 376 pc.baud(115200);
Frostworks 22:ad85b8acf8b5 377 pc.printf("please push the button to calibrate \n \r");
Frostworks 22:ad85b8acf8b5 378 while (1) {
Frostworks 22:ad85b8acf8b5 379 if (buttonCalibrate == 0) {
Frostworks 22:ad85b8acf8b5 380 calibrate = true;
Frostworks 30:492595db0fc3 381 threshold_Left = lowpassFilterLeft*0.9;
Frostworks 30:492595db0fc3 382 threshold_Right = lowpassFilterRight*0.9;
Frostworks 31:21a112643dc9 383 pc.printf("calibration complete, press to continue \n \r");
Frostworks 21:2b55d53e11f6 384 }
Frostworks 31:21a112643dc9 385 if ((buttonCalibrateComplete == 0) && (calibrate == true)) {
Frostworks 31:21a112643dc9 386 calibrate_complete = true;
Frostworks 31:21a112643dc9 387 }
Frostworks 31:21a112643dc9 388 if (calibrate_complete == true) {
Frostworks 30:492595db0fc3 389
Frostworks 31:21a112643dc9 390 //pc.printf("calibration complete, calL = %f, L=%f CalR = %f, R = %f, boolL=%b boolR=%b \n \r", threshold_Left, lowpassFilterLeft, threshold_Right, lowpassFilterRight, links, rechts);
Frostworks 31:21a112643dc9 391 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 392 GetDirections();
Frostworks 23:fdde3e4b9e69 393 if (draairechts == true) {
Frostworks 31:21a112643dc9 394 M1_Speed = 0.1;
Frostworks 31:21a112643dc9 395 M1_Rotate = 1;
Frostworks 23:fdde3e4b9e69 396 } else if (draailinks == true) {
Frostworks 31:21a112643dc9 397 M1_Speed = 0.1;
Frostworks 31:21a112643dc9 398 M1_Rotate = 0;
Frostworks 23:fdde3e4b9e69 399 } else if (turn == 1) {
Frostworks 28:d265c64d2bca 400 BurgerflipActie();
Frostworks 23:fdde3e4b9e69 401 } else if (turn == 0) {
Frostworks 23:fdde3e4b9e69 402 M2_Speed = 0;
Frostworks 23:fdde3e4b9e69 403 M3_Speed = 0;
Frostworks 23:fdde3e4b9e69 404 }
Frostworks 23:fdde3e4b9e69 405 if ((draailinks == false) && (draairechts == false)) {
Frostworks 23:fdde3e4b9e69 406 M1_Speed = 0;
Frostworks 30:492595db0fc3 407
Frostworks 23:fdde3e4b9e69 408 }
Frostworks 30:492595db0fc3 409
Frostworks 22:ad85b8acf8b5 410 }
Frostworks 22:ad85b8acf8b5 411 }
vsluiter 0:32bb76391d89 412 }