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@6:23b1ed826b59, 2016-10-31 (annotated)

Committer:
meikefrok
Date:
Mon Oct 31 13:59:48 2016 +0000
Revision:
6:23b1ed826b59
Parent:
5:3d88f7506cd9
Child:
7:9715323b20ac
The Claw with restrictions;

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 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 6:23b1ed826b59 58 double cart_speed = 0.3;
meikefrok 6:23b1ed826b59 59 double arm_speed = 0.1;
megrootens 0:048fbd80203e 60
meikefrok 5:3d88f7506cd9 61 // position
meikefrok 5:3d88f7506cd9 62 float factor_cart = 0.06559;
meikefrok 5:3d88f7506cd9 63 float factor_arm = 0.1539;
meikefrok 5:3d88f7506cd9 64 int position_cart;
meikefrok 5:3d88f7506cd9 65 int position_arm;
meikefrok 5:3d88f7506cd9 66 float ain_cart; //Variable to store the analog input of the cart
meikefrok 5:3d88f7506cd9 67 float ain_arm; //Variable to store the analog input of the arm
megrootens 0:048fbd80203e 68
meikefrok 6:23b1ed826b59 69 int position_claw;
meikefrok 6:23b1ed826b59 70
meikefrok 6:23b1ed826b59 71
meikefrok 5:3d88f7506cd9 72 // miscellaneous
meikefrok 5:3d88f7506cd9 73 const float kTimeToggle = 0.25f; // period with which to toggle the parts
meikefrok 5:3d88f7506cd9 74 const int LedOn = 0; // LED on if 0
meikefrok 5:3d88f7506cd9 75 volatile int part_id = 2; // ID of what part should move, begins with the cart
meikefrok 5:3d88f7506cd9 76 volatile int servo_id = 1; // ID to the side the servo should move, begins in center position
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 6:23b1ed826b59 98 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 99
meikefrok 6:23b1ed826b59 100 }else if(position_cart <= -90){ //If the cart is at 90 mm at the right, it slows down
meikefrok 6:23b1ed826b59 101 Cart.speed(0.2)==0.2;
meikefrok 5:3d88f7506cd9 102
meikefrok 6:23b1ed826b59 103 }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 104 Cart.stop(1) == 1;
meikefrok 6:23b1ed826b59 105
meikefrok 6:23b1ed826b59 106 }else if(position_cart >= 105 && position_arm <=-35 && position_claw == -18){
meikefrok 6:23b1ed826b59 107 Cart.stop(1) == 1;
meikefrok 5:3d88f7506cd9 108
meikefrok 5:3d88f7506cd9 109 }else{
meikefrok 5:3d88f7506cd9 110 Cart.speed(cart_speed)==cart_speed;
meikefrok 5:3d88f7506cd9 111 }
meikefrok 5:3d88f7506cd9 112
meikefrok 5:3d88f7506cd9 113 }else if (!btn && btn2) {
meikefrok 5:3d88f7506cd9 114 if(position_cart >= 105){ //If the cart is at the left side, it stops
meikefrok 6:23b1ed826b59 115 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 116
meikefrok 6:23b1ed826b59 117 }else if(position_cart >= 90){ //If the cart is at the right side, it stops
meikefrok 6:23b1ed826b59 118 Cart.speed(-0.2)==-0.2;
meikefrok 5:3d88f7506cd9 119
meikefrok 6:23b1ed826b59 120 }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 121 Cart.stop(1)==1;
meikefrok 5:3d88f7506cd9 122
meikefrok 6:23b1ed826b59 123 }else if(position_cart <= -105 && position_arm >=35 && position_claw == 27){
meikefrok 6:23b1ed826b59 124 Cart.stop(1)==1;
meikefrok 6:23b1ed826b59 125
meikefrok 5:3d88f7506cd9 126 }else{
meikefrok 5:3d88f7506cd9 127 Cart.speed(-cart_speed)==-cart_speed;
meikefrok 5:3d88f7506cd9 128 }
meikefrok 5:3d88f7506cd9 129
meikefrok 5:3d88f7506cd9 130 }else {
meikefrok 5:3d88f7506cd9 131 Arm.speed(0) == 0;
meikefrok 6:23b1ed826b59 132 if(position_cart<0){
meikefrok 6:23b1ed826b59 133 Cart.speed(-0.1)== -0.1;
meikefrok 6:23b1ed826b59 134 }else if(position_cart>0){
meikefrok 6:23b1ed826b59 135 Cart.speed(0.1)==0.1;
meikefrok 6:23b1ed826b59 136 }else{
meikefrok 6:23b1ed826b59 137 Cart.stop(0)==0;
meikefrok 6:23b1ed826b59 138 }
meikefrok 6:23b1ed826b59 139
meikefrok 5:3d88f7506cd9 140 }
megrootens 0:048fbd80203e 141 }
meikefrok 5:3d88f7506cd9 142 // controle LED
meikefrok 3:3a671d01bcb8 143 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 144 led_b = not LedOn;
meikefrok 5:3d88f7506cd9 145
meikefrok 5:3d88f7506cd9 146 // encoder
meikefrok 5:3d88f7506cd9 147 position_cart = (Encoder_Cart.getPulses()*factor_cart) ;
meikefrok 5:3d88f7506cd9 148 ain_cart = pot_cart.read();
meikefrok 5:3d88f7506cd9 149
meikefrok 5:3d88f7506cd9 150 if (ain_cart == 0){
meikefrok 5:3d88f7506cd9 151 Encoder_Cart.reset();
meikefrok 5:3d88f7506cd9 152 }else {}
meikefrok 5:3d88f7506cd9 153
meikefrok 5:3d88f7506cd9 154 wait(0.1);
meikefrok 5:3d88f7506cd9 155 pc.baud(115200);
meikefrok 5:3d88f7506cd9 156 pc.printf("Distance in mm: %i\n", position_cart);
meikefrok 5:3d88f7506cd9 157
megrootens 0:048fbd80203e 158 break;
megrootens 0:048fbd80203e 159 }
meikefrok 5:3d88f7506cd9 160
meikefrok 5:3d88f7506cd9 161 //Arm
meikefrok 3:3a671d01bcb8 162 case 3: {
meikefrok 5:3d88f7506cd9 163 led_g = LedOn;
meikefrok 5:3d88f7506cd9 164 if(led_g == LedOn){
megrootens 0:048fbd80203e 165 num_turned_on_1++;
meikefrok 5:3d88f7506cd9 166
meikefrok 5:3d88f7506cd9 167 if(btn && btn2) {
meikefrok 5:3d88f7506cd9 168 Arm.speed(0) == 0;
meikefrok 5:3d88f7506cd9 169 Cart.speed(0) == 0;
meikefrok 5:3d88f7506cd9 170 }
megrootens 0:048fbd80203e 171
meikefrok 5:3d88f7506cd9 172 else if (btn && !btn2) {
meikefrok 6:23b1ed826b59 173 if(position_cart > -105 && position_arm >= 45){ //If the cart is not at the end, the arm can't move any further than 45 degrees
meikefrok 6:23b1ed826b59 174 Arm.stop(1)==1;
meikefrok 6:23b1ed826b59 175
meikefrok 6:23b1ed826b59 176 }else if(position_cart > -105 && position_arm >= 25 && position_claw == 27){
meikefrok 6:23b1ed826b59 177 Arm.stop(1)==1;
meikefrok 5:3d88f7506cd9 178
meikefrok 5:3d88f7506cd9 179 }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 6:23b1ed826b59 180 Arm.stop(1)==1;
meikefrok 5:3d88f7506cd9 181
meikefrok 5:3d88f7506cd9 182 }else{
meikefrok 5:3d88f7506cd9 183 Arm.speed(arm_speed)==arm_speed;
meikefrok 5:3d88f7506cd9 184 }
meikefrok 5:3d88f7506cd9 185
meikefrok 5:3d88f7506cd9 186 }else if (!btn && btn2) {
meikefrok 6:23b1ed826b59 187 if(position_cart < 105 && position_arm <= -45){ //If the cart is not at the end, the arm can't move any further than 45 degrees
meikefrok 6:23b1ed826b59 188 Arm.stop(1)==1;
meikefrok 6:23b1ed826b59 189
meikefrok 6:23b1ed826b59 190 }else if(position_cart < 105 && position_arm <= -25 && position_claw == -18){
meikefrok 6:23b1ed826b59 191 Arm.stop(1)==1;
meikefrok 5:3d88f7506cd9 192
meikefrok 5:3d88f7506cd9 193 }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 6:23b1ed826b59 194 Arm.stop(1)==1;
meikefrok 5:3d88f7506cd9 195
meikefrok 5:3d88f7506cd9 196 }else{
meikefrok 5:3d88f7506cd9 197 Arm.speed(-arm_speed)==-arm_speed;
meikefrok 5:3d88f7506cd9 198 }
meikefrok 5:3d88f7506cd9 199
meikefrok 5:3d88f7506cd9 200 }else {
meikefrok 6:23b1ed826b59 201 Cart.speed(0) == 0;
meikefrok 6:23b1ed826b59 202 if(position_arm>0){
meikefrok 6:23b1ed826b59 203 Arm.speed(-0.1)== -0.1;
meikefrok 6:23b1ed826b59 204 }else if(position_arm<0){
meikefrok 6:23b1ed826b59 205 Arm.speed(0.1)==0.1;
meikefrok 6:23b1ed826b59 206 }else{
meikefrok 6:23b1ed826b59 207 Arm.stop(0)==0;
meikefrok 6:23b1ed826b59 208 }
meikefrok 5:3d88f7506cd9 209 }
meikefrok 5:3d88f7506cd9 210 }
meikefrok 5:3d88f7506cd9 211 // controle LED
meikefrok 3:3a671d01bcb8 212 led_r = not LedOn;
meikefrok 5:3d88f7506cd9 213 led_b = not LedOn;
meikefrok 5:3d88f7506cd9 214
meikefrok 5:3d88f7506cd9 215 // encoder
meikefrok 5:3d88f7506cd9 216 position_arm = (Encoder_Arm.getPulses()*factor_arm) ;
meikefrok 5:3d88f7506cd9 217 ain_arm = pot_arm.read();
meikefrok 5:3d88f7506cd9 218
meikefrok 5:3d88f7506cd9 219 if (ain_arm == 0){
meikefrok 5:3d88f7506cd9 220 Encoder_Arm.reset();
meikefrok 5:3d88f7506cd9 221 }else {}
meikefrok 5:3d88f7506cd9 222
meikefrok 5:3d88f7506cd9 223 wait(0.1);
meikefrok 5:3d88f7506cd9 224 pc.baud(115200);
meikefrok 5:3d88f7506cd9 225 pc.printf("Degrees: %i\n", position_arm);
meikefrok 5:3d88f7506cd9 226
megrootens 0:048fbd80203e 227 break;
megrootens 0:048fbd80203e 228 }
meikefrok 5:3d88f7506cd9 229
meikefrok 5:3d88f7506cd9 230 //Claw
meikefrok 3:3a671d01bcb8 231 case 4: {
meikefrok 3:3a671d01bcb8 232 led_b = LedOn;
meikefrok 5:3d88f7506cd9 233 if(led_b == LedOn){
megrootens 0:048fbd80203e 234 num_turned_on_2++;
meikefrok 5:3d88f7506cd9 235
meikefrok 5:3d88f7506cd9 236 if(btn && btn2){
meikefrok 5:3d88f7506cd9 237
meikefrok 5:3d88f7506cd9 238 }else if(btn && !btn2){
meikefrok 5:3d88f7506cd9 239 servo_id ++;
meikefrok 5:3d88f7506cd9 240
meikefrok 5:3d88f7506cd9 241 switch (servo_id) {
meikefrok 5:3d88f7506cd9 242 case 0: {
meikefrok 5:3d88f7506cd9 243 led_r = LedOn;
meikefrok 5:3d88f7506cd9 244 if (led_r == LedOn) {
meikefrok 5:3d88f7506cd9 245 num_claw_turned_on_0++;
meikefrok 5:3d88f7506cd9 246 }
meikefrok 5:3d88f7506cd9 247
meikefrok 5:3d88f7506cd9 248 led_b = not LedOn;
meikefrok 5:3d88f7506cd9 249 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 250
meikefrok 6:23b1ed826b59 251 servo.position(-18);
meikefrok 5:3d88f7506cd9 252 pc.printf("Servo position is: left \r\n");
meikefrok 5:3d88f7506cd9 253 break;
meikefrok 5:3d88f7506cd9 254 }
meikefrok 5:3d88f7506cd9 255 case 1: {
meikefrok 5:3d88f7506cd9 256 led_b = LedOn;
meikefrok 5:3d88f7506cd9 257 if (led_b == LedOn) {
meikefrok 5:3d88f7506cd9 258 num_claw_turned_on_1++;
meikefrok 5:3d88f7506cd9 259 }
meikefrok 5:3d88f7506cd9 260
meikefrok 5:3d88f7506cd9 261 led_r = not LedOn;
meikefrok 5:3d88f7506cd9 262 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 263
meikefrok 5:3d88f7506cd9 264 servo.position(3);
meikefrok 5:3d88f7506cd9 265 pc.printf("Servo position is: center \r\n");
meikefrok 5:3d88f7506cd9 266 break;
meikefrok 5:3d88f7506cd9 267 }
meikefrok 5:3d88f7506cd9 268 case 2: {
meikefrok 5:3d88f7506cd9 269 led_g = LedOn;
meikefrok 5:3d88f7506cd9 270 if (led_g == LedOn) {
meikefrok 5:3d88f7506cd9 271 num_claw_turned_on_2++;
meikefrok 5:3d88f7506cd9 272 }
meikefrok 5:3d88f7506cd9 273
meikefrok 5:3d88f7506cd9 274 led_r = not LedOn;
meikefrok 5:3d88f7506cd9 275 led_b = not LedOn;
meikefrok 5:3d88f7506cd9 276
meikefrok 6:23b1ed826b59 277 servo.position(27);
meikefrok 5:3d88f7506cd9 278 pc.printf("Servo position is: right \r\n");
meikefrok 5:3d88f7506cd9 279 break;
meikefrok 5:3d88f7506cd9 280 }
meikefrok 5:3d88f7506cd9 281 }
meikefrok 5:3d88f7506cd9 282
meikefrok 5:3d88f7506cd9 283
meikefrok 5:3d88f7506cd9 284 }else if(!btn && btn2){
meikefrok 5:3d88f7506cd9 285 servo_id --;
meikefrok 5:3d88f7506cd9 286
meikefrok 5:3d88f7506cd9 287 switch (servo_id) {
meikefrok 5:3d88f7506cd9 288 case 0: {
meikefrok 5:3d88f7506cd9 289 led_r = LedOn;
meikefrok 5:3d88f7506cd9 290 if (led_r == LedOn) {
meikefrok 5:3d88f7506cd9 291 num_claw_turned_on_0++;
meikefrok 5:3d88f7506cd9 292 }
meikefrok 5:3d88f7506cd9 293
meikefrok 5:3d88f7506cd9 294 led_b = not LedOn;
meikefrok 5:3d88f7506cd9 295 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 296
meikefrok 6:23b1ed826b59 297 servo.position(-18);
meikefrok 5:3d88f7506cd9 298 pc.printf("Servo position is: left \r\n");
meikefrok 5:3d88f7506cd9 299 break;
meikefrok 5:3d88f7506cd9 300 }
meikefrok 5:3d88f7506cd9 301 case 1: {
meikefrok 5:3d88f7506cd9 302 led_b = LedOn;
meikefrok 5:3d88f7506cd9 303 if (led_b == LedOn) {
meikefrok 5:3d88f7506cd9 304 num_claw_turned_on_1++;
meikefrok 5:3d88f7506cd9 305 }
meikefrok 5:3d88f7506cd9 306
meikefrok 5:3d88f7506cd9 307 led_r = not LedOn;
meikefrok 5:3d88f7506cd9 308 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 309
meikefrok 5:3d88f7506cd9 310 servo.position(3);
meikefrok 5:3d88f7506cd9 311 pc.printf("Servo position is: center \r\n");
meikefrok 5:3d88f7506cd9 312 break;
meikefrok 5:3d88f7506cd9 313 }
meikefrok 5:3d88f7506cd9 314 case 2: {
meikefrok 5:3d88f7506cd9 315 led_g = LedOn;
meikefrok 5:3d88f7506cd9 316 if (led_g == LedOn) {
meikefrok 5:3d88f7506cd9 317 num_claw_turned_on_2++;
meikefrok 5:3d88f7506cd9 318 }
meikefrok 5:3d88f7506cd9 319
meikefrok 5:3d88f7506cd9 320 led_r = not LedOn;
meikefrok 5:3d88f7506cd9 321 led_b = not LedOn;
meikefrok 5:3d88f7506cd9 322
meikefrok 6:23b1ed826b59 323 servo.position(27);
meikefrok 5:3d88f7506cd9 324 pc.printf("Servo position is: right \r\n");
meikefrok 5:3d88f7506cd9 325 break;
meikefrok 5:3d88f7506cd9 326 }
meikefrok 5:3d88f7506cd9 327 }
meikefrok 5:3d88f7506cd9 328
meikefrok 5:3d88f7506cd9 329 }else{}
megrootens 0:048fbd80203e 330 }
meikefrok 3:3a671d01bcb8 331 led_r = not LedOn;
meikefrok 6:23b1ed826b59 332 led_g = not LedOn;
megrootens 0:048fbd80203e 333
meikefrok 6:23b1ed826b59 334 position_claw = servo.read();
meikefrok 6:23b1ed826b59 335
megrootens 0:048fbd80203e 336 break;
megrootens 0:048fbd80203e 337 }
megrootens 0:048fbd80203e 338 }
megrootens 0:048fbd80203e 339 }
megrootens 0:048fbd80203e 340
megrootens 0:048fbd80203e 341
meikefrok 5:3d88f7506cd9 342 // Switch the part
meikefrok 2:ad4b181a6422 343 void SetValue2() {
meikefrok 3:3a671d01bcb8 344 part_id = 2;
meikefrok 6:23b1ed826b59 345 }
meikefrok 2:ad4b181a6422 346
meikefrok 2:ad4b181a6422 347 void SetValue3() {
meikefrok 3:3a671d01bcb8 348 part_id = 3;
meikefrok 6:23b1ed826b59 349 }
meikefrok 2:ad4b181a6422 350
meikefrok 2:ad4b181a6422 351
meikefrok 2:ad4b181a6422 352 void SetValue4() {
meikefrok 3:3a671d01bcb8 353 part_id = 4;
megrootens 0:048fbd80203e 354 }
megrootens 0:048fbd80203e 355
meikefrok 2:ad4b181a6422 356
meikefrok 5:3d88f7506cd9 357 // Main
megrootens 0:048fbd80203e 358 int main()
megrootens 0:048fbd80203e 359 {
meikefrok 3:3a671d01bcb8 360 led_r = not LedOn;
meikefrok 3:3a671d01bcb8 361 led_g = not LedOn;
meikefrok 3:3a671d01bcb8 362 led_b = not LedOn;
megrootens 0:048fbd80203e 363
meikefrok 5:3d88f7506cd9 364 tick_part.attach(&SwitchPart,kTimeToggle);
meikefrok 5:3d88f7506cd9 365
meikefrok 3:3a671d01bcb8 366 btn_cart.fall(&SetValue2);
meikefrok 3:3a671d01bcb8 367 btn_arm.fall(&SetValue3);
meikefrok 3:3a671d01bcb8 368 btn_claw.fall(&SetValue4);
meikefrok 2:ad4b181a6422 369
megrootens 0:048fbd80203e 370 while (true);
megrootens 0:048fbd80203e 371 }
meikefrok 2:ad4b181a6422 372
meikefrok 2:ad4b181a6422 373
meikefrok 2:ad4b181a6422 374