main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "Servo.h"
00004 #include "string.h"
00005 #include <iostream>
00006 
00007 // Open: 0 max open
00008 #define OPEN 0.43
00009 // Close 1 max close **Don't set to 1
00010 #define CLOSE 0.87
00011 // absolute positions for the z-axis
00012 #define UP 0.36 
00013 #define DOWN 0.512
00014 
00015 // Object declarations
00016 I2C i2c(p9, p10);
00017 Serial pc(USBTX, USBRX);
00018 Motor motor_x1(p21, p5, p6, &i2c);
00019 Motor motor_x2(p22, p11, p12, &i2c);
00020 Motor motor_y(p23, p15, p16, &i2c);
00021 Motor motor_z(p24, p13, p14);
00022 Servo claw(p25);
00023 AnalogIn z_dist(p20);
00024 Serial device(p28, p27);  // tx, rx
00025 Ticker pos_monitor;
00026 DigitalOut leds[] = { (LED1), (LED2), (LED3), (LED4) };
00027 
00028 const int XWIDTH = 208;
00029 const int YWIDTH = 554;
00030 char resetAll = 0x4E; // to reset all devices on one i2c bus, write data 0x4E to address 0x00.
00031 float height;
00032 
00033 //function declarations
00034 void send_char( char* );
00035 void getAllMeasurements(void);
00036 void moveX(int pos);
00037 void moveY(int pos);
00038 void moveZ(float pos);
00039 void movePiece(char c1, char r1, char c2, char r2);
00040 
00041 int main()
00042 {
00043     // initializing routine for all i2c devices
00044     // no motor_z.init since it it not a i2c device
00045     // since motor_x2's orientation is switched, flip "is_reversed" bit
00046     i2c.write(0x00, &resetAll, 1);
00047     motor_x1.init(0x20, 1, false);
00048     motor_x2.init(0x22, 2, false);
00049     motor_y.init(0x24, 3, true);
00050     motor_x2.is_reversed = true;
00051 
00052     /* for debugging
00053     motor_x1.getTicks();
00054     motor_x2.getTicks();
00055     motor_y.getTicks();
00056     */
00057     
00058     pos_monitor.attach(&getAllMeasurements, 0.003); // interrupt routine to measure current position on all axis
00059     
00060     // get claw ready
00061     // note to self: in future revisions, implement a way to tare X and Y
00062     moveZ(UP);
00063     claw = OPEN;
00064     
00065     //main variables
00066     char char_buf[4];
00067     char rchar = 'z';
00068     char status = '0';
00069     int count = 0;
00070     int numLeds = sizeof(leds)/sizeof(DigitalOut);
00071 
00072     // wake up device
00073     // done by sending EasyVR 'b' character
00074     // recommended wake routine: keep sending 'b' until confirmation, via 'o'
00075     device.putc('b');
00076     while (device.getc()!='o') {
00077         device.putc('b');
00078         leds[0] = 1;
00079         wait(0.2);
00080     }
00081 
00082 //reset all leds initially
00083     for(int i = 0; i < numLeds; i++) {
00084         leds[i] = 0;
00085     }
00086 
00087 //wait until ready
00088     while (device.readable()!=0) {}
00089 
00090     //movePiece('b', '2', 'b', '4');
00091 
00092 //main loop
00093     while (1) {        
00094         leds[0] = 0;
00095         leds[1] = 0;
00096         leds[2] = 0;
00097         leds[3] = 1;    //solid led4 means mbed is in wait for request mode
00098         while (pc.readable() == 0) {}   //wait for a readable byte
00099         status = pc.getc();
00100 
00101         if(status == 'n') {
00102             // REQUEST FOR VR COMMAND
00103             while ( count < 4 ) {
00104                 //flash led 4 for user confirmation of new recog
00105                 leds[3] = !leds[3];
00106                 //start recog based off either words or numbers
00107                 if(count == 0 || count == 2) {
00108                     device.putc('d');
00109                     wait(0.001);
00110                     device.putc('C'); //Ben's trained words (C) // FLOHRS (B)
00111                     wait(0.001);
00112                     //pc.printf("Say a column letter\n");
00113                 } else if(count == 1|| count == 3) {
00114                     device.putc('i');
00115                     wait(0.001);
00116                     device.putc('D'); //Wordset #3 - numbers
00117                     wait(0.001);
00118                     //pc.printf("Say a row number\n");
00119                 }
00120                 //wait for confirmation of new recog
00121                 while (device.readable()!=0) {}
00122                 rchar=device.getc();
00123 
00124                 // Word recognized
00125                 // Each move is a sequence of 4 words. Each word recognized makes an LED light up
00126                 if (rchar=='r') {
00127                     wait(.001);
00128                     device.putc(' ');
00129                     while (device.readable()!=0) {}
00130                     rchar=device.getc();
00131                     //pc.printf("Recognized:");
00132                     //pc.putc(rchar-'A'+'a');
00133                     char_buf[count] = (rchar-'A'+'a');
00134                     leds[count] = 1;
00135                     count++;
00136                     // error
00137                 } else if (rchar=='s') {
00138                     wait(.001);
00139                     device.putc(' ');
00140                     while (device.readable()!=0) {}
00141                     rchar=device.getc();
00142                     //pc.printf("Recognized:");
00143                     //pc.putc(rchar-'A'+'0');
00144                     char_buf[count] = (rchar-'A'+'0');
00145                     leds[count] = 1;
00146                     count++;
00147                 } else if (rchar=='e') {
00148                     wait(.001);
00149                     device.putc(' ');
00150                     while (device.readable()!=0) {}
00151                     rchar=device.getc();
00152                     device.putc(' ');
00153                     while (device.readable()!=0) {}
00154                     rchar=device.getc();
00155                 }
00156             }
00157             //successful input string created
00158 
00159             //reset all leds
00160             for(int i = 0; i < numLeds; i++) {
00161                 leds[i] = 0;
00162             }
00163             count = 0;
00164             //send data to EEBOX
00165             send_char(char_buf);
00166         } else if( status == 'm') {
00167             leds[0] = 0;
00168             leds[1] = 1;
00169             leds[2] = 0;
00170             leds[3] = 1;
00171             //status for claw move
00172             //wait for reply
00173             pc.putc('m');
00174             while( pc.readable() == 0);
00175             count = 0;
00176             char_buf[count] = pc.getc(); //grab char
00177             leds[count] = 1;
00178             count++;
00179             leds[0] = 0;
00180             leds[1] = 0;
00181             leds[2] = 0;
00182             leds[3] = 0;
00183             while (count < 4) {
00184                 leds[count] = 1;
00185                 pc.putc(' ');
00186                 while (pc.readable() == 0) {}  //wait for response (beginning of string)
00187                 char_buf[count] = pc.getc();
00188                 count++;
00189             }
00190             leds[0] = 0;
00191             leds[1] = 0;
00192             leds[2] = 0;
00193             leds[3] = 0;
00194             //Recieved the 4 char buffer for movement
00195             movePiece(char_buf[0], char_buf[1], char_buf[2], char_buf[3]);
00196             //wait(10);
00197             //pc.printf("mbed recieved following string for move:\n");
00198             //pc.printf("%c%c%c%c\n\n", char_buf[0], char_buf[1], char_buf[2], char_buf[3]);
00199             count = 0;
00200         }
00201     }
00202 
00203 
00204 }
00205 
00206 //Function Definitions
00207 void send_char( char output[] )
00208 {
00209     //Successful Request, send confirmation byte
00210     char status;
00211     do {
00212         pc.putc('n');
00213         while (pc.readable() == 0) {}       //wait for a readable byte
00214         status = pc.getc();
00215     } while( status != ' ');
00216 
00217     for(int i = 0; i < 4; i++) {
00218 
00219         do {
00220             while (pc.readable() == 0) {}   //wait for a readable byte
00221             status = pc.getc();
00222         } while( status != ' ');
00223 
00224         pc.putc(output[i]);
00225 
00226     }
00227 
00228     //terminate communication
00229     do {
00230         while (pc.readable() == 0) {}   //wait for a readable byte
00231         status = pc.getc();
00232     } while( status != ' ');
00233 
00234     pc.putc('s');                       //send termination byte
00235 
00236 }
00237 void moveX(int pos)   //make pos signed
00238 {
00239     float diff1, diff2, spd1, spd2, lower = 0.06, upper = 0.8, integral1 = 0.0, integral2 = 0.0;
00240     //int K = 1000;
00241     int K = 2000, Ki = 0.001;
00242     while (!((abs(motor_x1.ticks - pos) <= 9) && (abs(motor_x2.ticks - pos) <= 9))) {
00243         diff1 = (pos - motor_x1.ticks);
00244         diff2 = (pos - motor_x2.ticks);
00245         integral1 += (diff1*0.001);
00246         integral2 += (diff2*0.001);
00247         spd1 = (diff1/K) + (Ki*integral1);
00248         spd2 = (diff2/K) + (Ki*integral2);
00249         if (spd1 >= upper) spd1 = upper;
00250         else if(spd1 <= lower && spd1 > 0.0) spd1 = lower;
00251         else if(spd1 < 0.0 && spd1 >= -lower) spd1 = -lower;
00252         else if(spd1 <= -upper) spd1 = -upper;
00253         //rationalize a speed for the motor x2
00254         if (spd2 >= upper) spd2 = upper;
00255         else if(spd2 <= lower && spd2 > 0.0) spd2 = lower;
00256         else if(spd2 < 0.0 && spd2 >= -lower) spd2 = -lower;
00257         else if(spd2 <= -upper) spd2 = -upper;
00258 
00259         //printf("Motor %d is at position %d, moving with speed %f\n", motor_x1.motorID, motor_x1.ticks, spd1); // debugging
00260         //printf("Motor %d is at position %d, moving with speed %f\n", motor_x2.motorID, motor_x2.ticks, spd2); // debugging
00261 
00262         //adjust speed according to difference between two motors
00263         if (abs(motor_x1.ticks - motor_x2.ticks) <= 10) {
00264             motor_x1.speed(spd1);
00265             motor_x2.speed(spd2);
00266             wait(0.001);
00267         }
00268         //if motor x1 is lagging behind motor x2, slow motor x2
00269         else if (abs(diff1) > abs(diff2)) {
00270             //apply new speed settings to x motors
00271             motor_x2.speed(spd2/2);
00272             motor_x1.speed(spd1);
00273             wait(0.001);
00274         }
00275         //if motor x2 is lagging behind motor x1, slow motor x1
00276         else if (abs(diff2) > abs(diff1)) {
00277             //apply new speed settings to x motors
00278             motor_x1.speed(spd1/2);
00279             motor_x2.speed(spd2);
00280             wait(0.001);
00281         }
00282     }//while
00283     motor_x1.speed(0);
00284     motor_x2.speed(0);
00285     wait(0.5);
00286     //adjusting
00287     while (!((abs(motor_x1.ticks - pos) <= 6) && (abs(motor_x2.ticks - pos) <= 6))) {
00288         diff1 = (pos - motor_x1.ticks);
00289         diff2 = (pos - motor_x2.ticks);
00290         if(diff1 > 0) spd1 = 0.05;
00291         else if(diff1 < 0) spd1 = -0.05;
00292         else if(diff1 == 0) spd1 = 0;
00293         if(diff2 > 0) spd2 = 0.05;
00294         else if(diff2 < 0) spd2 = -0.05;
00295         else if(diff2 == 0) spd2 = 0;
00296         //printf("Motor %d is at position %d, moving with speed %f\n", motor_x1.motorID, motor_x1.ticks, spd1); // debugging
00297         //printf("Motor %d is at position %d, moving with speed %f\n", motor_x2.motorID, motor_x2.ticks, spd2); // debugging
00298 
00299         motor_x1.speed(spd1);
00300         motor_x2.speed(spd2);
00301         wait(0.005);
00302     }
00303     motor_x1.speed(0);
00304     motor_x2.speed(0);
00305     wait(0.5);
00306     //printf("Done! Motor %d is at position %d\n", motor_x1.motorID, motor_x1.ticks); // debugging
00307     //printf("Done! Motor %d is at position %d\n", motor_x2.motorID, motor_x2.ticks); // debugging
00308 }
00309 
00310 void moveZ(float pos)  // ~0.31 for top, ~0.46 for bottom
00311 {
00312     float tempPos = pos;
00313     if(pos == DOWN){
00314         pos = 0.47;
00315     }
00316     //float height1, height2;
00317     while(!((abs(height - pos) < 0.01))) {
00318         //pc.printf("z height: %f\n", height);
00319         if(height < pos) {
00320             motor_z.speed(0.025);
00321             wait(0.02);
00322             motor_z.speed(0.015);
00323         } else if(height > pos) {
00324             motor_z.speed(-0.16);
00325             wait(0.02);
00326             motor_z.speed(-0.08);
00327         }
00328     } // ~ .47
00329     pos = tempPos; 
00330     wait(0.5);
00331     while(!((abs(height - pos) < 0.01))) { // adjusting loop
00332         //pc.printf("z height: %f\n", height);
00333         if(height < pos) {
00334             motor_z.speed(0.02);
00335             wait(0.02);
00336             motor_z.speed(0.01);
00337         } else if(height > pos) {
00338             motor_z.speed(-0.1);
00339             wait(0.02);
00340             motor_z.speed(-0.05);
00341         }
00342     }
00343     motor_z.speed(0);
00344 }
00345 void getAllMeasurements()
00346 {
00347     motor_x1.getTicks();
00348     motor_x2.getTicks();
00349     motor_y.getTicks();
00350     height = z_dist;
00351 }
00352 
00353 void moveY(int pos)
00354 {
00355     motor_y.move(pos);
00356 }
00357 
00358 void movePiece(char c1, char r1, char c2, char r2)
00359 {
00360     // c1, c2 should be within 'a' and 'h'
00361     // subtracting 'a' from c1 or c2 get it's "square offset" from initial position
00362     // i.e. if c1 = 'b', 'b'-'a'=1 tells it to move the distance of 1 square forward
00363     // r1, r2 should be within '1' and '9', or ':'
00364     // same logic, but with '1'
00365     
00366     //*** NOTE:
00367     //When a move is a capture, [c2 r2] will always be [b :]
00368     int goToY = (c1 - 'a')*YWIDTH;
00369     int goToX = (r1 - '1')*XWIDTH;
00370     moveX(goToX);
00371     moveY(goToY);
00372     
00373     // go down to pick up a piece, and go back up
00374     moveZ(DOWN);
00375     wait(0.7);
00376     claw = CLOSE;
00377     wait(0.5);
00378     moveZ(UP);
00379 
00380     // move to destination
00381     goToY = (c2 - 'a')*YWIDTH;
00382     goToX = (r2 - '1')*XWIDTH;
00383     moveX(goToX);
00384     moveY(goToY);
00385     moveZ(DOWN);
00386     wait(0.7);
00387     claw = OPEN;
00388     wait(0.5);
00389     moveZ(UP);
00390 }