Meike Froklage / Mbed 2 deprecated The_Claw_with_EMG_Control_Final

Robot

Dependencies:   HIDScope MODSERIAL Motordriver QEI Servo mbed

Fork of The_Claw_with_EMG_Control_PID by Meike Froklage

main.cpp@15:caf29b6f5261, 2016-11-03 (annotated)

Committer:
meikefrok
Date:
Thu Nov 03 12:12:03 2016 +0000
Revision:
15:caf29b6f5261
Parent:
14:65780d480874
Child:
16:3c9a3ff09765
met hid

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:048fbd80203e 1 #include "mbed.h"
meikefrok 5:3d88f7506cd9 2 #include "MODSERIAL.h"
meikefrok 5:3d88f7506cd9 3 #define SERIAL_BAUD 115200
meikefrok 3:3a671d01bcb8 4 #include "motordriver.h"
meikefrok 5:3d88f7506cd9 5 #include "QEI.h"
meikefrok 5:3d88f7506cd9 6 #include "Servo.h"
meikefrok 10:31e4c3d71ee6 7 #include "HIDScope.h"
megrootens 0:048fbd80203e 8
meikefrok 5:3d88f7506cd9 9 //======== Serial Communication ================================================
megrootens 0:048fbd80203e 10 MODSERIAL pc(USBTX,USBRX);
megrootens 0:048fbd80203e 11
meikefrok 5:3d88f7506cd9 12 //======== Motor and QEI =======================================================
meikefrok 5:3d88f7506cd9 13 int Brakeable;
meikefrok 5:3d88f7506cd9 14 int sign;
meikefrok 5:3d88f7506cd9 15
meikefrok 5:3d88f7506cd9 16 // motor
meikefrok 5:3d88f7506cd9 17 Motor Cart(D5, D4, D4, Brakeable); // right motor
meikefrok 5:3d88f7506cd9 18 Motor Arm(D6,D7, D7, Brakeable); // left motor
meikefrok 5:3d88f7506cd9 19
meikefrok 5:3d88f7506cd9 20 // qei
meikefrok 6:23b1ed826b59 21 QEI Encoder_Cart(D10, D11, NC, 6400);
meikefrok 6:23b1ed826b59 22 QEI Encoder_Arm(D12, D13, NC, 6400);
megrootens 0:048fbd80203e 23
meikefrok 5:3d88f7506cd9 24 // servo
meikefrok 5:3d88f7506cd9 25 Servo servo(D9);
megrootens 0:048fbd80203e 26
meikefrok 15:caf29b6f5261 27
meikefrok 10:31e4c3d71ee6 28 HIDScope scope(2); //scope has two ports for the two EMG signals
meikefrok 10:31e4c3d71ee6 29
meikefrok 5:3d88f7506cd9 30 //======== Miscellaneous =======================================================
meikefrok 5:3d88f7506cd9 31 // button
meikefrok 5:3d88f7506cd9 32 InterruptIn btn(SW2);
meikefrok 5:3d88f7506cd9 33 InterruptIn btn2(SW3);
meikefrok 5:3d88f7506cd9 34
meikefrok 5:3d88f7506cd9 35 InterruptIn btn_cart(D1);
meikefrok 5:3d88f7506cd9 36 InterruptIn btn_arm(D2);
meikefrok 5:3d88f7506cd9 37 InterruptIn btn_claw(D3);
meikefrok 5:3d88f7506cd9 38
meikefrok 5:3d88f7506cd9 39 // led
meikefrok 2:ad4b181a6422 40 DigitalOut led_r(LED_RED);
meikefrok 2:ad4b181a6422 41 DigitalOut led_g(LED_GREEN);
meikefrok 2:ad4b181a6422 42 DigitalOut led_b(LED_BLUE);
megrootens 0:048fbd80203e 43
meikefrok 5:3d88f7506cd9 44 // potmeter
meikefrok 5:3d88f7506cd9 45 AnalogIn pot_cart(A2);
meikefrok 5:3d88f7506cd9 46 AnalogIn pot_arm(A3);
megrootens 0:048fbd80203e 47
meikefrok 7:9715323b20ac 48 AnalogIn emgl(A1); // labels are attached to the olimex shields the left tricep should obviously be connected to the should with an L label on it
meikefrok 7:9715323b20ac 49 AnalogIn emgr(A0);
meikefrok 7:9715323b20ac 50
meikefrok 5:3d88f7506cd9 51 // ticker
meikefrok 8:ac4e5afbdbcd 52 Ticker tick_part; // ticker to switch parts
meikefrok 15:caf29b6f5261 53 Ticker tick_part_arm;
meikefrok 15:caf29b6f5261 54 Ticker tick_part_claw;
meikefrok 10:31e4c3d71ee6 55 Ticker sampleTicker;
meikefrok 15:caf29b6f5261 56 Ticker measureTicker;
meikefrok 15:caf29b6f5261 57 Ticker sampleTicker_claw;
meikefrok 5:3d88f7506cd9 58
meikefrok 5:3d88f7506cd9 59 //======== Variables ===========================================================
megrootens 0:048fbd80203e 60 // counters
meikefrok 5:3d88f7506cd9 61 int num_turned_on_0 = 0; // count number of times red LED turned on
meikefrok 5:3d88f7506cd9 62 int num_turned_on_1 = 0; // count number of times green LED turned on
meikefrok 5:3d88f7506cd9 63 int num_turned_on_2 = 0; // count number of times blue LED turned on
meikefrok 5:3d88f7506cd9 64
meikefrok 5:3d88f7506cd9 65 int num_claw_turned_on_0 = 0; // count number of times red LED turned on
meikefrok 5:3d88f7506cd9 66 int num_claw_turned_on_1 = 0; // count number of times green LED turned on
meikefrok 5:3d88f7506cd9 67 int num_claw_turned_on_2 = 0; // count number of times blue LED turned on
megrootens 0:048fbd80203e 68
meikefrok 5:3d88f7506cd9 69 // speed
meikefrok 6:23b1ed826b59 70 double cart_speed = 0.3;
meikefrok 6:23b1ed826b59 71 double arm_speed = 0.1;
megrootens 0:048fbd80203e 72
meikefrok 5:3d88f7506cd9 73 // position
meikefrok 12:b31df384b170 74 float factor_cart = 0.05802;
meikefrok 5:3d88f7506cd9 75 float factor_arm = 0.1539;
meikefrok 5:3d88f7506cd9 76 int position_cart;
meikefrok 5:3d88f7506cd9 77 int position_arm;
meikefrok 5:3d88f7506cd9 78 float ain_cart; //Variable to store the analog input of the cart
meikefrok 5:3d88f7506cd9 79 float ain_arm; //Variable to store the analog input of the arm
megrootens 0:048fbd80203e 80
meikefrok 6:23b1ed826b59 81 int position_claw;
meikefrok 6:23b1ed826b59 82
meikefrok 6:23b1ed826b59 83
meikefrok 5:3d88f7506cd9 84 // miscellaneous
meikefrok 12:b31df384b170 85 const float kTimeToggle = 0.05f; // period with which to toggle the parts
meikefrok 5:3d88f7506cd9 86 const int LedOn = 0; // LED on if 0
meikefrok 7:9715323b20ac 87 volatile int part_id = 1; // ID of what part should move, begins with the cart
meikefrok 5:3d88f7506cd9 88 volatile int servo_id = 1; // ID to the side the servo should move, begins in center position
meikefrok 3:3a671d01bcb8 89
meikefrok 15:caf29b6f5261 90 int waiting_claw = 1000;
meikefrok 15:caf29b6f5261 91
meikefrok 7:9715323b20ac 92 //======== Variables Filter ====================================================
meikefrok 7:9715323b20ac 93 /*coefficients of each filter
meikefrok 7:9715323b20ac 94 lno = left tricep notch filter
meikefrok 7:9715323b20ac 95 lhf = left tricep high pass filter
meikefrok 7:9715323b20ac 96 llf = left tricep lowpass filter
meikefrok 7:9715323b20ac 97 same goes for rno etc.
meikefrok 7:9715323b20ac 98 */
meikefrok 7:9715323b20ac 99 double lno_b0 = 0.9911;
meikefrok 7:9715323b20ac 100 double lno_b1 = -1.6036;
meikefrok 7:9715323b20ac 101 double lno_b2 = 0.9911;
meikefrok 7:9715323b20ac 102 double lno_a1 = -1.603;
meikefrok 7:9715323b20ac 103 double lno_a2 = 0.9822;
meikefrok 7:9715323b20ac 104
meikefrok 7:9715323b20ac 105 double rno_b0 = 0.9911;
meikefrok 7:9715323b20ac 106 double rno_b1 = -1.6036;
meikefrok 7:9715323b20ac 107 double rno_b2 = 0.9911;
meikefrok 7:9715323b20ac 108 double rno_a1 = -1.603;
meikefrok 7:9715323b20ac 109 double rno_a2 = 0.9822;
meikefrok 7:9715323b20ac 110
meikefrok 7:9715323b20ac 111 double lno2_b0 = 0.9824;
meikefrok 7:9715323b20ac 112 double lno2_b1 = -0.6071;
meikefrok 7:9715323b20ac 113 double lno2_b2 = 0.9824;
meikefrok 7:9715323b20ac 114 double lno2_a1 = -0.6071;
meikefrok 7:9715323b20ac 115 double lno2_a2 = 0.9647;
meikefrok 7:9715323b20ac 116
meikefrok 7:9715323b20ac 117 double rno2_b0 = 0.9824;
meikefrok 7:9715323b20ac 118 double rno2_b1 = -0.6071;
meikefrok 7:9715323b20ac 119 double rno2_b2 = 0.9824;
meikefrok 7:9715323b20ac 120 double rno2_a1 = -0.6071;
meikefrok 7:9715323b20ac 121 double rno2_a2 = 0.9647;
meikefrok 7:9715323b20ac 122
meikefrok 7:9715323b20ac 123 double lhf_b0 = 0.9355;
meikefrok 7:9715323b20ac 124 double lhf_b1 = -1.8711;
meikefrok 7:9715323b20ac 125 double lhf_b2 = 0.9355;
meikefrok 7:9715323b20ac 126 double lhf_a1 = -1.8669;
meikefrok 7:9715323b20ac 127 double lhf_a2 = 0.8752;
meikefrok 7:9715323b20ac 128
meikefrok 7:9715323b20ac 129 double rhf_b0 = 0.9355;
meikefrok 7:9715323b20ac 130 double rhf_b1 = -1.8711;
meikefrok 7:9715323b20ac 131 double rhf_b2 = 0.9355;
meikefrok 7:9715323b20ac 132 double rhf_a1 = -1.8669;
meikefrok 7:9715323b20ac 133 double rhf_a2 = 0.8752;
meikefrok 7:9715323b20ac 134
meikefrok 7:9715323b20ac 135
meikefrok 7:9715323b20ac 136 double llf_b0 = 8.7656e-5;
meikefrok 7:9715323b20ac 137 double llf_b1 = 1.17531e-4;
meikefrok 7:9715323b20ac 138 double llf_b2 = 8.7656e-5;
meikefrok 7:9715323b20ac 139 double llf_a1 = -1.9733;
meikefrok 7:9715323b20ac 140 double llf_a2 = 0.9737;
meikefrok 7:9715323b20ac 141
meikefrok 7:9715323b20ac 142 double rlf_b0 = 8.7656e-5;
meikefrok 7:9715323b20ac 143 double rlf_b1 = 1.17531e-4;
meikefrok 7:9715323b20ac 144 double rlf_b2 = 8.7656e-5;
meikefrok 7:9715323b20ac 145 double rlf_a1 = -1.9733;
meikefrok 7:9715323b20ac 146 double rlf_a2 = 0.9737;
meikefrok 7:9715323b20ac 147
meikefrok 7:9715323b20ac 148
meikefrok 7:9715323b20ac 149 //starting values of the biquads of the corresponding filters
meikefrok 7:9715323b20ac 150 double lno_v1 = 0, lno_v2 = 0;
meikefrok 7:9715323b20ac 151 double lno2_v1 = 0, lno2_v2 = 0;
meikefrok 7:9715323b20ac 152 double lhf_v1 = 0, lhf_v2 = 0;
meikefrok 7:9715323b20ac 153 double llf_v1 = 0, llf_v2 = 0;
meikefrok 7:9715323b20ac 154
meikefrok 7:9715323b20ac 155 double rno_v1 = 0, rno_v2 = 0;
meikefrok 7:9715323b20ac 156 double rno2_v1 = 0, rno2_v2 = 0;
meikefrok 7:9715323b20ac 157 double rhf_v1 = 0, rhf_v2 = 0;
meikefrok 7:9715323b20ac 158 double rlf_v1 = 0, rlf_v2 = 0;
meikefrok 7:9715323b20ac 159
meikefrok 7:9715323b20ac 160 /* declaration of the outputs of each biquad.
meikefrok 7:9715323b20ac 161 the output of the previous biquad is the input for the next biquad.
meikefrok 7:9715323b20ac 162 so lno_y goes into lhf_y etc.
meikefrok 7:9715323b20ac 163 */
meikefrok 7:9715323b20ac 164 double lno_y;
meikefrok 7:9715323b20ac 165 double lno2_y;
meikefrok 7:9715323b20ac 166 double lhf_y;
meikefrok 7:9715323b20ac 167 double llf_y;
meikefrok 7:9715323b20ac 168 double lrect_y;
meikefrok 7:9715323b20ac 169 double rno_y;
meikefrok 7:9715323b20ac 170 double rno2_y;
meikefrok 7:9715323b20ac 171 double rhf_y;
meikefrok 7:9715323b20ac 172 double rlf_y;
meikefrok 7:9715323b20ac 173 double rrect_y;
meikefrok 7:9715323b20ac 174
meikefrok 7:9715323b20ac 175 // set the threshold value for the filtered signal
meikefrok 7:9715323b20ac 176 //if the signal exceeds this value the motors will start to rotate
meikefrok 7:9715323b20ac 177 const double threshold_value= 0.08;
meikefrok 15:caf29b6f5261 178 const double threshold_value_claw= 0.08;
meikefrok 15:caf29b6f5261 179
meikefrok 7:9715323b20ac 180
meikefrok 7:9715323b20ac 181
meikefrok 7:9715323b20ac 182 /* declaration of each biquad
meikefrok 7:9715323b20ac 183 The coefficients will be filled in later on in void scopeSend
meikefrok 7:9715323b20ac 184 As said before the input of each biquad is the output of the previous one
meikefrok 7:9715323b20ac 185 The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal.
meikefrok 7:9715323b20ac 186 This is done for both left and right so this makes two chains of 3 biquads */
meikefrok 7:9715323b20ac 187
meikefrok 7:9715323b20ac 188 double biquad_lno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 189 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 190 {
meikefrok 7:9715323b20ac 191 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 192 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 193 v2 = v1;
meikefrok 7:9715323b20ac 194 v1 = v;
meikefrok 7:9715323b20ac 195 return y;
meikefrok 7:9715323b20ac 196 }
meikefrok 7:9715323b20ac 197
meikefrok 7:9715323b20ac 198 double biquad_lno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 199 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 200 {
meikefrok 7:9715323b20ac 201 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 202 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 203 v2 = v1;
meikefrok 7:9715323b20ac 204 v1 = v;
meikefrok 7:9715323b20ac 205 return y;
meikefrok 7:9715323b20ac 206 }
meikefrok 7:9715323b20ac 207
meikefrok 7:9715323b20ac 208 double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 209 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 210 {
meikefrok 7:9715323b20ac 211 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 212 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 213 v2 = v1;
meikefrok 7:9715323b20ac 214 v1 = v;
meikefrok 7:9715323b20ac 215 return y;
meikefrok 7:9715323b20ac 216 }
meikefrok 7:9715323b20ac 217
meikefrok 7:9715323b20ac 218 double biquad_llf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 219 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 220 {
meikefrok 7:9715323b20ac 221 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 222 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 223 v2 = v1;
meikefrok 7:9715323b20ac 224 v1 = v;
meikefrok 7:9715323b20ac 225 return y;
meikefrok 7:9715323b20ac 226 }
meikefrok 7:9715323b20ac 227 double biquad_rno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 228 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 229 {
meikefrok 7:9715323b20ac 230 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 231 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 232 v2 = v1;
meikefrok 7:9715323b20ac 233 v1 = v;
meikefrok 7:9715323b20ac 234 return y;
meikefrok 7:9715323b20ac 235 }
meikefrok 7:9715323b20ac 236
meikefrok 7:9715323b20ac 237 double biquad_rno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 238 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 239 {
meikefrok 7:9715323b20ac 240 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 241 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 242 v2 = v1;
meikefrok 7:9715323b20ac 243 v1 = v;
meikefrok 7:9715323b20ac 244 return y;
meikefrok 7:9715323b20ac 245 }
meikefrok 7:9715323b20ac 246
meikefrok 7:9715323b20ac 247 double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 248 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 249 {
meikefrok 7:9715323b20ac 250 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 251 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 252 v2 = v1;
meikefrok 7:9715323b20ac 253 v1 = v;
meikefrok 7:9715323b20ac 254 return y;
meikefrok 7:9715323b20ac 255 }
meikefrok 7:9715323b20ac 256
meikefrok 7:9715323b20ac 257 double biquad_rlf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 258 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 259 {
meikefrok 7:9715323b20ac 260 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 261 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 262 v2 = v1;
meikefrok 7:9715323b20ac 263 v1 = v;
meikefrok 7:9715323b20ac 264 return y;
meikefrok 7:9715323b20ac 265 }
meikefrok 7:9715323b20ac 266
meikefrok 7:9715323b20ac 267 /* function that calculates the filtered EMG signal from the raw EMG signal.
meikefrok 7:9715323b20ac 268 So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
meikefrok 7:9715323b20ac 269 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
meikefrok 7:9715323b20ac 270 The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
meikefrok 7:9715323b20ac 271
meikefrok 7:9715323b20ac 272
meikefrok 7:9715323b20ac 273 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
meikefrok 7:9715323b20ac 274 //======== Functions and main ==============================================================
meikefrok 7:9715323b20ac 275 /* function that calculates the filtered EMG signal from the raw EMG signal.
meikefrok 7:9715323b20ac 276 So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
meikefrok 7:9715323b20ac 277 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
meikefrok 7:9715323b20ac 278 The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
meikefrok 7:9715323b20ac 279 void scopeSend(void){
meikefrok 7:9715323b20ac 280 lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
meikefrok 7:9715323b20ac 281 lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
meikefrok 7:9715323b20ac 282 lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
meikefrok 7:9715323b20ac 283 lrect_y = fabs(lhf_y);
meikefrok 7:9715323b20ac 284 llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
meikefrok 7:9715323b20ac 285 rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
meikefrok 7:9715323b20ac 286 rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);
meikefrok 7:9715323b20ac 287 rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
meikefrok 7:9715323b20ac 288 rrect_y = fabs(rhf_y);
meikefrok 7:9715323b20ac 289 rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;
meikefrok 15:caf29b6f5261 290 //scope.set(1, llf_y);
meikefrok 15:caf29b6f5261 291 //scope.set(0, rlf_y);
meikefrok 15:caf29b6f5261 292 //scope.send();
meikefrok 7:9715323b20ac 293 }
meikefrok 7:9715323b20ac 294
meikefrok 3:3a671d01bcb8 295
meikefrok 5:3d88f7506cd9 296 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
meikefrok 5:3d88f7506cd9 297 //======== Functions and main ==============================================================
meikefrok 10:31e4c3d71ee6 298 void Measure(){
meikefrok 10:31e4c3d71ee6 299 // encoder
meikefrok 10:31e4c3d71ee6 300 position_cart = (Encoder_Cart.getPulses()*factor_cart) ;
meikefrok 10:31e4c3d71ee6 301 ain_cart = pot_cart.read();
meikefrok 12:b31df384b170 302
meikefrok 13:dd7b41766f5f 303 // encoder
meikefrok 13:dd7b41766f5f 304 position_arm = (Encoder_Arm.getPulses()*factor_arm) ;
meikefrok 13:dd7b41766f5f 305 ain_arm = pot_arm.read();
meikefrok 13:dd7b41766f5f 306
meikefrok 13:dd7b41766f5f 307 //if (ain_arm == 0){
meikefrok 13:dd7b41766f5f 308 // Encoder_Arm.reset();
meikefrok 13:dd7b41766f5f 309 //}else {}
meikefrok 13:dd7b41766f5f 310
meikefrok 12:b31df384b170 311
meikefrok 10:31e4c3d71ee6 312
meikefrok 10:31e4c3d71ee6 313 }
meikefrok 10:31e4c3d71ee6 314
meikefrok 3:3a671d01bcb8 315
meikefrok 5:3d88f7506cd9 316 // Switch between Cart, Arm and Claw
meikefrok 7:9715323b20ac 317
meikefrok 8:ac4e5afbdbcd 318 void SwitchCart(){
meikefrok 2:ad4b181a6422 319 switch (part_id) {
meikefrok 5:3d88f7506cd9 320 //Cart
meikefrok 3:3a671d01bcb8 321 case 2: {
meikefrok 15:caf29b6f5261 322 led_r = LedOn;
meikefrok 5:3d88f7506cd9 323 if(led_r == LedOn){
megrootens 0:048fbd80203e 324 num_turned_on_0++;
meikefrok 5:3d88f7506cd9 325
meikefrok 9:90227be52903 326 if (rlf_y > threshold_value) {
meikefrok 12:b31df384b170 327 if(position_cart <= -170){ //If the cart is at the right side, it stops
meikefrok 6:23b1ed826b59 328 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 329
meikefrok 12:b31df384b170 330 }else if(position_cart <=100){
meikefrok 12:b31df384b170 331 Cart.speed(0.2)== 0.2;
meikefrok 12:b31df384b170 332
meikefrok 12:b31df384b170 333 }else if(position_cart >= 150 && position_arm <=-45){ //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
meikefrok 6:23b1ed826b59 334 Cart.stop(1) == 1;
meikefrok 6:23b1ed826b59 335
meikefrok 12:b31df384b170 336 }else if(position_cart >= 150 && position_arm <=-35 && position_claw == -18){
meikefrok 6:23b1ed826b59 337 Cart.stop(1) == 1;
meikefrok 5:3d88f7506cd9 338
meikefrok 5:3d88f7506cd9 339 }else{
meikefrok 5:3d88f7506cd9 340 Cart.speed(cart_speed)==cart_speed;
meikefrok 5:3d88f7506cd9 341 }
meikefrok 12:b31df384b170 342
meikefrok 12:b31df384b170 343 if (llf_y > threshold_value){
meikefrok 12:b31df384b170 344 Cart.stop(1)==1;
meikefrok 12:b31df384b170 345 }
meikefrok 5:3d88f7506cd9 346
meikefrok 9:90227be52903 347 }else if (llf_y > threshold_value) {
meikefrok 12:b31df384b170 348 if(position_cart >= 150){ //If the cart is at the left side, it stops
meikefrok 6:23b1ed826b59 349 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 350
meikefrok 12:b31df384b170 351 }else if(position_cart >=100){
meikefrok 12:b31df384b170 352 Cart.speed(-0.2)== -0.2;
meikefrok 12:b31df384b170 353
meikefrok 12:b31df384b170 354 }else if(position_cart <= -170 && position_arm >=45){ //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
meikefrok 12:b31df384b170 355 Cart.stop(1) == 1;
meikefrok 12:b31df384b170 356
meikefrok 12:b31df384b170 357 }else if(position_cart <= -170 && position_arm >=35 && position_claw == 27){
meikefrok 6:23b1ed826b59 358 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 359
meikefrok 5:3d88f7506cd9 360 }else{
meikefrok 5:3d88f7506cd9 361 Cart.speed(-cart_speed)==-cart_speed;
meikefrok 5:3d88f7506cd9 362 }
meikefrok 12:b31df384b170 363
meikefrok 12:b31df384b170 364 if (rlf_y > threshold_value){
meikefrok 12:b31df384b170 365 Cart.stop(1)==1;
meikefrok 12:b31df384b170 366 }
meikefrok 5:3d88f7506cd9 367
meikefrok 5:3d88f7506cd9 368 }else {
meikefrok 12:b31df384b170 369 Cart.stop(1)==1;
meikefrok 5:3d88f7506cd9 370 }
megrootens 0:048fbd80203e 371 }
meikefrok 12:b31df384b170 372
meikefrok 12:b31df384b170 373 if(!btn && !btn2) {
meikefrok 12:b31df384b170 374 Arm.speed(0) == 0;
meikefrok 12:b31df384b170 375 if(position_cart<0){
meikefrok 12:b31df384b170 376 Cart.speed(-0.1)== -0.1;
meikefrok 12:b31df384b170 377 }else if(position_cart>0){
meikefrok 12:b31df384b170 378 Cart.speed(0.1)==0.1;
meikefrok 12:b31df384b170 379 }else{
meikefrok 12:b31df384b170 380 Cart.stop(0)==0;
meikefrok 12:b31df384b170 381 }
meikefrok 12:b31df384b170 382 }
meikefrok 12:b31df384b170 383
meikefrok 12:b31df384b170 384
meikefrok 12:b31df384b170 385
meikefrok 5:3d88f7506cd9 386 // controle LED
meikefrok 3:3a671d01bcb8 387 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 388 led_b = not LedOn;
meikefrok 12:b31df384b170 389
meikefrok 5:3d88f7506cd9 390 pc.baud(115200);
meikefrok 5:3d88f7506cd9 391 pc.printf("Distance in mm: %i\n", position_cart);
meikefrok 5:3d88f7506cd9 392
megrootens 0:048fbd80203e 393 break;
megrootens 0:048fbd80203e 394 }
meikefrok 8:ac4e5afbdbcd 395 }
meikefrok 8:ac4e5afbdbcd 396 }
meikefrok 8:ac4e5afbdbcd 397
meikefrok 8:ac4e5afbdbcd 398 void SwitchArm(){
meikefrok 8:ac4e5afbdbcd 399 switch (part_id) {
meikefrok 8:ac4e5afbdbcd 400 //Cart
meikefrok 8:ac4e5afbdbcd 401 case 3: {
meikefrok 15:caf29b6f5261 402 led_g = LedOn;
meikefrok 8:ac4e5afbdbcd 403 if(led_g == LedOn){
meikefrok 8:ac4e5afbdbcd 404 num_turned_on_1++;
meikefrok 13:dd7b41766f5f 405 Arm.stop(1)==1;
meikefrok 13:dd7b41766f5f 406
meikefrok 8:ac4e5afbdbcd 407
meikefrok 15:caf29b6f5261 408 if (rlf_y > threshold_value) {
meikefrok 13:dd7b41766f5f 409 if(position_cart > -105 && position_arm >= 95){ //If the cart is not at the end, the arm can't move any further than 45 degrees
meikefrok 8:ac4e5afbdbcd 410 Arm.stop(1)==1;
meikefrok 8:ac4e5afbdbcd 411
meikefrok 8:ac4e5afbdbcd 412 }else if(position_cart > -105 && position_arm >= 25 && position_claw == 27){
meikefrok 8:ac4e5afbdbcd 413 Arm.stop(1)==1;
meikefrok 8:ac4e5afbdbcd 414
meikefrok 14:65780d480874 415 }else if(position_cart<= -105 && position_arm>=160){ //If the cart is at the right end, the arm can't move any further than 70 degrees
meikefrok 8:ac4e5afbdbcd 416 Arm.stop(1)==1;
meikefrok 8:ac4e5afbdbcd 417
meikefrok 8:ac4e5afbdbcd 418 }else{
meikefrok 8:ac4e5afbdbcd 419 Arm.speed(arm_speed)==arm_speed;
meikefrok 8:ac4e5afbdbcd 420 }
meikefrok 13:dd7b41766f5f 421
meikefrok 13:dd7b41766f5f 422 if (llf_y > threshold_value){
meikefrok 13:dd7b41766f5f 423 Cart.stop(1)==1;
meikefrok 13:dd7b41766f5f 424 }
meikefrok 8:ac4e5afbdbcd 425
meikefrok 15:caf29b6f5261 426 }else if (llf_y > threshold_value) {
meikefrok 13:dd7b41766f5f 427 if(position_cart < 105 && position_arm <= -95){ //If the cart is not at the end, the arm can't move any further than 45 degrees
meikefrok 8:ac4e5afbdbcd 428 Arm.stop(1)==1;
meikefrok 8:ac4e5afbdbcd 429
meikefrok 8:ac4e5afbdbcd 430 }else if(position_cart < 105 && position_arm <= -25 && position_claw == -18){
meikefrok 8:ac4e5afbdbcd 431 Arm.stop(1)==1;
meikefrok 8:ac4e5afbdbcd 432
meikefrok 14:65780d480874 433 }else if(position_cart>=105 && position_arm<=-160){ //If the cart is at the left end, the arm can't move any further than 70 degrees
meikefrok 8:ac4e5afbdbcd 434 Arm.stop(1)==1;
meikefrok 8:ac4e5afbdbcd 435
meikefrok 8:ac4e5afbdbcd 436 }else{
meikefrok 8:ac4e5afbdbcd 437 Arm.speed(-arm_speed)==-arm_speed;
meikefrok 8:ac4e5afbdbcd 438 }
meikefrok 13:dd7b41766f5f 439
meikefrok 13:dd7b41766f5f 440 if (rlf_y > threshold_value){
meikefrok 13:dd7b41766f5f 441 Cart.stop(1)==1;
meikefrok 13:dd7b41766f5f 442 }
meikefrok 8:ac4e5afbdbcd 443
meikefrok 8:ac4e5afbdbcd 444 }else {
meikefrok 13:dd7b41766f5f 445 Arm.stop(1)==1;
meikefrok 13:dd7b41766f5f 446 }
meikefrok 13:dd7b41766f5f 447
meikefrok 13:dd7b41766f5f 448 if(!btn&&!btn2){
meikefrok 13:dd7b41766f5f 449 Cart.speed(0) == 0;
meikefrok 8:ac4e5afbdbcd 450 if(position_arm>0){
meikefrok 8:ac4e5afbdbcd 451 Arm.speed(-0.1)== -0.1;
meikefrok 8:ac4e5afbdbcd 452 }else if(position_arm<0){
meikefrok 8:ac4e5afbdbcd 453 Arm.speed(0.1)==0.1;
meikefrok 8:ac4e5afbdbcd 454 }else{
meikefrok 8:ac4e5afbdbcd 455 Arm.stop(0)==0;
meikefrok 8:ac4e5afbdbcd 456 }
meikefrok 13:dd7b41766f5f 457 }
meikefrok 8:ac4e5afbdbcd 458 }
meikefrok 8:ac4e5afbdbcd 459 // controle LED
meikefrok 8:ac4e5afbdbcd 460 led_r = not LedOn;
meikefrok 8:ac4e5afbdbcd 461 led_b = not LedOn;
meikefrok 8:ac4e5afbdbcd 462
meikefrok 13:dd7b41766f5f 463 pc.baud(115200);
meikefrok 13:dd7b41766f5f 464 pc.printf("Degrees: %i\n", position_arm);
meikefrok 13:dd7b41766f5f 465
meikefrok 8:ac4e5afbdbcd 466 break;
megrootens 0:048fbd80203e 467 }
meikefrok 8:ac4e5afbdbcd 468 }
meikefrok 8:ac4e5afbdbcd 469 }
meikefrok 8:ac4e5afbdbcd 470
meikefrok 15:caf29b6f5261 471 void SwitchClaw(){
meikefrok 15:caf29b6f5261 472 switch (part_id) {
meikefrok 15:caf29b6f5261 473 case 4: {
meikefrok 15:caf29b6f5261 474 led_b = LedOn;
meikefrok 15:caf29b6f5261 475
meikefrok 15:caf29b6f5261 476 if(rlf_y > threshold_value_claw){
meikefrok 15:caf29b6f5261 477 servo_id ++;
meikefrok 15:caf29b6f5261 478
meikefrok 15:caf29b6f5261 479 switch (servo_id) {
meikefrok 15:caf29b6f5261 480 case 0: {
meikefrok 15:caf29b6f5261 481 led_r = LedOn;
meikefrok 15:caf29b6f5261 482 if (led_r == LedOn) {
meikefrok 15:caf29b6f5261 483 num_claw_turned_on_0++;
meikefrok 15:caf29b6f5261 484 }
meikefrok 15:caf29b6f5261 485
meikefrok 15:caf29b6f5261 486 led_b = not LedOn;
meikefrok 15:caf29b6f5261 487 led_g = not LedOn;
meikefrok 15:caf29b6f5261 488 servo.position(-18);
meikefrok 15:caf29b6f5261 489 //pc.printf("Servo position is: left \r\n");
meikefrok 15:caf29b6f5261 490
meikefrok 15:caf29b6f5261 491 break;
meikefrok 15:caf29b6f5261 492 }
meikefrok 15:caf29b6f5261 493 case 1: {
meikefrok 15:caf29b6f5261 494 led_b = LedOn;
meikefrok 15:caf29b6f5261 495 if (led_b == LedOn) {
meikefrok 15:caf29b6f5261 496 num_claw_turned_on_1++;
meikefrok 15:caf29b6f5261 497 }
meikefrok 15:caf29b6f5261 498
meikefrok 15:caf29b6f5261 499 led_r = not LedOn;
meikefrok 15:caf29b6f5261 500 led_g = not LedOn;
meikefrok 15:caf29b6f5261 501
meikefrok 15:caf29b6f5261 502 servo.position(3);
meikefrok 15:caf29b6f5261 503 //pc.printf("Servo position is: center \r\n");
meikefrok 15:caf29b6f5261 504
meikefrok 15:caf29b6f5261 505 wait_ms(waiting_claw);
meikefrok 15:caf29b6f5261 506
meikefrok 15:caf29b6f5261 507 break;
meikefrok 15:caf29b6f5261 508 }
meikefrok 15:caf29b6f5261 509 case 2: {
meikefrok 15:caf29b6f5261 510 led_g = LedOn;
meikefrok 15:caf29b6f5261 511 if (led_g == LedOn) {
meikefrok 15:caf29b6f5261 512 num_claw_turned_on_2++;
meikefrok 15:caf29b6f5261 513 }
meikefrok 15:caf29b6f5261 514
meikefrok 15:caf29b6f5261 515 led_r = not LedOn;
meikefrok 15:caf29b6f5261 516 led_b = not LedOn;
meikefrok 15:caf29b6f5261 517
meikefrok 15:caf29b6f5261 518 servo.position(27);
meikefrok 15:caf29b6f5261 519 //pc.printf("Servo position is: %i\r\n", num_claw_turned_on);
meikefrok 15:caf29b6f5261 520
meikefrok 15:caf29b6f5261 521 break;
meikefrok 15:caf29b6f5261 522 }
meikefrok 15:caf29b6f5261 523 }
meikefrok 15:caf29b6f5261 524
meikefrok 15:caf29b6f5261 525
meikefrok 15:caf29b6f5261 526 }else if(llf_y > threshold_value_claw){
meikefrok 15:caf29b6f5261 527 servo_id --;
meikefrok 15:caf29b6f5261 528
meikefrok 15:caf29b6f5261 529 switch (servo_id) {
meikefrok 15:caf29b6f5261 530 case 0: {
meikefrok 15:caf29b6f5261 531 led_r = LedOn;
meikefrok 15:caf29b6f5261 532 led_b = not LedOn;
meikefrok 15:caf29b6f5261 533 led_g = not LedOn;
meikefrok 15:caf29b6f5261 534
meikefrok 15:caf29b6f5261 535 servo.position(-18);
meikefrok 15:caf29b6f5261 536 //pc.printf("Servo position is: left \r\n");
meikefrok 15:caf29b6f5261 537
meikefrok 15:caf29b6f5261 538
meikefrok 15:caf29b6f5261 539 break;
meikefrok 15:caf29b6f5261 540 }
meikefrok 15:caf29b6f5261 541 case 1: {
meikefrok 15:caf29b6f5261 542 led_b = LedOn;
meikefrok 15:caf29b6f5261 543 led_r = not LedOn;
meikefrok 15:caf29b6f5261 544 led_g = not LedOn;
meikefrok 15:caf29b6f5261 545
meikefrok 15:caf29b6f5261 546 servo.position(3);
meikefrok 15:caf29b6f5261 547 //pc.printf("Servo position is: center \r\n");
meikefrok 15:caf29b6f5261 548
meikefrok 15:caf29b6f5261 549 wait_ms(waiting_claw);
meikefrok 15:caf29b6f5261 550
meikefrok 15:caf29b6f5261 551 break;
meikefrok 15:caf29b6f5261 552 }
meikefrok 15:caf29b6f5261 553 case 2: {
meikefrok 15:caf29b6f5261 554 led_g = LedOn;
meikefrok 15:caf29b6f5261 555 led_r = not LedOn;
meikefrok 15:caf29b6f5261 556 led_b = not LedOn;
meikefrok 15:caf29b6f5261 557
meikefrok 15:caf29b6f5261 558 servo.position(27);
meikefrok 15:caf29b6f5261 559 //pc.printf("Servo position is: right \r\n");
meikefrok 15:caf29b6f5261 560
meikefrok 15:caf29b6f5261 561
meikefrok 15:caf29b6f5261 562 break;
meikefrok 15:caf29b6f5261 563 }
meikefrok 15:caf29b6f5261 564 }
meikefrok 15:caf29b6f5261 565
meikefrok 15:caf29b6f5261 566 }else{}
meikefrok 15:caf29b6f5261 567 }
meikefrok 15:caf29b6f5261 568 led_r = not LedOn;
meikefrok 15:caf29b6f5261 569 led_g = not LedOn;
meikefrok 15:caf29b6f5261 570
meikefrok 15:caf29b6f5261 571 position_claw = servo.read();
meikefrok 15:caf29b6f5261 572 pc.baud(115200);
meikefrok 15:caf29b6f5261 573 pc.printf("Servo position is: %i, %i, %i \r\n", num_claw_turned_on_0, num_claw_turned_on_1, num_claw_turned_on_2);
meikefrok 15:caf29b6f5261 574
meikefrok 15:caf29b6f5261 575 break;
meikefrok 15:caf29b6f5261 576 }
meikefrok 15:caf29b6f5261 577 }
megrootens 0:048fbd80203e 578
megrootens 0:048fbd80203e 579
meikefrok 5:3d88f7506cd9 580 // Switch the part
meikefrok 2:ad4b181a6422 581 void SetValue2() {
meikefrok 3:3a671d01bcb8 582 part_id = 2;
meikefrok 6:23b1ed826b59 583 }
meikefrok 2:ad4b181a6422 584
meikefrok 2:ad4b181a6422 585 void SetValue3() {
meikefrok 3:3a671d01bcb8 586 part_id = 3;
meikefrok 6:23b1ed826b59 587 }
meikefrok 2:ad4b181a6422 588
meikefrok 2:ad4b181a6422 589
meikefrok 2:ad4b181a6422 590 void SetValue4() {
meikefrok 3:3a671d01bcb8 591 part_id = 4;
megrootens 0:048fbd80203e 592 }
megrootens 0:048fbd80203e 593
meikefrok 2:ad4b181a6422 594
meikefrok 5:3d88f7506cd9 595 // Main
megrootens 0:048fbd80203e 596 int main()
megrootens 0:048fbd80203e 597 {
meikefrok 3:3a671d01bcb8 598 led_r = not LedOn;
meikefrok 3:3a671d01bcb8 599 led_g = not LedOn;
meikefrok 3:3a671d01bcb8 600 led_b = not LedOn;
megrootens 0:048fbd80203e 601
meikefrok 8:ac4e5afbdbcd 602 tick_part.attach(&SwitchCart,kTimeToggle);
meikefrok 15:caf29b6f5261 603 tick_part_arm.attach(&SwitchArm,kTimeToggle);
meikefrok 15:caf29b6f5261 604 tick_part_claw.attach(&SwitchClaw,0.1f);
meikefrok 11:97f824629da5 605 measureTicker.attach(Measure, 0.005);
meikefrok 15:caf29b6f5261 606 sampleTicker_claw.attach(scopeSend,1);
meikefrok 15:caf29b6f5261 607 sampleTicker.attach(scopeSend,0.01);
meikefrok 15:caf29b6f5261 608
meikefrok 5:3d88f7506cd9 609
meikefrok 3:3a671d01bcb8 610 btn_cart.fall(&SetValue2);
meikefrok 3:3a671d01bcb8 611 btn_arm.fall(&SetValue3);
meikefrok 3:3a671d01bcb8 612 btn_claw.fall(&SetValue4);
meikefrok 2:ad4b181a6422 613
meikefrok 12:b31df384b170 614 if(part_id == 4){
meikefrok 12:b31df384b170 615 Cart.stop(1)==1;
meikefrok 12:b31df384b170 616 }
meikefrok 12:b31df384b170 617
megrootens 0:048fbd80203e 618 while (true);
meikefrok 12:b31df384b170 619
megrootens 0:048fbd80203e 620 }
meikefrok 2:ad4b181a6422 621
meikefrok 2:ad4b181a6422 622
meikefrok 2:ad4b181a6422 623