Peihsun Yeh / Mbed 2 deprecated ECE4180FinalV4

CarlsenBot working

Dependencies:   Servo mbed

main.cpp@0:cdd566529530, 2013-05-01 (annotated)

Committer:
pyeh9
Date:
Wed May 01 16:00:53 2013 +0000
Revision:
0:cdd566529530
CarlsenBot Working commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pyeh9 0:cdd566529530 1 #include "mbed.h"
pyeh9 0:cdd566529530 2 #include "Motor.h"
pyeh9 0:cdd566529530 3 #include "Servo.h"
pyeh9 0:cdd566529530 4 #include "string.h"
pyeh9 0:cdd566529530 5 #include <iostream>
pyeh9 0:cdd566529530 6
pyeh9 0:cdd566529530 7 // Open: 0 max open
pyeh9 0:cdd566529530 8 #define OPEN 0.43
pyeh9 0:cdd566529530 9 // Close 1 max close **Don't set to 1
pyeh9 0:cdd566529530 10 #define CLOSE 0.87
pyeh9 0:cdd566529530 11 // absolute positions for the z-axis
pyeh9 0:cdd566529530 12 #define UP 0.36
pyeh9 0:cdd566529530 13 #define DOWN 0.512
pyeh9 0:cdd566529530 14
pyeh9 0:cdd566529530 15 // Object declarations
pyeh9 0:cdd566529530 16 I2C i2c(p9, p10);
pyeh9 0:cdd566529530 17 Serial pc(USBTX, USBRX);
pyeh9 0:cdd566529530 18 Motor motor_x1(p21, p5, p6, &i2c);
pyeh9 0:cdd566529530 19 Motor motor_x2(p22, p11, p12, &i2c);
pyeh9 0:cdd566529530 20 Motor motor_y(p23, p15, p16, &i2c);
pyeh9 0:cdd566529530 21 Motor motor_z(p24, p13, p14);
pyeh9 0:cdd566529530 22 Servo claw(p25);
pyeh9 0:cdd566529530 23 AnalogIn z_dist(p20);
pyeh9 0:cdd566529530 24 Serial device(p28, p27); // tx, rx
pyeh9 0:cdd566529530 25 Ticker pos_monitor;
pyeh9 0:cdd566529530 26 DigitalOut leds[] = { (LED1), (LED2), (LED3), (LED4) };
pyeh9 0:cdd566529530 27
pyeh9 0:cdd566529530 28 const int XWIDTH = 208;
pyeh9 0:cdd566529530 29 const int YWIDTH = 554;
pyeh9 0:cdd566529530 30 char resetAll = 0x4E; // to reset all devices on one i2c bus, write data 0x4E to address 0x00.
pyeh9 0:cdd566529530 31 float height;
pyeh9 0:cdd566529530 32
pyeh9 0:cdd566529530 33 //function declarations
pyeh9 0:cdd566529530 34 void send_char( char* );
pyeh9 0:cdd566529530 35 void getAllMeasurements(void);
pyeh9 0:cdd566529530 36 void moveX(int pos);
pyeh9 0:cdd566529530 37 void moveY(int pos);
pyeh9 0:cdd566529530 38 void moveZ(float pos);
pyeh9 0:cdd566529530 39 void movePiece(char c1, char r1, char c2, char r2);
pyeh9 0:cdd566529530 40
pyeh9 0:cdd566529530 41 int main()
pyeh9 0:cdd566529530 42 {
pyeh9 0:cdd566529530 43 // initializing routine for all i2c devices
pyeh9 0:cdd566529530 44 // no motor_z.init since it it not a i2c device
pyeh9 0:cdd566529530 45 // since motor_x2's orientation is switched, flip "is_reversed" bit
pyeh9 0:cdd566529530 46 i2c.write(0x00, &resetAll, 1);
pyeh9 0:cdd566529530 47 motor_x1.init(0x20, 1, false);
pyeh9 0:cdd566529530 48 motor_x2.init(0x22, 2, false);
pyeh9 0:cdd566529530 49 motor_y.init(0x24, 3, true);
pyeh9 0:cdd566529530 50 motor_x2.is_reversed = true;
pyeh9 0:cdd566529530 51
pyeh9 0:cdd566529530 52 /* for debugging
pyeh9 0:cdd566529530 53 motor_x1.getTicks();
pyeh9 0:cdd566529530 54 motor_x2.getTicks();
pyeh9 0:cdd566529530 55 motor_y.getTicks();
pyeh9 0:cdd566529530 56 */
pyeh9 0:cdd566529530 57
pyeh9 0:cdd566529530 58 pos_monitor.attach(&getAllMeasurements, 0.003); // interrupt routine to measure current position on all axis
pyeh9 0:cdd566529530 59
pyeh9 0:cdd566529530 60 // get claw ready
pyeh9 0:cdd566529530 61 // note to self: in future revisions, implement a way to tare X and Y
pyeh9 0:cdd566529530 62 moveZ(UP);
pyeh9 0:cdd566529530 63 claw = OPEN;
pyeh9 0:cdd566529530 64
pyeh9 0:cdd566529530 65 //main variables
pyeh9 0:cdd566529530 66 char char_buf[4];
pyeh9 0:cdd566529530 67 char rchar = 'z';
pyeh9 0:cdd566529530 68 char status = '0';
pyeh9 0:cdd566529530 69 int count = 0;
pyeh9 0:cdd566529530 70 int numLeds = sizeof(leds)/sizeof(DigitalOut);
pyeh9 0:cdd566529530 71
pyeh9 0:cdd566529530 72 // wake up device
pyeh9 0:cdd566529530 73 // done by sending EasyVR 'b' character
pyeh9 0:cdd566529530 74 // recommended wake routine: keep sending 'b' until confirmation, via 'o'
pyeh9 0:cdd566529530 75 device.putc('b');
pyeh9 0:cdd566529530 76 while (device.getc()!='o') {
pyeh9 0:cdd566529530 77 device.putc('b');
pyeh9 0:cdd566529530 78 leds[0] = 1;
pyeh9 0:cdd566529530 79 wait(0.2);
pyeh9 0:cdd566529530 80 }
pyeh9 0:cdd566529530 81
pyeh9 0:cdd566529530 82 //reset all leds initially
pyeh9 0:cdd566529530 83 for(int i = 0; i < numLeds; i++) {
pyeh9 0:cdd566529530 84 leds[i] = 0;
pyeh9 0:cdd566529530 85 }
pyeh9 0:cdd566529530 86
pyeh9 0:cdd566529530 87 //wait until ready
pyeh9 0:cdd566529530 88 while (device.readable()!=0) {}
pyeh9 0:cdd566529530 89
pyeh9 0:cdd566529530 90 //movePiece('b', '2', 'b', '4');
pyeh9 0:cdd566529530 91
pyeh9 0:cdd566529530 92 //main loop
pyeh9 0:cdd566529530 93 while (1) {
pyeh9 0:cdd566529530 94 leds[0] = 0;
pyeh9 0:cdd566529530 95 leds[1] = 0;
pyeh9 0:cdd566529530 96 leds[2] = 0;
pyeh9 0:cdd566529530 97 leds[3] = 1; //solid led4 means mbed is in wait for request mode
pyeh9 0:cdd566529530 98 while (pc.readable() == 0) {} //wait for a readable byte
pyeh9 0:cdd566529530 99 status = pc.getc();
pyeh9 0:cdd566529530 100
pyeh9 0:cdd566529530 101 if(status == 'n') {
pyeh9 0:cdd566529530 102 // REQUEST FOR VR COMMAND
pyeh9 0:cdd566529530 103 while ( count < 4 ) {
pyeh9 0:cdd566529530 104 //flash led 4 for user confirmation of new recog
pyeh9 0:cdd566529530 105 leds[3] = !leds[3];
pyeh9 0:cdd566529530 106 //start recog based off either words or numbers
pyeh9 0:cdd566529530 107 if(count == 0 || count == 2) {
pyeh9 0:cdd566529530 108 device.putc('d');
pyeh9 0:cdd566529530 109 wait(0.001);
pyeh9 0:cdd566529530 110 device.putc('C'); //Ben's trained words (C) // FLOHRS (B)
pyeh9 0:cdd566529530 111 wait(0.001);
pyeh9 0:cdd566529530 112 //pc.printf("Say a column letter\n");
pyeh9 0:cdd566529530 113 } else if(count == 1|| count == 3) {
pyeh9 0:cdd566529530 114 device.putc('i');
pyeh9 0:cdd566529530 115 wait(0.001);
pyeh9 0:cdd566529530 116 device.putc('D'); //Wordset #3 - numbers
pyeh9 0:cdd566529530 117 wait(0.001);
pyeh9 0:cdd566529530 118 //pc.printf("Say a row number\n");
pyeh9 0:cdd566529530 119 }
pyeh9 0:cdd566529530 120 //wait for confirmation of new recog
pyeh9 0:cdd566529530 121 while (device.readable()!=0) {}
pyeh9 0:cdd566529530 122 rchar=device.getc();
pyeh9 0:cdd566529530 123
pyeh9 0:cdd566529530 124 // Word recognized
pyeh9 0:cdd566529530 125 // Each move is a sequence of 4 words. Each word recognized makes an LED light up
pyeh9 0:cdd566529530 126 if (rchar=='r') {
pyeh9 0:cdd566529530 127 wait(.001);
pyeh9 0:cdd566529530 128 device.putc(' ');
pyeh9 0:cdd566529530 129 while (device.readable()!=0) {}
pyeh9 0:cdd566529530 130 rchar=device.getc();
pyeh9 0:cdd566529530 131 //pc.printf("Recognized:");
pyeh9 0:cdd566529530 132 //pc.putc(rchar-'A'+'a');
pyeh9 0:cdd566529530 133 char_buf[count] = (rchar-'A'+'a');
pyeh9 0:cdd566529530 134 leds[count] = 1;
pyeh9 0:cdd566529530 135 count++;
pyeh9 0:cdd566529530 136 // error
pyeh9 0:cdd566529530 137 } else if (rchar=='s') {
pyeh9 0:cdd566529530 138 wait(.001);
pyeh9 0:cdd566529530 139 device.putc(' ');
pyeh9 0:cdd566529530 140 while (device.readable()!=0) {}
pyeh9 0:cdd566529530 141 rchar=device.getc();
pyeh9 0:cdd566529530 142 //pc.printf("Recognized:");
pyeh9 0:cdd566529530 143 //pc.putc(rchar-'A'+'0');
pyeh9 0:cdd566529530 144 char_buf[count] = (rchar-'A'+'0');
pyeh9 0:cdd566529530 145 leds[count] = 1;
pyeh9 0:cdd566529530 146 count++;
pyeh9 0:cdd566529530 147 } else if (rchar=='e') {
pyeh9 0:cdd566529530 148 wait(.001);
pyeh9 0:cdd566529530 149 device.putc(' ');
pyeh9 0:cdd566529530 150 while (device.readable()!=0) {}
pyeh9 0:cdd566529530 151 rchar=device.getc();
pyeh9 0:cdd566529530 152 device.putc(' ');
pyeh9 0:cdd566529530 153 while (device.readable()!=0) {}
pyeh9 0:cdd566529530 154 rchar=device.getc();
pyeh9 0:cdd566529530 155 }
pyeh9 0:cdd566529530 156 }
pyeh9 0:cdd566529530 157 //successful input string created
pyeh9 0:cdd566529530 158
pyeh9 0:cdd566529530 159 //reset all leds
pyeh9 0:cdd566529530 160 for(int i = 0; i < numLeds; i++) {
pyeh9 0:cdd566529530 161 leds[i] = 0;
pyeh9 0:cdd566529530 162 }
pyeh9 0:cdd566529530 163 count = 0;
pyeh9 0:cdd566529530 164 //send data to EEBOX
pyeh9 0:cdd566529530 165 send_char(char_buf);
pyeh9 0:cdd566529530 166 } else if( status == 'm') {
pyeh9 0:cdd566529530 167 leds[0] = 0;
pyeh9 0:cdd566529530 168 leds[1] = 1;
pyeh9 0:cdd566529530 169 leds[2] = 0;
pyeh9 0:cdd566529530 170 leds[3] = 1;
pyeh9 0:cdd566529530 171 //status for claw move
pyeh9 0:cdd566529530 172 //wait for reply
pyeh9 0:cdd566529530 173 pc.putc('m');
pyeh9 0:cdd566529530 174 while( pc.readable() == 0);
pyeh9 0:cdd566529530 175 count = 0;
pyeh9 0:cdd566529530 176 char_buf[count] = pc.getc(); //grab char
pyeh9 0:cdd566529530 177 leds[count] = 1;
pyeh9 0:cdd566529530 178 count++;
pyeh9 0:cdd566529530 179 leds[0] = 0;
pyeh9 0:cdd566529530 180 leds[1] = 0;
pyeh9 0:cdd566529530 181 leds[2] = 0;
pyeh9 0:cdd566529530 182 leds[3] = 0;
pyeh9 0:cdd566529530 183 while (count < 4) {
pyeh9 0:cdd566529530 184 leds[count] = 1;
pyeh9 0:cdd566529530 185 pc.putc(' ');
pyeh9 0:cdd566529530 186 while (pc.readable() == 0) {} //wait for response (beginning of string)
pyeh9 0:cdd566529530 187 char_buf[count] = pc.getc();
pyeh9 0:cdd566529530 188 count++;
pyeh9 0:cdd566529530 189 }
pyeh9 0:cdd566529530 190 leds[0] = 0;
pyeh9 0:cdd566529530 191 leds[1] = 0;
pyeh9 0:cdd566529530 192 leds[2] = 0;
pyeh9 0:cdd566529530 193 leds[3] = 0;
pyeh9 0:cdd566529530 194 //Recieved the 4 char buffer for movement
pyeh9 0:cdd566529530 195 movePiece(char_buf[0], char_buf[1], char_buf[2], char_buf[3]);
pyeh9 0:cdd566529530 196 //wait(10);
pyeh9 0:cdd566529530 197 //pc.printf("mbed recieved following string for move:\n");
pyeh9 0:cdd566529530 198 //pc.printf("%c%c%c%c\n\n", char_buf[0], char_buf[1], char_buf[2], char_buf[3]);
pyeh9 0:cdd566529530 199 count = 0;
pyeh9 0:cdd566529530 200 }
pyeh9 0:cdd566529530 201 }
pyeh9 0:cdd566529530 202
pyeh9 0:cdd566529530 203
pyeh9 0:cdd566529530 204 }
pyeh9 0:cdd566529530 205
pyeh9 0:cdd566529530 206 //Function Definitions
pyeh9 0:cdd566529530 207 void send_char( char output[] )
pyeh9 0:cdd566529530 208 {
pyeh9 0:cdd566529530 209 //Successful Request, send confirmation byte
pyeh9 0:cdd566529530 210 char status;
pyeh9 0:cdd566529530 211 do {
pyeh9 0:cdd566529530 212 pc.putc('n');
pyeh9 0:cdd566529530 213 while (pc.readable() == 0) {} //wait for a readable byte
pyeh9 0:cdd566529530 214 status = pc.getc();
pyeh9 0:cdd566529530 215 } while( status != ' ');
pyeh9 0:cdd566529530 216
pyeh9 0:cdd566529530 217 for(int i = 0; i < 4; i++) {
pyeh9 0:cdd566529530 218
pyeh9 0:cdd566529530 219 do {
pyeh9 0:cdd566529530 220 while (pc.readable() == 0) {} //wait for a readable byte
pyeh9 0:cdd566529530 221 status = pc.getc();
pyeh9 0:cdd566529530 222 } while( status != ' ');
pyeh9 0:cdd566529530 223
pyeh9 0:cdd566529530 224 pc.putc(output[i]);
pyeh9 0:cdd566529530 225
pyeh9 0:cdd566529530 226 }
pyeh9 0:cdd566529530 227
pyeh9 0:cdd566529530 228 //terminate communication
pyeh9 0:cdd566529530 229 do {
pyeh9 0:cdd566529530 230 while (pc.readable() == 0) {} //wait for a readable byte
pyeh9 0:cdd566529530 231 status = pc.getc();
pyeh9 0:cdd566529530 232 } while( status != ' ');
pyeh9 0:cdd566529530 233
pyeh9 0:cdd566529530 234 pc.putc('s'); //send termination byte
pyeh9 0:cdd566529530 235
pyeh9 0:cdd566529530 236 }
pyeh9 0:cdd566529530 237 void moveX(int pos) //make pos signed
pyeh9 0:cdd566529530 238 {
pyeh9 0:cdd566529530 239 float diff1, diff2, spd1, spd2, lower = 0.06, upper = 0.8, integral1 = 0.0, integral2 = 0.0;
pyeh9 0:cdd566529530 240 //int K = 1000;
pyeh9 0:cdd566529530 241 int K = 2000, Ki = 0.001;
pyeh9 0:cdd566529530 242 while (!((abs(motor_x1.ticks - pos) <= 9) && (abs(motor_x2.ticks - pos) <= 9))) {
pyeh9 0:cdd566529530 243 diff1 = (pos - motor_x1.ticks);
pyeh9 0:cdd566529530 244 diff2 = (pos - motor_x2.ticks);
pyeh9 0:cdd566529530 245 integral1 += (diff1*0.001);
pyeh9 0:cdd566529530 246 integral2 += (diff2*0.001);
pyeh9 0:cdd566529530 247 spd1 = (diff1/K) + (Ki*integral1);
pyeh9 0:cdd566529530 248 spd2 = (diff2/K) + (Ki*integral2);
pyeh9 0:cdd566529530 249 if (spd1 >= upper) spd1 = upper;
pyeh9 0:cdd566529530 250 else if(spd1 <= lower && spd1 > 0.0) spd1 = lower;
pyeh9 0:cdd566529530 251 else if(spd1 < 0.0 && spd1 >= -lower) spd1 = -lower;
pyeh9 0:cdd566529530 252 else if(spd1 <= -upper) spd1 = -upper;
pyeh9 0:cdd566529530 253 //rationalize a speed for the motor x2
pyeh9 0:cdd566529530 254 if (spd2 >= upper) spd2 = upper;
pyeh9 0:cdd566529530 255 else if(spd2 <= lower && spd2 > 0.0) spd2 = lower;
pyeh9 0:cdd566529530 256 else if(spd2 < 0.0 && spd2 >= -lower) spd2 = -lower;
pyeh9 0:cdd566529530 257 else if(spd2 <= -upper) spd2 = -upper;
pyeh9 0:cdd566529530 258
pyeh9 0:cdd566529530 259 //printf("Motor %d is at position %d, moving with speed %f\n", motor_x1.motorID, motor_x1.ticks, spd1); // debugging
pyeh9 0:cdd566529530 260 //printf("Motor %d is at position %d, moving with speed %f\n", motor_x2.motorID, motor_x2.ticks, spd2); // debugging
pyeh9 0:cdd566529530 261
pyeh9 0:cdd566529530 262 //adjust speed according to difference between two motors
pyeh9 0:cdd566529530 263 if (abs(motor_x1.ticks - motor_x2.ticks) <= 10) {
pyeh9 0:cdd566529530 264 motor_x1.speed(spd1);
pyeh9 0:cdd566529530 265 motor_x2.speed(spd2);
pyeh9 0:cdd566529530 266 wait(0.001);
pyeh9 0:cdd566529530 267 }
pyeh9 0:cdd566529530 268 //if motor x1 is lagging behind motor x2, slow motor x2
pyeh9 0:cdd566529530 269 else if (abs(diff1) > abs(diff2)) {
pyeh9 0:cdd566529530 270 //apply new speed settings to x motors
pyeh9 0:cdd566529530 271 motor_x2.speed(spd2/2);
pyeh9 0:cdd566529530 272 motor_x1.speed(spd1);
pyeh9 0:cdd566529530 273 wait(0.001);
pyeh9 0:cdd566529530 274 }
pyeh9 0:cdd566529530 275 //if motor x2 is lagging behind motor x1, slow motor x1
pyeh9 0:cdd566529530 276 else if (abs(diff2) > abs(diff1)) {
pyeh9 0:cdd566529530 277 //apply new speed settings to x motors
pyeh9 0:cdd566529530 278 motor_x1.speed(spd1/2);
pyeh9 0:cdd566529530 279 motor_x2.speed(spd2);
pyeh9 0:cdd566529530 280 wait(0.001);
pyeh9 0:cdd566529530 281 }
pyeh9 0:cdd566529530 282 }//while
pyeh9 0:cdd566529530 283 motor_x1.speed(0);
pyeh9 0:cdd566529530 284 motor_x2.speed(0);
pyeh9 0:cdd566529530 285 wait(0.5);
pyeh9 0:cdd566529530 286 //adjusting
pyeh9 0:cdd566529530 287 while (!((abs(motor_x1.ticks - pos) <= 6) && (abs(motor_x2.ticks - pos) <= 6))) {
pyeh9 0:cdd566529530 288 diff1 = (pos - motor_x1.ticks);
pyeh9 0:cdd566529530 289 diff2 = (pos - motor_x2.ticks);
pyeh9 0:cdd566529530 290 if(diff1 > 0) spd1 = 0.05;
pyeh9 0:cdd566529530 291 else if(diff1 < 0) spd1 = -0.05;
pyeh9 0:cdd566529530 292 else if(diff1 == 0) spd1 = 0;
pyeh9 0:cdd566529530 293 if(diff2 > 0) spd2 = 0.05;
pyeh9 0:cdd566529530 294 else if(diff2 < 0) spd2 = -0.05;
pyeh9 0:cdd566529530 295 else if(diff2 == 0) spd2 = 0;
pyeh9 0:cdd566529530 296 //printf("Motor %d is at position %d, moving with speed %f\n", motor_x1.motorID, motor_x1.ticks, spd1); // debugging
pyeh9 0:cdd566529530 297 //printf("Motor %d is at position %d, moving with speed %f\n", motor_x2.motorID, motor_x2.ticks, spd2); // debugging
pyeh9 0:cdd566529530 298
pyeh9 0:cdd566529530 299 motor_x1.speed(spd1);
pyeh9 0:cdd566529530 300 motor_x2.speed(spd2);
pyeh9 0:cdd566529530 301 wait(0.005);
pyeh9 0:cdd566529530 302 }
pyeh9 0:cdd566529530 303 motor_x1.speed(0);
pyeh9 0:cdd566529530 304 motor_x2.speed(0);
pyeh9 0:cdd566529530 305 wait(0.5);
pyeh9 0:cdd566529530 306 //printf("Done! Motor %d is at position %d\n", motor_x1.motorID, motor_x1.ticks); // debugging
pyeh9 0:cdd566529530 307 //printf("Done! Motor %d is at position %d\n", motor_x2.motorID, motor_x2.ticks); // debugging
pyeh9 0:cdd566529530 308 }
pyeh9 0:cdd566529530 309
pyeh9 0:cdd566529530 310 void moveZ(float pos) // ~0.31 for top, ~0.46 for bottom
pyeh9 0:cdd566529530 311 {
pyeh9 0:cdd566529530 312 float tempPos = pos;
pyeh9 0:cdd566529530 313 if(pos == DOWN){
pyeh9 0:cdd566529530 314 pos = 0.47;
pyeh9 0:cdd566529530 315 }
pyeh9 0:cdd566529530 316 //float height1, height2;
pyeh9 0:cdd566529530 317 while(!((abs(height - pos) < 0.01))) {
pyeh9 0:cdd566529530 318 //pc.printf("z height: %f\n", height);
pyeh9 0:cdd566529530 319 if(height < pos) {
pyeh9 0:cdd566529530 320 motor_z.speed(0.025);
pyeh9 0:cdd566529530 321 wait(0.02);
pyeh9 0:cdd566529530 322 motor_z.speed(0.015);
pyeh9 0:cdd566529530 323 } else if(height > pos) {
pyeh9 0:cdd566529530 324 motor_z.speed(-0.16);
pyeh9 0:cdd566529530 325 wait(0.02);
pyeh9 0:cdd566529530 326 motor_z.speed(-0.08);
pyeh9 0:cdd566529530 327 }
pyeh9 0:cdd566529530 328 } // ~ .47
pyeh9 0:cdd566529530 329 pos = tempPos;
pyeh9 0:cdd566529530 330 wait(0.5);
pyeh9 0:cdd566529530 331 while(!((abs(height - pos) < 0.01))) { // adjusting loop
pyeh9 0:cdd566529530 332 //pc.printf("z height: %f\n", height);
pyeh9 0:cdd566529530 333 if(height < pos) {
pyeh9 0:cdd566529530 334 motor_z.speed(0.02);
pyeh9 0:cdd566529530 335 wait(0.02);
pyeh9 0:cdd566529530 336 motor_z.speed(0.01);
pyeh9 0:cdd566529530 337 } else if(height > pos) {
pyeh9 0:cdd566529530 338 motor_z.speed(-0.1);
pyeh9 0:cdd566529530 339 wait(0.02);
pyeh9 0:cdd566529530 340 motor_z.speed(-0.05);
pyeh9 0:cdd566529530 341 }
pyeh9 0:cdd566529530 342 }
pyeh9 0:cdd566529530 343 motor_z.speed(0);
pyeh9 0:cdd566529530 344 }
pyeh9 0:cdd566529530 345 void getAllMeasurements()
pyeh9 0:cdd566529530 346 {
pyeh9 0:cdd566529530 347 motor_x1.getTicks();
pyeh9 0:cdd566529530 348 motor_x2.getTicks();
pyeh9 0:cdd566529530 349 motor_y.getTicks();
pyeh9 0:cdd566529530 350 height = z_dist;
pyeh9 0:cdd566529530 351 }
pyeh9 0:cdd566529530 352
pyeh9 0:cdd566529530 353 void moveY(int pos)
pyeh9 0:cdd566529530 354 {
pyeh9 0:cdd566529530 355 motor_y.move(pos);
pyeh9 0:cdd566529530 356 }
pyeh9 0:cdd566529530 357
pyeh9 0:cdd566529530 358 void movePiece(char c1, char r1, char c2, char r2)
pyeh9 0:cdd566529530 359 {
pyeh9 0:cdd566529530 360 // c1, c2 should be within 'a' and 'h'
pyeh9 0:cdd566529530 361 // subtracting 'a' from c1 or c2 get it's "square offset" from initial position
pyeh9 0:cdd566529530 362 // i.e. if c1 = 'b', 'b'-'a'=1 tells it to move the distance of 1 square forward
pyeh9 0:cdd566529530 363 // r1, r2 should be within '1' and '9', or ':'
pyeh9 0:cdd566529530 364 // same logic, but with '1'
pyeh9 0:cdd566529530 365
pyeh9 0:cdd566529530 366 //*** NOTE:
pyeh9 0:cdd566529530 367 //When a move is a capture, [c2 r2] will always be [b :]
pyeh9 0:cdd566529530 368 int goToY = (c1 - 'a')*YWIDTH;
pyeh9 0:cdd566529530 369 int goToX = (r1 - '1')*XWIDTH;
pyeh9 0:cdd566529530 370 moveX(goToX);
pyeh9 0:cdd566529530 371 moveY(goToY);
pyeh9 0:cdd566529530 372
pyeh9 0:cdd566529530 373 // go down to pick up a piece, and go back up
pyeh9 0:cdd566529530 374 moveZ(DOWN);
pyeh9 0:cdd566529530 375 wait(0.7);
pyeh9 0:cdd566529530 376 claw = CLOSE;
pyeh9 0:cdd566529530 377 wait(0.5);
pyeh9 0:cdd566529530 378 moveZ(UP);
pyeh9 0:cdd566529530 379
pyeh9 0:cdd566529530 380 // move to destination
pyeh9 0:cdd566529530 381 goToY = (c2 - 'a')*YWIDTH;
pyeh9 0:cdd566529530 382 goToX = (r2 - '1')*XWIDTH;
pyeh9 0:cdd566529530 383 moveX(goToX);
pyeh9 0:cdd566529530 384 moveY(goToY);
pyeh9 0:cdd566529530 385 moveZ(DOWN);
pyeh9 0:cdd566529530 386 wait(0.7);
pyeh9 0:cdd566529530 387 claw = OPEN;
pyeh9 0:cdd566529530 388 wait(0.5);
pyeh9 0:cdd566529530 389 moveZ(UP);
pyeh9 0:cdd566529530 390 }