Cleaner version

Dependencies:   HIDScope MODSERIAL Motordriver QEI Servo mbed

Fork of The_Claw_with_EMG_Control_PID by Meike Froklage

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?

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 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