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