Cleaner version
Dependencies: HIDScope MODSERIAL Motordriver QEI Servo mbed
Fork of The_Claw_with_EMG_Control_PID by
main.cpp@5:3d88f7506cd9, 2016-10-28 (annotated)
- Committer:
- meikefrok
- Date:
- Fri Oct 28 09:01:41 2016 +0000
- Revision:
- 5:3d88f7506cd9
- Parent:
- 4:e62a2df0a5b5
- Child:
- 6:23b1ed826b59
Code of "The Claw" without EMG;
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" |
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 | 5:3d88f7506cd9 | 20 | QEI Encoder_Cart(D10, D11, NC, 64); |
meikefrok | 5:3d88f7506cd9 | 21 | QEI Encoder_Arm(D12, D13, NC, 64); |
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 | 5:3d88f7506cd9 | 44 | // ticker |
meikefrok | 5:3d88f7506cd9 | 45 | Ticker tick_part; // ticker to switch parts |
meikefrok | 5:3d88f7506cd9 | 46 | |
meikefrok | 5:3d88f7506cd9 | 47 | //======== Variables =========================================================== |
megrootens | 0:048fbd80203e | 48 | // counters |
meikefrok | 5:3d88f7506cd9 | 49 | int num_turned_on_0 = 0; // count number of times red LED turned on |
meikefrok | 5:3d88f7506cd9 | 50 | int num_turned_on_1 = 0; // count number of times green LED turned on |
meikefrok | 5:3d88f7506cd9 | 51 | int num_turned_on_2 = 0; // count number of times blue LED turned on |
meikefrok | 5:3d88f7506cd9 | 52 | |
meikefrok | 5:3d88f7506cd9 | 53 | int num_claw_turned_on_0 = 0; // count number of times red LED turned on |
meikefrok | 5:3d88f7506cd9 | 54 | int num_claw_turned_on_1 = 0; // count number of times green LED turned on |
meikefrok | 5:3d88f7506cd9 | 55 | int num_claw_turned_on_2 = 0; // count number of times blue LED turned on |
megrootens | 0:048fbd80203e | 56 | |
meikefrok | 5:3d88f7506cd9 | 57 | // speed |
meikefrok | 5:3d88f7506cd9 | 58 | double cart_speed = 0.5; |
meikefrok | 5:3d88f7506cd9 | 59 | double cart_stop = 0.2; |
meikefrok | 5:3d88f7506cd9 | 60 | double arm_speed = 0.3; |
meikefrok | 5:3d88f7506cd9 | 61 | double arm_stop = 0.1; |
megrootens | 0:048fbd80203e | 62 | |
meikefrok | 5:3d88f7506cd9 | 63 | // position |
meikefrok | 5:3d88f7506cd9 | 64 | float factor_cart = 0.06559; |
meikefrok | 5:3d88f7506cd9 | 65 | float factor_arm = 0.1539; |
meikefrok | 5:3d88f7506cd9 | 66 | int position_cart; |
meikefrok | 5:3d88f7506cd9 | 67 | int position_arm; |
meikefrok | 5:3d88f7506cd9 | 68 | float ain_cart; //Variable to store the analog input of the cart |
meikefrok | 5:3d88f7506cd9 | 69 | float ain_arm; //Variable to store the analog input of the arm |
megrootens | 0:048fbd80203e | 70 | |
meikefrok | 5:3d88f7506cd9 | 71 | // miscellaneous |
meikefrok | 5:3d88f7506cd9 | 72 | const float kTimeToggle = 0.25f; // period with which to toggle the parts |
meikefrok | 5:3d88f7506cd9 | 73 | const int LedOn = 0; // LED on if 0 |
meikefrok | 5:3d88f7506cd9 | 74 | volatile int part_id = 2; // ID of what part should move, begins with the cart |
meikefrok | 5:3d88f7506cd9 | 75 | volatile int servo_id = 1; // ID to the side the servo should move, begins in center position |
meikefrok | 3:3a671d01bcb8 | 76 | |
meikefrok | 3:3a671d01bcb8 | 77 | |
meikefrok | 3:3a671d01bcb8 | 78 | |
meikefrok | 5:3d88f7506cd9 | 79 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |
meikefrok | 5:3d88f7506cd9 | 80 | //======== Functions and main ============================================================== |
meikefrok | 3:3a671d01bcb8 | 81 | |
meikefrok | 5:3d88f7506cd9 | 82 | // Switch between Cart, Arm and Claw |
meikefrok | 3:3a671d01bcb8 | 83 | void SwitchPart() |
megrootens | 0:048fbd80203e | 84 | { |
meikefrok | 2:ad4b181a6422 | 85 | switch (part_id) { |
meikefrok | 5:3d88f7506cd9 | 86 | //Cart |
meikefrok | 3:3a671d01bcb8 | 87 | case 2: { |
meikefrok | 5:3d88f7506cd9 | 88 | led_r = LedOn; |
meikefrok | 5:3d88f7506cd9 | 89 | if(led_r == LedOn){ |
megrootens | 0:048fbd80203e | 90 | num_turned_on_0++; |
meikefrok | 5:3d88f7506cd9 | 91 | |
meikefrok | 5:3d88f7506cd9 | 92 | if(btn && btn2) { |
meikefrok | 5:3d88f7506cd9 | 93 | Arm.speed(0) == 0; |
meikefrok | 5:3d88f7506cd9 | 94 | Cart.speed(0) == 0; |
meikefrok | 5:3d88f7506cd9 | 95 | |
meikefrok | 5:3d88f7506cd9 | 96 | }else if (btn && !btn2) { |
meikefrok | 5:3d88f7506cd9 | 97 | if(position_cart <= -105){ //If the cart is at the right side, it stops |
meikefrok | 5:3d88f7506cd9 | 98 | Cart.speed(-cart_stop)==-cart_stop; |
meikefrok | 5:3d88f7506cd9 | 99 | |
meikefrok | 5:3d88f7506cd9 | 100 | }else if(position_cart >= 105 && position_arm <=-60){ //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 | 5:3d88f7506cd9 | 101 | Cart.speed(cart_stop) == cart_stop; |
meikefrok | 5:3d88f7506cd9 | 102 | |
meikefrok | 5:3d88f7506cd9 | 103 | }else{ |
meikefrok | 5:3d88f7506cd9 | 104 | Cart.speed(cart_speed)==cart_speed; |
meikefrok | 5:3d88f7506cd9 | 105 | } |
meikefrok | 5:3d88f7506cd9 | 106 | |
meikefrok | 5:3d88f7506cd9 | 107 | }else if (!btn && btn2) { |
meikefrok | 5:3d88f7506cd9 | 108 | if(position_cart >= 105){ //If the cart is at the left side, it stops |
meikefrok | 5:3d88f7506cd9 | 109 | Cart.speed(cart_stop)==cart_stop; |
meikefrok | 5:3d88f7506cd9 | 110 | |
meikefrok | 5:3d88f7506cd9 | 111 | }else if(position_cart <= -105 && position_arm >=60){ //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 | 5:3d88f7506cd9 | 112 | Cart.speed(-cart_stop) == -cart_stop; |
meikefrok | 5:3d88f7506cd9 | 113 | |
meikefrok | 5:3d88f7506cd9 | 114 | }else{ |
meikefrok | 5:3d88f7506cd9 | 115 | Cart.speed(-cart_speed)==-cart_speed; |
meikefrok | 5:3d88f7506cd9 | 116 | } |
meikefrok | 5:3d88f7506cd9 | 117 | |
meikefrok | 5:3d88f7506cd9 | 118 | }else { |
meikefrok | 5:3d88f7506cd9 | 119 | Arm.speed(0) == 0; |
meikefrok | 5:3d88f7506cd9 | 120 | Cart.speed(0) == 0; |
meikefrok | 5:3d88f7506cd9 | 121 | } |
megrootens | 0:048fbd80203e | 122 | } |
meikefrok | 5:3d88f7506cd9 | 123 | // controle LED |
meikefrok | 3:3a671d01bcb8 | 124 | led_g = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 125 | led_b = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 126 | |
meikefrok | 5:3d88f7506cd9 | 127 | // encoder |
meikefrok | 5:3d88f7506cd9 | 128 | position_cart = (Encoder_Cart.getPulses()*factor_cart) ; |
meikefrok | 5:3d88f7506cd9 | 129 | ain_cart = pot_cart.read(); |
meikefrok | 5:3d88f7506cd9 | 130 | |
meikefrok | 5:3d88f7506cd9 | 131 | if (ain_cart == 0){ |
meikefrok | 5:3d88f7506cd9 | 132 | Encoder_Cart.reset(); |
meikefrok | 5:3d88f7506cd9 | 133 | }else {} |
meikefrok | 5:3d88f7506cd9 | 134 | |
meikefrok | 5:3d88f7506cd9 | 135 | wait(0.1); |
meikefrok | 5:3d88f7506cd9 | 136 | pc.baud(115200); |
meikefrok | 5:3d88f7506cd9 | 137 | pc.printf("Distance in mm: %i\n", position_cart); |
meikefrok | 5:3d88f7506cd9 | 138 | |
megrootens | 0:048fbd80203e | 139 | |
megrootens | 0:048fbd80203e | 140 | break; |
megrootens | 0:048fbd80203e | 141 | } |
meikefrok | 5:3d88f7506cd9 | 142 | |
meikefrok | 5:3d88f7506cd9 | 143 | //Arm |
meikefrok | 3:3a671d01bcb8 | 144 | case 3: { |
meikefrok | 5:3d88f7506cd9 | 145 | led_g = LedOn; |
meikefrok | 5:3d88f7506cd9 | 146 | if(led_g == LedOn){ |
megrootens | 0:048fbd80203e | 147 | num_turned_on_1++; |
meikefrok | 5:3d88f7506cd9 | 148 | |
meikefrok | 5:3d88f7506cd9 | 149 | if(btn && btn2) { |
meikefrok | 5:3d88f7506cd9 | 150 | Arm.speed(0) == 0; |
meikefrok | 5:3d88f7506cd9 | 151 | Cart.speed(0) == 0; |
meikefrok | 5:3d88f7506cd9 | 152 | } |
megrootens | 0:048fbd80203e | 153 | |
meikefrok | 5:3d88f7506cd9 | 154 | else if (btn && !btn2) { |
meikefrok | 5:3d88f7506cd9 | 155 | if(position_cart > -105 && position_arm >= 30){ //If the cart is not at the end, the arm can't move any further than 30 degrees |
meikefrok | 5:3d88f7506cd9 | 156 | Arm.speed(-arm_stop)==-arm_stop; |
meikefrok | 5:3d88f7506cd9 | 157 | |
meikefrok | 5:3d88f7506cd9 | 158 | }else if(position_cart<= -105 && position_arm>=80){ //If the cart is at the right end, the arm can't move any further than 70 degrees |
meikefrok | 5:3d88f7506cd9 | 159 | Arm.speed(-arm_stop)==-arm_stop; |
meikefrok | 5:3d88f7506cd9 | 160 | |
meikefrok | 5:3d88f7506cd9 | 161 | }else{ |
meikefrok | 5:3d88f7506cd9 | 162 | Arm.speed(arm_speed)==arm_speed; |
meikefrok | 5:3d88f7506cd9 | 163 | } |
meikefrok | 5:3d88f7506cd9 | 164 | |
meikefrok | 5:3d88f7506cd9 | 165 | }else if (!btn && btn2) { |
meikefrok | 5:3d88f7506cd9 | 166 | if(position_cart < 105 && position_arm <= -30){ //If the cart is not at the end, the arm can't move any further than 30 degrees |
meikefrok | 5:3d88f7506cd9 | 167 | Arm.speed(arm_stop)==arm_stop; |
meikefrok | 5:3d88f7506cd9 | 168 | |
meikefrok | 5:3d88f7506cd9 | 169 | }else if(position_cart>=105 && position_arm<=-80){ //If the cart is at the left end, the arm can't move any further than 70 degrees |
meikefrok | 5:3d88f7506cd9 | 170 | Arm.speed(arm_stop)==arm_stop; |
meikefrok | 5:3d88f7506cd9 | 171 | |
meikefrok | 5:3d88f7506cd9 | 172 | }else{ |
meikefrok | 5:3d88f7506cd9 | 173 | Arm.speed(-arm_speed)==-arm_speed; |
meikefrok | 5:3d88f7506cd9 | 174 | } |
meikefrok | 5:3d88f7506cd9 | 175 | |
meikefrok | 5:3d88f7506cd9 | 176 | }else { |
meikefrok | 5:3d88f7506cd9 | 177 | Arm.speed(0) == 0; |
meikefrok | 5:3d88f7506cd9 | 178 | Cart.speed(0) == 0; |
meikefrok | 5:3d88f7506cd9 | 179 | } |
meikefrok | 5:3d88f7506cd9 | 180 | } |
meikefrok | 5:3d88f7506cd9 | 181 | // controle LED |
meikefrok | 3:3a671d01bcb8 | 182 | led_r = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 183 | led_b = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 184 | |
meikefrok | 5:3d88f7506cd9 | 185 | // encoder |
meikefrok | 5:3d88f7506cd9 | 186 | position_arm = (Encoder_Arm.getPulses()*factor_arm) ; |
meikefrok | 5:3d88f7506cd9 | 187 | ain_arm = pot_arm.read(); |
meikefrok | 5:3d88f7506cd9 | 188 | |
meikefrok | 5:3d88f7506cd9 | 189 | if (ain_arm == 0){ |
meikefrok | 5:3d88f7506cd9 | 190 | Encoder_Arm.reset(); |
meikefrok | 5:3d88f7506cd9 | 191 | }else {} |
meikefrok | 5:3d88f7506cd9 | 192 | |
meikefrok | 5:3d88f7506cd9 | 193 | wait(0.1); |
meikefrok | 5:3d88f7506cd9 | 194 | pc.baud(115200); |
meikefrok | 5:3d88f7506cd9 | 195 | pc.printf("Degrees: %i\n", position_arm); |
meikefrok | 5:3d88f7506cd9 | 196 | |
meikefrok | 5:3d88f7506cd9 | 197 | |
megrootens | 0:048fbd80203e | 198 | break; |
megrootens | 0:048fbd80203e | 199 | } |
meikefrok | 5:3d88f7506cd9 | 200 | |
meikefrok | 5:3d88f7506cd9 | 201 | //Claw |
meikefrok | 3:3a671d01bcb8 | 202 | case 4: { |
meikefrok | 3:3a671d01bcb8 | 203 | led_b = LedOn; |
meikefrok | 5:3d88f7506cd9 | 204 | if(led_b == LedOn){ |
megrootens | 0:048fbd80203e | 205 | num_turned_on_2++; |
meikefrok | 5:3d88f7506cd9 | 206 | |
meikefrok | 5:3d88f7506cd9 | 207 | if(btn && btn2){ |
meikefrok | 5:3d88f7506cd9 | 208 | |
meikefrok | 5:3d88f7506cd9 | 209 | }else if(btn && !btn2){ |
meikefrok | 5:3d88f7506cd9 | 210 | servo_id ++; |
meikefrok | 5:3d88f7506cd9 | 211 | |
meikefrok | 5:3d88f7506cd9 | 212 | switch (servo_id) { |
meikefrok | 5:3d88f7506cd9 | 213 | case 0: { |
meikefrok | 5:3d88f7506cd9 | 214 | led_r = LedOn; |
meikefrok | 5:3d88f7506cd9 | 215 | if (led_r == LedOn) { |
meikefrok | 5:3d88f7506cd9 | 216 | num_claw_turned_on_0++; |
meikefrok | 5:3d88f7506cd9 | 217 | } |
meikefrok | 5:3d88f7506cd9 | 218 | |
meikefrok | 5:3d88f7506cd9 | 219 | led_b = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 220 | led_g = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 221 | |
meikefrok | 5:3d88f7506cd9 | 222 | servo.position(27); |
meikefrok | 5:3d88f7506cd9 | 223 | pc.printf("Servo position is: left \r\n"); |
meikefrok | 5:3d88f7506cd9 | 224 | break; |
meikefrok | 5:3d88f7506cd9 | 225 | } |
meikefrok | 5:3d88f7506cd9 | 226 | case 1: { |
meikefrok | 5:3d88f7506cd9 | 227 | led_b = LedOn; |
meikefrok | 5:3d88f7506cd9 | 228 | if (led_b == LedOn) { |
meikefrok | 5:3d88f7506cd9 | 229 | num_claw_turned_on_1++; |
meikefrok | 5:3d88f7506cd9 | 230 | } |
meikefrok | 5:3d88f7506cd9 | 231 | |
meikefrok | 5:3d88f7506cd9 | 232 | led_r = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 233 | led_g = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 234 | |
meikefrok | 5:3d88f7506cd9 | 235 | servo.position(3); |
meikefrok | 5:3d88f7506cd9 | 236 | pc.printf("Servo position is: center \r\n"); |
meikefrok | 5:3d88f7506cd9 | 237 | break; |
meikefrok | 5:3d88f7506cd9 | 238 | } |
meikefrok | 5:3d88f7506cd9 | 239 | case 2: { |
meikefrok | 5:3d88f7506cd9 | 240 | led_g = LedOn; |
meikefrok | 5:3d88f7506cd9 | 241 | if (led_g == LedOn) { |
meikefrok | 5:3d88f7506cd9 | 242 | num_claw_turned_on_2++; |
meikefrok | 5:3d88f7506cd9 | 243 | } |
meikefrok | 5:3d88f7506cd9 | 244 | |
meikefrok | 5:3d88f7506cd9 | 245 | led_r = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 246 | led_b = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 247 | |
meikefrok | 5:3d88f7506cd9 | 248 | servo.position(-18); |
meikefrok | 5:3d88f7506cd9 | 249 | pc.printf("Servo position is: right \r\n"); |
meikefrok | 5:3d88f7506cd9 | 250 | break; |
meikefrok | 5:3d88f7506cd9 | 251 | } |
meikefrok | 5:3d88f7506cd9 | 252 | } |
meikefrok | 5:3d88f7506cd9 | 253 | |
meikefrok | 5:3d88f7506cd9 | 254 | |
meikefrok | 5:3d88f7506cd9 | 255 | }else if(!btn && btn2){ |
meikefrok | 5:3d88f7506cd9 | 256 | servo_id --; |
meikefrok | 5:3d88f7506cd9 | 257 | |
meikefrok | 5:3d88f7506cd9 | 258 | switch (servo_id) { |
meikefrok | 5:3d88f7506cd9 | 259 | case 0: { |
meikefrok | 5:3d88f7506cd9 | 260 | led_r = LedOn; |
meikefrok | 5:3d88f7506cd9 | 261 | if (led_r == LedOn) { |
meikefrok | 5:3d88f7506cd9 | 262 | num_claw_turned_on_0++; |
meikefrok | 5:3d88f7506cd9 | 263 | } |
meikefrok | 5:3d88f7506cd9 | 264 | |
meikefrok | 5:3d88f7506cd9 | 265 | led_b = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 266 | led_g = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 267 | |
meikefrok | 5:3d88f7506cd9 | 268 | servo.position(27); |
meikefrok | 5:3d88f7506cd9 | 269 | pc.printf("Servo position is: left \r\n"); |
meikefrok | 5:3d88f7506cd9 | 270 | break; |
meikefrok | 5:3d88f7506cd9 | 271 | } |
meikefrok | 5:3d88f7506cd9 | 272 | case 1: { |
meikefrok | 5:3d88f7506cd9 | 273 | led_b = LedOn; |
meikefrok | 5:3d88f7506cd9 | 274 | if (led_b == LedOn) { |
meikefrok | 5:3d88f7506cd9 | 275 | num_claw_turned_on_1++; |
meikefrok | 5:3d88f7506cd9 | 276 | } |
meikefrok | 5:3d88f7506cd9 | 277 | |
meikefrok | 5:3d88f7506cd9 | 278 | led_r = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 279 | led_g = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 280 | |
meikefrok | 5:3d88f7506cd9 | 281 | servo.position(3); |
meikefrok | 5:3d88f7506cd9 | 282 | pc.printf("Servo position is: center \r\n"); |
meikefrok | 5:3d88f7506cd9 | 283 | break; |
meikefrok | 5:3d88f7506cd9 | 284 | } |
meikefrok | 5:3d88f7506cd9 | 285 | case 2: { |
meikefrok | 5:3d88f7506cd9 | 286 | led_g = LedOn; |
meikefrok | 5:3d88f7506cd9 | 287 | if (led_g == LedOn) { |
meikefrok | 5:3d88f7506cd9 | 288 | num_claw_turned_on_2++; |
meikefrok | 5:3d88f7506cd9 | 289 | } |
meikefrok | 5:3d88f7506cd9 | 290 | |
meikefrok | 5:3d88f7506cd9 | 291 | led_r = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 292 | led_b = not LedOn; |
meikefrok | 5:3d88f7506cd9 | 293 | |
meikefrok | 5:3d88f7506cd9 | 294 | servo.position(-18); |
meikefrok | 5:3d88f7506cd9 | 295 | pc.printf("Servo position is: right \r\n"); |
meikefrok | 5:3d88f7506cd9 | 296 | break; |
meikefrok | 5:3d88f7506cd9 | 297 | } |
meikefrok | 5:3d88f7506cd9 | 298 | } |
meikefrok | 5:3d88f7506cd9 | 299 | |
meikefrok | 5:3d88f7506cd9 | 300 | }else{} |
megrootens | 0:048fbd80203e | 301 | } |
meikefrok | 3:3a671d01bcb8 | 302 | led_r = not LedOn; |
meikefrok | 3:3a671d01bcb8 | 303 | led_g = not LedOn; |
megrootens | 0:048fbd80203e | 304 | |
meikefrok | 5:3d88f7506cd9 | 305 | |
megrootens | 0:048fbd80203e | 306 | break; |
megrootens | 0:048fbd80203e | 307 | } |
megrootens | 0:048fbd80203e | 308 | } |
megrootens | 0:048fbd80203e | 309 | } |
megrootens | 0:048fbd80203e | 310 | |
megrootens | 0:048fbd80203e | 311 | |
meikefrok | 5:3d88f7506cd9 | 312 | // Switch the part |
meikefrok | 2:ad4b181a6422 | 313 | void SetValue2() { |
meikefrok | 3:3a671d01bcb8 | 314 | part_id = 2; |
meikefrok | 2:ad4b181a6422 | 315 | } |
meikefrok | 2:ad4b181a6422 | 316 | |
meikefrok | 2:ad4b181a6422 | 317 | void SetValue3() { |
meikefrok | 3:3a671d01bcb8 | 318 | part_id = 3; |
meikefrok | 2:ad4b181a6422 | 319 | } |
meikefrok | 2:ad4b181a6422 | 320 | |
meikefrok | 2:ad4b181a6422 | 321 | |
meikefrok | 2:ad4b181a6422 | 322 | void SetValue4() { |
meikefrok | 3:3a671d01bcb8 | 323 | part_id = 4; |
megrootens | 0:048fbd80203e | 324 | } |
megrootens | 0:048fbd80203e | 325 | |
meikefrok | 2:ad4b181a6422 | 326 | |
meikefrok | 5:3d88f7506cd9 | 327 | // Main |
megrootens | 0:048fbd80203e | 328 | int main() |
megrootens | 0:048fbd80203e | 329 | { |
meikefrok | 3:3a671d01bcb8 | 330 | led_r = not LedOn; |
meikefrok | 3:3a671d01bcb8 | 331 | led_g = not LedOn; |
meikefrok | 3:3a671d01bcb8 | 332 | led_b = not LedOn; |
megrootens | 0:048fbd80203e | 333 | |
meikefrok | 5:3d88f7506cd9 | 334 | tick_part.attach(&SwitchPart,kTimeToggle); |
meikefrok | 5:3d88f7506cd9 | 335 | |
meikefrok | 3:3a671d01bcb8 | 336 | btn_cart.fall(&SetValue2); |
meikefrok | 3:3a671d01bcb8 | 337 | btn_arm.fall(&SetValue3); |
meikefrok | 3:3a671d01bcb8 | 338 | btn_claw.fall(&SetValue4); |
meikefrok | 2:ad4b181a6422 | 339 | |
megrootens | 0:048fbd80203e | 340 | while (true); |
megrootens | 0:048fbd80203e | 341 | } |
meikefrok | 2:ad4b181a6422 | 342 | |
meikefrok | 2:ad4b181a6422 | 343 | |
meikefrok | 2:ad4b181a6422 | 344 |