main.cpp

00001 #include "mbed.h"
00002 #include "motordriver.h"
00003 #include "Servo.h"
00004 #include "HMC6352.h"
00005 
00006 HMC6352 compass(p9, p10);
00007 //Smartbot
00008 
00009 DigitalOut mbedled1(LED1); //signals draw mode
00010 DigitalOut mbedled2(LED2); //singals relocate mode
00011 DigitalOut mbedled3(LED2); //signals left wheel movement
00012 DigitalOut mbedled4(LED2); //signals right wheel movement
00013 
00014 Serial pc(USBTX, USBRX); // tx, rx
00015 Serial controller(p13, p14); //change to whatever rcv method we use
00016 
00017 //See http://mbed.org/cookbook/Motor
00018 //http://mbed.org/users/4180_1/notebook/sparkfun-magician-robot-base-kit/
00019 //Connections to dual H-brdige driver for the two drive motors
00020 Motor left(p21, p22, p20, 1); // pwm, fwd, rev, has brake feature
00021 Motor right(p24, p25, p27, 1);
00022 
00023 Servo penservo(p23);
00024 
00025 int main()
00026 {
00027     //init command vars
00028     char command = 0; //bit0 = 1 if shake; bit7 = 0 if y, 1 if x; bit6 = 0 if pos, 1 if neg
00029     char XorY = 128; // X or Y move command bitmask
00030     char NorP = 64; // Down or Up move command bitmask
00031     char mag = 62; // bitmask for magnitude of move command
00032     char magnitude = 0; //magnitude of move
00033 
00034     //Direction and Movement Variables, start on page 0 facing +y with dx, dy =0
00035     float nphi, myphi, dphi = 0.0; //angle math, (36)0 = +y, 90 = +x, 180 = -y, 270 = -x
00036     float dy, dx = 0.0; // where on page?
00037     float ny, nx = 0.0; // next move command coords
00038     //int pagesize = 1050; //square page side draw length + border
00039     float moveweight = 0; //length of current move order, effects speed
00040     int comcount = 4; //number of move commands to add into 1 move order
00041    // int flip = 0; //backwards to save time?
00042     int i;
00043     
00044     //Init Servo
00045     penservo = .5;
00046 
00047     //Initialize LEDs
00048     mbedled1 = 1;
00049     mbedled2 = 0;
00050 
00051     //Init IR Receiver
00052     controller.baud(2400);
00053     wait(1);
00054     
00055     //Init Compass to continuous mode, periodic set/reset, 20Hz measurement rate.
00056     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00057     wait(3);
00058     
00059     //Init Compass reading to 0
00060     for (i = 0; i < 5; i++) {  //Take Averaged Compass reading
00061         myphi += compass.sample()/50.0;
00062         wait(.05);
00063     }
00064     dphi = nphi - myphi;               // calc angle diff
00065     while (abs(dphi) > 15) {           // if diff sig, turn to 0
00066         myphi = 0.0;
00067         left.speed(-.5);
00068         for (i = 0; i < 3; i++) {  //Take Averaged Compass reading
00069         myphi += compass.sample()/30.0;
00070         wait(.01);
00071         }
00072         dphi = nphi - myphi;  
00073         wait(.01);             
00074     }         
00075     right.stop(1);                   //stop wheel(s) after turn 
00076     left.stop(1);              
00077     wait(.5);
00078     
00079     //pc.printf("myphi: %f", myphi); 
00080     //pc.printf("starting \n");
00081 
00082     while(1) {
00083 
00084         //if readable, get command
00085         if(controller.readable()) {
00086             mbedled2 = 1;
00087             command = controller.getc(); //Serial read
00088             comcount--;
00089             //pc.printf("%x \n", (command));
00090             mbedled2 = 0;
00091         }   else if (comcount < 4) { //decrement if command countdown started even if no command read
00092             command = 0;
00093             comcount--;
00094         }   else {
00095             command = 0;
00096         }
00097 
00098         //check for shake
00099         if (command==1) { //lift pen up and reposition
00100             mbedled1 = 0;
00101             penservo = 1;
00102             wait(3);
00103             dx = 0; //reset vars after reposition
00104             dy = 0;
00105             comcount = 5;
00106             mbedled1 = 1;
00107             penservo = .5;
00108         }
00109 
00110         //check for move command
00111         if ((abs(command) > 1) && (comcount > 0)) {
00112             magnitude = mag & command;
00113             if ( (command & XorY) == 128 ) {        // if bit7 then X axis
00114                 if ( (command & NorP) == 64 ) {         // if bit6 then -x
00115                    // nx -= (.3+magnitude/16.0)/5.0;
00116                    nx -= magnitude / 4.0;
00117                 } else {                                //else if !bit6 then +x
00118                    // nx += (.3+magnitude/16.0)/5.0;
00119                    nx += magnitude / 4.0;
00120                 }
00121             } else {                                //else if !bit7 then Y axis
00122                 if ( (command & NorP) == 64 ) {         // if bit6 then -y
00123                    // ny -= (.3+magnitude/16.0)/5.0;
00124                    ny -= magnitude / 4.0;
00125                 } else {                                //else if !bit6 then +y
00126                    // ny += (.3+magnitude/16.0)/5.0;
00127                    ny += magnitude / 4.0;
00128                 }
00129             }
00130             wait(.05);
00131         } else if (comcount == 0) { //move math
00132             if ((nx != 0) && (ny != 0)) {    // if both x&y change, calculate vector angle and length in polar coords
00133                 dx += nx;
00134                 dy += ny;
00135                 moveweight = pow(ny, 2) + pow(nx, 2);   //moveweight = r
00136                 moveweight = sqrt(moveweight);
00137                 nphi = atan2(nx, ny);                   //new phi = principal arctan from -180 to 180. x and y flipped to get angle from y to x
00138                 //pc.printf("POLAR myphi: %f, newphi: %f, movelength: %f \n", myphi, nphi, moveweight); 
00139             } else {
00140                 dphi = 0;
00141                 moveweight = 0;
00142             }
00143             
00144             for (i = 0; i < 3; i++) {  //Take Averaged Compass reading
00145                 myphi += compass.sample()/30.0;
00146                 wait(.01);
00147             }
00148             
00149             if (abs(nx) < 2) {                   //if only signif. y axis move
00150                 moveweight = abs(ny);                        // set speed
00151                 if (ny < -2) {nphi = 180;}         // set angles to axis 
00152                 else if (ny > 2) {nphi = 0;}
00153                 else {nphi = myphi;
00154                     moveweight = 0;}
00155             }
00156     
00157             if (abs(ny) < 2) {                   //if only signif. x axis move
00158                 moveweight = abs(nx);
00159                 if (nx < -2) {nphi = 270;}
00160                 else if (nx > 2) {nphi = 90;}
00161                 else {nphi = myphi;
00162                     moveweight = 0;}
00163             }
00164              
00165             dphi = nphi - myphi;               // calc angle diff
00166             //if (abs(dphi) > 340) { dphi = 0;}        // fix pos y axis
00167             /*if ((160 < abs(dphi)) && (abs(dphi)< 200)) {
00168                 moveweight = -moveweight;
00169                 dphi = 0;}       //flip speed instead of flip around
00170             if (dphi > 180) {                 // switch angle diff with mag above 180 and flip
00171                 nphi -= 180;
00172                  moveweight = -moveweight;   //flip speed
00173             }
00174             if (dphi < -180) {
00175                 nphi += 180;
00176                  moveweight = -moveweight;      //flip speed
00177             }
00178             */
00179             
00180             
00181             if (abs(dphi) > 15) { //turn left
00182                 right.stop(1);                   //stop wheels before turn 
00183                 left.stop(1);
00184                 wait(.1);
00185                 while (abs(dphi) > 15) {           // if diff sig, turn to 0
00186                     myphi = 0.0;
00187                     left.speed(-.5);
00188                     for (i = 0; i < 2; i++) { //Take quick Averaged Compass reading
00189                     myphi += compass.sample()/20.0;
00190                     wait(.01);}
00191                     dphi = nphi - myphi;               
00192                 }         
00193                 right.stop(1);                   //stop wheels after turn 
00194                 left.stop(1);              
00195                 wait(.1);
00196             }                         
00197             
00198             while (moveweight >= 1) {   //go fwd for r
00199                 left.speed(.7);  
00200                 right.speed(.7);
00201                 wait(.1);
00202                 moveweight--;
00203                 //moveweight = moveweight/2;
00204             }
00205             
00206             while (moveweight <= -1) {   //go back for r
00207                 left.speed(-.7);  
00208                 right.speed(-.7);
00209                 wait(.1);
00210                 moveweight++;
00211                 //moveweight = moveweight/2;
00212             }
00213     
00214             comcount = 4;            //reset vars
00215             nx = 0;
00216             ny = 0;
00217             moveweight = 0;
00218             dphi = 0;
00219 
00220         } else { //else stop
00221             left.stop(1);
00222             right.stop(1);
00223             wait(.1);
00224         }
00225     }
00226 }