Nick van den Berg / Mbed 2 deprecated The_Claw_with_EMG_Control_PID

Cleaner version

Dependencies:   HIDScope MODSERIAL Motordriver QEI Servo mbed

Fork of The_Claw_with_EMG_Control_PID by Meike Froklage

main.cpp@7:9715323b20ac, 2016-11-02 (annotated)

Committer:
meikefrok
Date:
Wed Nov 02 15:22:16 2016 +0000
Revision:
7:9715323b20ac
Parent:
6:23b1ed826b59
Child:
8:ac4e5afbdbcd
Backup

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"
megrootens 0:048fbd80203e 7
meikefrok 5:3d88f7506cd9 8 //======== Serial Communication ================================================
megrootens 0:048fbd80203e 9 MODSERIAL pc(USBTX,USBRX);
megrootens 0:048fbd80203e 10
meikefrok 5:3d88f7506cd9 11 //======== Motor and QEI =======================================================
meikefrok 5:3d88f7506cd9 12 int Brakeable;
meikefrok 5:3d88f7506cd9 13 int sign;
meikefrok 5:3d88f7506cd9 14
meikefrok 5:3d88f7506cd9 15 // motor
meikefrok 5:3d88f7506cd9 16 Motor Cart(D5, D4, D4, Brakeable); // right motor
meikefrok 5:3d88f7506cd9 17 Motor Arm(D6,D7, D7, Brakeable); // left motor
meikefrok 5:3d88f7506cd9 18
meikefrok 5:3d88f7506cd9 19 // qei
meikefrok 6:23b1ed826b59 20 QEI Encoder_Cart(D10, D11, NC, 6400);
meikefrok 6:23b1ed826b59 21 QEI Encoder_Arm(D12, D13, NC, 6400);
megrootens 0:048fbd80203e 22
meikefrok 5:3d88f7506cd9 23 // servo
meikefrok 5:3d88f7506cd9 24 Servo servo(D9);
megrootens 0:048fbd80203e 25
meikefrok 5:3d88f7506cd9 26 //======== Miscellaneous =======================================================
meikefrok 5:3d88f7506cd9 27 // button
meikefrok 5:3d88f7506cd9 28 InterruptIn btn(SW2);
meikefrok 5:3d88f7506cd9 29 InterruptIn btn2(SW3);
meikefrok 5:3d88f7506cd9 30
meikefrok 5:3d88f7506cd9 31 InterruptIn btn_cart(D1);
meikefrok 5:3d88f7506cd9 32 InterruptIn btn_arm(D2);
meikefrok 5:3d88f7506cd9 33 InterruptIn btn_claw(D3);
meikefrok 5:3d88f7506cd9 34
meikefrok 5:3d88f7506cd9 35 // led
meikefrok 2:ad4b181a6422 36 DigitalOut led_r(LED_RED);
meikefrok 2:ad4b181a6422 37 DigitalOut led_g(LED_GREEN);
meikefrok 2:ad4b181a6422 38 DigitalOut led_b(LED_BLUE);
megrootens 0:048fbd80203e 39
meikefrok 5:3d88f7506cd9 40 // potmeter
meikefrok 5:3d88f7506cd9 41 AnalogIn pot_cart(A2);
meikefrok 5:3d88f7506cd9 42 AnalogIn pot_arm(A3);
megrootens 0:048fbd80203e 43
meikefrok 7:9715323b20ac 44 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 45 AnalogIn emgr(A0);
meikefrok 7:9715323b20ac 46
meikefrok 5:3d88f7506cd9 47 // ticker
meikefrok 7:9715323b20ac 48 Ticker tick_part; // ticker to switch parts
meikefrok 7:9715323b20ac 49 Ticker sampleTicker;
meikefrok 5:3d88f7506cd9 50
meikefrok 5:3d88f7506cd9 51 //======== Variables ===========================================================
megrootens 0:048fbd80203e 52 // counters
meikefrok 5:3d88f7506cd9 53 int num_turned_on_0 = 0; // count number of times red LED turned on
meikefrok 5:3d88f7506cd9 54 int num_turned_on_1 = 0; // count number of times green LED turned on
meikefrok 5:3d88f7506cd9 55 int num_turned_on_2 = 0; // count number of times blue LED turned on
meikefrok 5:3d88f7506cd9 56
meikefrok 5:3d88f7506cd9 57 int num_claw_turned_on_0 = 0; // count number of times red LED turned on
meikefrok 5:3d88f7506cd9 58 int num_claw_turned_on_1 = 0; // count number of times green LED turned on
meikefrok 5:3d88f7506cd9 59 int num_claw_turned_on_2 = 0; // count number of times blue LED turned on
megrootens 0:048fbd80203e 60
meikefrok 5:3d88f7506cd9 61 // speed
meikefrok 6:23b1ed826b59 62 double cart_speed = 0.3;
meikefrok 6:23b1ed826b59 63 double arm_speed = 0.1;
megrootens 0:048fbd80203e 64
meikefrok 5:3d88f7506cd9 65 // position
meikefrok 5:3d88f7506cd9 66 float factor_cart = 0.06559;
meikefrok 5:3d88f7506cd9 67 float factor_arm = 0.1539;
meikefrok 5:3d88f7506cd9 68 int position_cart;
meikefrok 5:3d88f7506cd9 69 int position_arm;
meikefrok 5:3d88f7506cd9 70 float ain_cart; //Variable to store the analog input of the cart
meikefrok 5:3d88f7506cd9 71 float ain_arm; //Variable to store the analog input of the arm
megrootens 0:048fbd80203e 72
meikefrok 6:23b1ed826b59 73 int position_claw;
meikefrok 6:23b1ed826b59 74
meikefrok 6:23b1ed826b59 75
meikefrok 5:3d88f7506cd9 76 // miscellaneous
meikefrok 5:3d88f7506cd9 77 const float kTimeToggle = 0.25f; // period with which to toggle the parts
meikefrok 5:3d88f7506cd9 78 const int LedOn = 0; // LED on if 0
meikefrok 7:9715323b20ac 79 volatile int part_id = 1; // ID of what part should move, begins with the cart
meikefrok 5:3d88f7506cd9 80 volatile int servo_id = 1; // ID to the side the servo should move, begins in center position
meikefrok 3:3a671d01bcb8 81
meikefrok 7:9715323b20ac 82 //======== Variables Filter ====================================================
meikefrok 7:9715323b20ac 83 /*coefficients of each filter
meikefrok 7:9715323b20ac 84 lno = left tricep notch filter
meikefrok 7:9715323b20ac 85 lhf = left tricep high pass filter
meikefrok 7:9715323b20ac 86 llf = left tricep lowpass filter
meikefrok 7:9715323b20ac 87 same goes for rno etc.
meikefrok 7:9715323b20ac 88 */
meikefrok 7:9715323b20ac 89 double lno_b0 = 0.9911;
meikefrok 7:9715323b20ac 90 double lno_b1 = -1.6036;
meikefrok 7:9715323b20ac 91 double lno_b2 = 0.9911;
meikefrok 7:9715323b20ac 92 double lno_a1 = -1.603;
meikefrok 7:9715323b20ac 93 double lno_a2 = 0.9822;
meikefrok 7:9715323b20ac 94
meikefrok 7:9715323b20ac 95 double rno_b0 = 0.9911;
meikefrok 7:9715323b20ac 96 double rno_b1 = -1.6036;
meikefrok 7:9715323b20ac 97 double rno_b2 = 0.9911;
meikefrok 7:9715323b20ac 98 double rno_a1 = -1.603;
meikefrok 7:9715323b20ac 99 double rno_a2 = 0.9822;
meikefrok 7:9715323b20ac 100
meikefrok 7:9715323b20ac 101 double lno2_b0 = 0.9824;
meikefrok 7:9715323b20ac 102 double lno2_b1 = -0.6071;
meikefrok 7:9715323b20ac 103 double lno2_b2 = 0.9824;
meikefrok 7:9715323b20ac 104 double lno2_a1 = -0.6071;
meikefrok 7:9715323b20ac 105 double lno2_a2 = 0.9647;
meikefrok 7:9715323b20ac 106
meikefrok 7:9715323b20ac 107 double rno2_b0 = 0.9824;
meikefrok 7:9715323b20ac 108 double rno2_b1 = -0.6071;
meikefrok 7:9715323b20ac 109 double rno2_b2 = 0.9824;
meikefrok 7:9715323b20ac 110 double rno2_a1 = -0.6071;
meikefrok 7:9715323b20ac 111 double rno2_a2 = 0.9647;
meikefrok 7:9715323b20ac 112
meikefrok 7:9715323b20ac 113 double lhf_b0 = 0.9355;
meikefrok 7:9715323b20ac 114 double lhf_b1 = -1.8711;
meikefrok 7:9715323b20ac 115 double lhf_b2 = 0.9355;
meikefrok 7:9715323b20ac 116 double lhf_a1 = -1.8669;
meikefrok 7:9715323b20ac 117 double lhf_a2 = 0.8752;
meikefrok 7:9715323b20ac 118
meikefrok 7:9715323b20ac 119 double rhf_b0 = 0.9355;
meikefrok 7:9715323b20ac 120 double rhf_b1 = -1.8711;
meikefrok 7:9715323b20ac 121 double rhf_b2 = 0.9355;
meikefrok 7:9715323b20ac 122 double rhf_a1 = -1.8669;
meikefrok 7:9715323b20ac 123 double rhf_a2 = 0.8752;
meikefrok 7:9715323b20ac 124
meikefrok 7:9715323b20ac 125
meikefrok 7:9715323b20ac 126 double llf_b0 = 8.7656e-5;
meikefrok 7:9715323b20ac 127 double llf_b1 = 1.17531e-4;
meikefrok 7:9715323b20ac 128 double llf_b2 = 8.7656e-5;
meikefrok 7:9715323b20ac 129 double llf_a1 = -1.9733;
meikefrok 7:9715323b20ac 130 double llf_a2 = 0.9737;
meikefrok 7:9715323b20ac 131
meikefrok 7:9715323b20ac 132 double rlf_b0 = 8.7656e-5;
meikefrok 7:9715323b20ac 133 double rlf_b1 = 1.17531e-4;
meikefrok 7:9715323b20ac 134 double rlf_b2 = 8.7656e-5;
meikefrok 7:9715323b20ac 135 double rlf_a1 = -1.9733;
meikefrok 7:9715323b20ac 136 double rlf_a2 = 0.9737;
meikefrok 7:9715323b20ac 137
meikefrok 7:9715323b20ac 138
meikefrok 7:9715323b20ac 139 //starting values of the biquads of the corresponding filters
meikefrok 7:9715323b20ac 140 double lno_v1 = 0, lno_v2 = 0;
meikefrok 7:9715323b20ac 141 double lno2_v1 = 0, lno2_v2 = 0;
meikefrok 7:9715323b20ac 142 double lhf_v1 = 0, lhf_v2 = 0;
meikefrok 7:9715323b20ac 143 double llf_v1 = 0, llf_v2 = 0;
meikefrok 7:9715323b20ac 144
meikefrok 7:9715323b20ac 145 double rno_v1 = 0, rno_v2 = 0;
meikefrok 7:9715323b20ac 146 double rno2_v1 = 0, rno2_v2 = 0;
meikefrok 7:9715323b20ac 147 double rhf_v1 = 0, rhf_v2 = 0;
meikefrok 7:9715323b20ac 148 double rlf_v1 = 0, rlf_v2 = 0;
meikefrok 7:9715323b20ac 149
meikefrok 7:9715323b20ac 150 /* declaration of the outputs of each biquad.
meikefrok 7:9715323b20ac 151 the output of the previous biquad is the input for the next biquad.
meikefrok 7:9715323b20ac 152 so lno_y goes into lhf_y etc.
meikefrok 7:9715323b20ac 153 */
meikefrok 7:9715323b20ac 154 double lno_y;
meikefrok 7:9715323b20ac 155 double lno2_y;
meikefrok 7:9715323b20ac 156 double lhf_y;
meikefrok 7:9715323b20ac 157 double llf_y;
meikefrok 7:9715323b20ac 158 double lrect_y;
meikefrok 7:9715323b20ac 159 double rno_y;
meikefrok 7:9715323b20ac 160 double rno2_y;
meikefrok 7:9715323b20ac 161 double rhf_y;
meikefrok 7:9715323b20ac 162 double rlf_y;
meikefrok 7:9715323b20ac 163 double rrect_y;
meikefrok 7:9715323b20ac 164
meikefrok 7:9715323b20ac 165 // set the threshold value for the filtered signal
meikefrok 7:9715323b20ac 166 //if the signal exceeds this value the motors will start to rotate
meikefrok 7:9715323b20ac 167 const double threshold_value_left= 0.08;
meikefrok 7:9715323b20ac 168 const double threshold_value= 0.08;
meikefrok 7:9715323b20ac 169
meikefrok 7:9715323b20ac 170
meikefrok 7:9715323b20ac 171 /* declaration of each biquad
meikefrok 7:9715323b20ac 172 The coefficients will be filled in later on in void scopeSend
meikefrok 7:9715323b20ac 173 As said before the input of each biquad is the output of the previous one
meikefrok 7:9715323b20ac 174 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 175 This is done for both left and right so this makes two chains of 3 biquads */
meikefrok 7:9715323b20ac 176
meikefrok 7:9715323b20ac 177 double biquad_lno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 178 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 179 {
meikefrok 7:9715323b20ac 180 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 181 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 182 v2 = v1;
meikefrok 7:9715323b20ac 183 v1 = v;
meikefrok 7:9715323b20ac 184 return y;
meikefrok 7:9715323b20ac 185 }
meikefrok 7:9715323b20ac 186
meikefrok 7:9715323b20ac 187 double biquad_lno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 188 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 189 {
meikefrok 7:9715323b20ac 190 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 191 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 192 v2 = v1;
meikefrok 7:9715323b20ac 193 v1 = v;
meikefrok 7:9715323b20ac 194 return y;
meikefrok 7:9715323b20ac 195 }
meikefrok 7:9715323b20ac 196
meikefrok 7:9715323b20ac 197 double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 198 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 199 {
meikefrok 7:9715323b20ac 200 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 201 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 202 v2 = v1;
meikefrok 7:9715323b20ac 203 v1 = v;
meikefrok 7:9715323b20ac 204 return y;
meikefrok 7:9715323b20ac 205 }
meikefrok 7:9715323b20ac 206
meikefrok 7:9715323b20ac 207 double biquad_llf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 208 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 209 {
meikefrok 7:9715323b20ac 210 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 211 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 212 v2 = v1;
meikefrok 7:9715323b20ac 213 v1 = v;
meikefrok 7:9715323b20ac 214 return y;
meikefrok 7:9715323b20ac 215 }
meikefrok 7:9715323b20ac 216 double biquad_rno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 217 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 218 {
meikefrok 7:9715323b20ac 219 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 220 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 221 v2 = v1;
meikefrok 7:9715323b20ac 222 v1 = v;
meikefrok 7:9715323b20ac 223 return y;
meikefrok 7:9715323b20ac 224 }
meikefrok 7:9715323b20ac 225
meikefrok 7:9715323b20ac 226 double biquad_rno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 227 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 228 {
meikefrok 7:9715323b20ac 229 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 230 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 231 v2 = v1;
meikefrok 7:9715323b20ac 232 v1 = v;
meikefrok 7:9715323b20ac 233 return y;
meikefrok 7:9715323b20ac 234 }
meikefrok 7:9715323b20ac 235
meikefrok 7:9715323b20ac 236 double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 237 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 238 {
meikefrok 7:9715323b20ac 239 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 240 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 241 v2 = v1;
meikefrok 7:9715323b20ac 242 v1 = v;
meikefrok 7:9715323b20ac 243 return y;
meikefrok 7:9715323b20ac 244 }
meikefrok 7:9715323b20ac 245
meikefrok 7:9715323b20ac 246 double biquad_rlf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
meikefrok 7:9715323b20ac 247 const double b1 , const double b2 )
meikefrok 7:9715323b20ac 248 {
meikefrok 7:9715323b20ac 249 double v = u - a1*v1 - a2*v2;
meikefrok 7:9715323b20ac 250 double y = b0*v + b1*v1 + b2*v2;
meikefrok 7:9715323b20ac 251 v2 = v1;
meikefrok 7:9715323b20ac 252 v1 = v;
meikefrok 7:9715323b20ac 253 return y;
meikefrok 7:9715323b20ac 254 }
meikefrok 7:9715323b20ac 255
meikefrok 7:9715323b20ac 256 /* function that calculates the filtered EMG signal from the raw EMG signal.
meikefrok 7:9715323b20ac 257 So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
meikefrok 7:9715323b20ac 258 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
meikefrok 7:9715323b20ac 259 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 260
meikefrok 7:9715323b20ac 261
meikefrok 7:9715323b20ac 262 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
meikefrok 7:9715323b20ac 263 //======== Functions and main ==============================================================
meikefrok 7:9715323b20ac 264 /* function that calculates the filtered EMG signal from the raw EMG signal.
meikefrok 7:9715323b20ac 265 So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
meikefrok 7:9715323b20ac 266 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
meikefrok 7:9715323b20ac 267 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 268 void scopeSend(void){
meikefrok 7:9715323b20ac 269 lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
meikefrok 7:9715323b20ac 270 lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
meikefrok 7:9715323b20ac 271 lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
meikefrok 7:9715323b20ac 272 lrect_y = fabs(lhf_y);
meikefrok 7:9715323b20ac 273 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 274 rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
meikefrok 7:9715323b20ac 275 rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);
meikefrok 7:9715323b20ac 276 rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
meikefrok 7:9715323b20ac 277 rrect_y = fabs(rhf_y);
meikefrok 7:9715323b20ac 278 rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;
meikefrok 7:9715323b20ac 279 }
meikefrok 7:9715323b20ac 280
meikefrok 3:3a671d01bcb8 281
meikefrok 5:3d88f7506cd9 282 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
meikefrok 5:3d88f7506cd9 283 //======== Functions and main ==============================================================
meikefrok 3:3a671d01bcb8 284
meikefrok 5:3d88f7506cd9 285 // Switch between Cart, Arm and Claw
meikefrok 7:9715323b20ac 286
meikefrok 7:9715323b20ac 287 void SwitchPart(){
meikefrok 2:ad4b181a6422 288 switch (part_id) {
meikefrok 5:3d88f7506cd9 289 //Cart
meikefrok 3:3a671d01bcb8 290 case 2: {
meikefrok 5:3d88f7506cd9 291 led_r = LedOn;
meikefrok 5:3d88f7506cd9 292 if(led_r == LedOn){
megrootens 0:048fbd80203e 293 num_turned_on_0++;
meikefrok 5:3d88f7506cd9 294
meikefrok 5:3d88f7506cd9 295 if(btn && btn2) {
meikefrok 5:3d88f7506cd9 296 Arm.speed(0) == 0;
meikefrok 5:3d88f7506cd9 297 Cart.speed(0) == 0;
meikefrok 5:3d88f7506cd9 298
meikefrok 5:3d88f7506cd9 299 }else if (btn && !btn2) {
meikefrok 5:3d88f7506cd9 300 if(position_cart <= -105){ //If the cart is at the right side, it stops
meikefrok 6:23b1ed826b59 301 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 302
meikefrok 6:23b1ed826b59 303 }else if(position_cart <= -90){ //If the cart is at 90 mm at the right, it slows down
meikefrok 6:23b1ed826b59 304 Cart.speed(0.2)==0.2;
meikefrok 5:3d88f7506cd9 305
meikefrok 6:23b1ed826b59 306 }else if(position_cart >= 105 && 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 307 Cart.stop(1) == 1;
meikefrok 6:23b1ed826b59 308
meikefrok 6:23b1ed826b59 309 }else if(position_cart >= 105 && position_arm <=-35 && position_claw == -18){
meikefrok 6:23b1ed826b59 310 Cart.stop(1) == 1;
meikefrok 5:3d88f7506cd9 311
meikefrok 5:3d88f7506cd9 312 }else{
meikefrok 5:3d88f7506cd9 313 Cart.speed(cart_speed)==cart_speed;
meikefrok 5:3d88f7506cd9 314 }
meikefrok 5:3d88f7506cd9 315
meikefrok 5:3d88f7506cd9 316 }else if (!btn && btn2) {
meikefrok 5:3d88f7506cd9 317 if(position_cart >= 105){ //If the cart is at the left side, it stops
meikefrok 6:23b1ed826b59 318 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 319
meikefrok 6:23b1ed826b59 320 }else if(position_cart >= 90){ //If the cart is at the right side, it stops
meikefrok 6:23b1ed826b59 321 Cart.speed(-0.2)==-0.2;
meikefrok 5:3d88f7506cd9 322
meikefrok 6:23b1ed826b59 323 }else if(position_cart <= -105 && position_arm >=45){ //If the cart is at the right side and the arm is rotated 60 degrees to the right, the cart can't move to the left.
meikefrok 6:23b1ed826b59 324 Cart.stop(1)==1;
meikefrok 5:3d88f7506cd9 325
meikefrok 6:23b1ed826b59 326 }else if(position_cart <= -105 && position_arm >=35 && position_claw == 27){
meikefrok 6:23b1ed826b59 327 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 328
meikefrok 5:3d88f7506cd9 329 }else{
meikefrok 5:3d88f7506cd9 330 Cart.speed(-cart_speed)==-cart_speed;
meikefrok 5:3d88f7506cd9 331 }
meikefrok 5:3d88f7506cd9 332
meikefrok 5:3d88f7506cd9 333 }else {
meikefrok 5:3d88f7506cd9 334 Arm.speed(0) == 0;
meikefrok 6:23b1ed826b59 335 if(position_cart<0){
meikefrok 6:23b1ed826b59 336 Cart.speed(-0.1)== -0.1;
meikefrok 6:23b1ed826b59 337 }else if(position_cart>0){
meikefrok 6:23b1ed826b59 338 Cart.speed(0.1)==0.1;
meikefrok 6:23b1ed826b59 339 }else{
meikefrok 6:23b1ed826b59 340 Cart.stop(0)==0;
meikefrok 6:23b1ed826b59 341 }
meikefrok 6:23b1ed826b59 342
meikefrok 5:3d88f7506cd9 343 }
megrootens 0:048fbd80203e 344 }
meikefrok 5:3d88f7506cd9 345 // controle LED
meikefrok 3:3a671d01bcb8 346 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 347 led_b = not LedOn;
meikefrok 5:3d88f7506cd9 348
meikefrok 5:3d88f7506cd9 349 // encoder
meikefrok 5:3d88f7506cd9 350 position_cart = (Encoder_Cart.getPulses()*factor_cart) ;
meikefrok 5:3d88f7506cd9 351 ain_cart = pot_cart.read();
meikefrok 5:3d88f7506cd9 352
meikefrok 5:3d88f7506cd9 353 wait(0.1);
meikefrok 5:3d88f7506cd9 354 pc.baud(115200);
meikefrok 5:3d88f7506cd9 355 pc.printf("Distance in mm: %i\n", position_cart);
meikefrok 5:3d88f7506cd9 356
megrootens 0:048fbd80203e 357 break;
megrootens 0:048fbd80203e 358 }
megrootens 0:048fbd80203e 359 }
megrootens 0:048fbd80203e 360 }
megrootens 0:048fbd80203e 361
megrootens 0:048fbd80203e 362
meikefrok 5:3d88f7506cd9 363 // Switch the part
meikefrok 2:ad4b181a6422 364 void SetValue2() {
meikefrok 3:3a671d01bcb8 365 part_id = 2;
meikefrok 6:23b1ed826b59 366 }
meikefrok 2:ad4b181a6422 367
meikefrok 2:ad4b181a6422 368 void SetValue3() {
meikefrok 3:3a671d01bcb8 369 part_id = 3;
meikefrok 6:23b1ed826b59 370 }
meikefrok 2:ad4b181a6422 371
meikefrok 2:ad4b181a6422 372
meikefrok 2:ad4b181a6422 373 void SetValue4() {
meikefrok 3:3a671d01bcb8 374 part_id = 4;
megrootens 0:048fbd80203e 375 }
megrootens 0:048fbd80203e 376
meikefrok 2:ad4b181a6422 377
meikefrok 5:3d88f7506cd9 378 // Main
megrootens 0:048fbd80203e 379 int main()
megrootens 0:048fbd80203e 380 {
meikefrok 3:3a671d01bcb8 381 led_r = not LedOn;
meikefrok 3:3a671d01bcb8 382 led_g = not LedOn;
meikefrok 3:3a671d01bcb8 383 led_b = not LedOn;
megrootens 0:048fbd80203e 384
meikefrok 5:3d88f7506cd9 385 tick_part.attach(&SwitchPart,kTimeToggle);
meikefrok 7:9715323b20ac 386 sampleTicker.attach(scopeSend,0.01);
meikefrok 5:3d88f7506cd9 387
meikefrok 3:3a671d01bcb8 388 btn_cart.fall(&SetValue2);
meikefrok 3:3a671d01bcb8 389 btn_arm.fall(&SetValue3);
meikefrok 3:3a671d01bcb8 390 btn_claw.fall(&SetValue4);
meikefrok 2:ad4b181a6422 391
megrootens 0:048fbd80203e 392 while (true);
megrootens 0:048fbd80203e 393 }
meikefrok 2:ad4b181a6422 394
meikefrok 2:ad4b181a6422 395
meikefrok 2:ad4b181a6422 396