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@27:16327d1337cf, 2016-10-28 (annotated)

Committer:
Frostworks
Date:
Fri Oct 28 10:37:21 2016 +0000
Revision:
27:16327d1337cf
Parent:
26:c640851fa1e7
Child:
28:d265c64d2bca
Controllers werken, nu nog tunen en pizza en goback BOIII en de emg ook ok doei weekend :) burger

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