Peihsun Yeh
/
ECE4180FinalV4
CarlsenBot working
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?
User | Revision | Line number | New 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 | } |