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@30:492595db0fc3, 2016-11-02 (annotated)

Committer:
Frostworks
Date:
Wed Nov 02 13:30:57 2016 +0000
Revision:
30:492595db0fc3
Parent:
29:b6d7bcb26f47
Child:
31:21a112643dc9
EMG lijkt nu te werken :^)

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