main.h

00001 #ifndef MAIN_H
00002 #define MAIN_H
00003 
00004 #include "mbed.h"
00005 #include "ITG3200.h"
00006 #include "motor.h"
00007 #include "QEI.h"
00008 #include <stack>          // std::stack
00009 #include <utility>      // std::pair, std::make_pair
00010 
00011 #define PULSES 3520
00012 #define SAMPLE_NUM 40
00013 #define WHEEL_SPEED 0.12
00014 
00015 // Motors
00016 /*
00017 PwmOut left1(PB_7);
00018 PwmOut left2(PB_8);
00019 PwmOut right1(PA_10);
00020 PwmOut right2(PA_11);
00021 
00022 DigitalOut enableLeftMotor(PB_4);
00023 DigitalOut enableRightMotor(PB_5);
00024 */
00025 
00026 // RGB LED
00027 DigitalOut redLed(PC_0);
00028 DigitalOut blueLed(PC_1);
00029 DigitalOut greenLed(PC_2);
00030 
00031 // IRPairs
00032 IRPair IRP_4( PB_13, PC_4 );         // swapped 4 and 3 here so that we do not have to flip it everywhere else
00033 IRPair IRP_3( PB_1, PC_5);
00034 IRPair IRP_2( PB_14, PA_7 );         // swapped 2 and 1 here so we do not have to flip it everywhere else!
00035 IRPair IRP_1( PB_0, PA_6 );
00036 
00037 Motor left_motor( PB_8, PB_7, PB_4 ); // forward, backwards, enable
00038 Motor right_motor( PA_11, PA_10, PB_5 ); // forward, backwards, enable
00039 
00040 /*
00041 DigitalOut IR_1(PB_1);
00042 DigitalOut IR_2(PB_13);
00043 DigitalOut IR_3(PB_0);
00044 DigitalOut IR_4(PB_14);
00045 // Receivers
00046 AnalogIn Rec_1(PC_5);
00047 AnalogIn Rec_2(PC_4);
00048 AnalogIn Rec_3(PA_6);
00049 AnalogIn Rec_4(PA_7);
00050 */
00051 
00052 // Doing DEBUGGING
00053 #define DEBUGGING 0
00054 Serial serial(PC_6, PC_7);
00055 
00056 // Gyro
00057 ITG3200 gyro(PC_9, PA_8);
00058 
00059 volatile double reading = 0;
00060 
00061 int gyroX = 0;
00062 int gyroY = 0;
00063 int gyroZ = 0;
00064 
00065 InterruptIn dipButton1(PB_15);
00066 InterruptIn dipButton2(PB_10);
00067 InterruptIn dipButton3(PB_9);
00068 InterruptIn dipButton4(PB_12);
00069 
00070 void enableButton1();
00071 void enableButton2();
00072 void enableButton3();
00073 void enableButton4();
00074 void disableButton1();
00075 void disableButton2();
00076 void disableButton3();
00077 void disableButton4();
00078 
00079 bool isWallInFront(int x, int y);
00080 bool isWallInBack(int x, int y);
00081 bool isWallOnRight(int x, int y);
00082 bool isWallOnLeft(int x, int y);
00083 
00084 int chooseNextMovement();
00085 void changeManhattanDistance(bool headCenter);
00086 bool hasVisited(int x, int y);
00087 
00088 volatile int dipFlags = 0;
00089 #define BUTTON1_FLAG 0x1
00090 #define BUTTON2_FLAG 0x2
00091 #define BUTTON3_FLAG 0x4
00092 #define BUTTON4_FLAG 0x8
00093 
00094 int turnFlag = 0;
00095 #define LEFT_FLAG 0x1
00096 #define RIGHT_FLAG 0x2
00097 
00098 QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
00099 QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
00100 
00101 #define F_WALL 0x1
00102 #define L_WALL 0x2
00103 #define R_WALL 0x4
00104 #define B_WALL 0x8
00105 
00106 #define MAZE_LEN 16
00107 
00108 int mouseX = 0;
00109 int mouseY = 0;
00110 bool justTurned = false;
00111 bool goingToCenter = true;
00112 
00113 stack< pair<int, int> > cellsToVisit;
00114 
00115 int currDir = 100;              // modulo this to keep track of the current direction of the mouse!
00116 // 0 = forward, 1 = right, 2 = down, 3 = left
00117 int wallArray[16][16] = {0};    // array to keep track of the walls
00118 int visitedCells[16][16] = {0};     // array to keep track of the mouse's current location
00119 int manhattanDistances[16][16] = {
00120                                   {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
00121                                   {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
00122                                   {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
00123                                   {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
00124                                   {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
00125                                   {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
00126                                   {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
00127                                   {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
00128                                   {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
00129                                   {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
00130                                   {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
00131                                   {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
00132                                   {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
00133                                   {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
00134                                   {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
00135                                   {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
00136                           };
00137 
00138 int distanceToCenter[16][16] = {
00139                                   {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
00140                                   {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
00141                                   {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
00142                                   {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
00143                                   {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
00144                                   {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
00145                                   {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
00146                                   {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
00147                                   {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
00148                                   {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
00149                                   {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
00150                                   {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
00151                                   {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
00152                                   {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
00153                                   {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
00154                                   {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
00155                           };
00156 
00157 int distanceToStart[16][16] = {0};
00158                           
00159     
00160                           
00161 /* Constants for when HIGH_PWM_VOLTAGE = 0.2
00162 #define IP_CONSTANT 6
00163 #define II_CONSTANT 0
00164 #define ID_CONSTANT 1
00165 */
00166  
00167 // Constants for when HIGH_PWM_VOLTAGE = 0.1
00168 // #define IP_CONSTANT 8.85
00169 // #define II_CONSTANT 0.005
00170 // #define ID_CONSTANT 3.15
00171 //#define IP_CONSTANT 8.2
00172 //#define II_CONSTANT 0.06
00173 //#define ID_CONSTANT 7.55
00174 
00175 const int desiredCount180 = 3120;   // change accordingly to the terrain
00176 const int desiredCountR = 1340;
00177 const int desiredCountL = 1455;
00178 
00179 const int oneCellCount = 5480;
00180 const int oneCellCountMomentum = 4578;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
00181 
00182 double receiverOneReading = 0.0;
00183 double receiverTwoReading = 0.0;
00184 double receiverThreeReading = 0.0;
00185 double receiverFourReading = 0.0;
00186 
00187 const double frontStop = 8.35;
00188 const double LRAvg = 3.5;
00189 
00190 float ir1base = 0.0;
00191 float ir2base = 0.0;
00192 float ir3base = 0.0;
00193 float ir4base = 0.0;
00194 
00195 float averageDivUpper = 0.5;
00196 
00197 inline void turnLeft()
00198 {
00199     double speed0 = 0.11;
00200     double speed1 = -0.12;    // change back to 0.13 if turns stop working, testing something out!
00201 
00202     //double kp = 0.000082;
00203     double kp = 0.00010;
00204 
00205     int counter = 0;
00206     int initial0 = encoder0.getPulses();
00207     int initial1 = encoder1.getPulses();
00208 
00209     int desiredCount0 = initial0 - desiredCountL; // left wheel
00210     int desiredCount1 = initial1 + desiredCountL; // right wheel
00211 
00212     int count0 = initial0;
00213     int count1 = initial1;
00214 
00215     double error0 = desiredCount0 - count0; // is negative
00216     double error1 = desiredCount1 - count1; // is positive
00217 
00218     while(1) {
00219  
00220         if(!(abs(error0) < 4) && !(abs(error1) < 4)) {
00221             count0 = encoder0.getPulses();
00222             count1 = encoder1.getPulses();
00223 
00224             error0 = desiredCount0 - count0; // is negative
00225             error1 = desiredCount1 - count1; // is positive
00226 
00227             right_motor.move(error1*kp);
00228             left_motor.move(error0*kp);
00229             counter = 0;
00230         } else {
00231             counter++;
00232             count0 = encoder0.getPulses();
00233             count1 = encoder1.getPulses();
00234 
00235             error0 = desiredCount0 - count0; // is negative
00236             error1 = desiredCount1 - count1; // is positive
00237             right_motor.brake();
00238             left_motor.brake();
00239         }
00240         if (counter > 100) {
00241             break;
00242         }
00243     }
00244 
00245     right_motor.brake();
00246     left_motor.brake();
00247     turnFlag = 0;           // zeroing out the flags!
00248     currDir -= 1;
00249 }
00250 
00251 
00252 inline void turnRight()
00253 {
00254     double speed0 = -0.11;
00255     double speed1 = 0.12;     // change back to 0.13 if turns stop working, testing something out!
00256  
00257 //    double kp = 0.00009;
00258     double kp = 0.00015;
00259  
00260     int counter = 0;
00261     int initial0 = encoder0.getPulses();
00262     int initial1 = encoder1.getPulses();
00263  
00264     int desiredCount0 = initial0 + desiredCountR; // left wheel
00265     int desiredCount1 = initial1 - desiredCountR; // right wheel
00266  
00267     int count0 = initial0;
00268     int count1 = initial1;
00269  
00270     double error0 = desiredCount0 - count0; // is positive
00271     double error1 = desiredCount1 - count1; // is negative
00272  
00273     while(1) {
00274  
00275         if(!(abs(error0) < 4) && !(abs(error1) < 4)) {
00276             count0 = encoder0.getPulses();
00277             count1 = encoder1.getPulses();
00278  
00279             error0 = desiredCount0 - count0; // is positive
00280             error1 = desiredCount1 - count1; // is negative
00281  
00282             right_motor.move(error1*kp);
00283             left_motor.move(error0*kp);
00284             counter = 0;
00285         } else {
00286             counter++;
00287             count0 = encoder0.getPulses();
00288             count1 = encoder1.getPulses();
00289  
00290             error0 = desiredCount0 - count0; // is positive
00291             error1 = desiredCount1 - count1; // is negative
00292             right_motor.brake();
00293             left_motor.brake();
00294         }
00295         if (counter > 100) {
00296             break;
00297         }
00298     }
00299  
00300     right_motor.brake();
00301     left_motor.brake();
00302     turnFlag = 0;
00303     currDir += 1;
00304 }
00305 
00306 inline void turn180()
00307 {
00308     double speed0 = -0.10;
00309     double speed1 = 0.11;
00310     
00311 //    double kp = 0.000055;
00312     double kp = 0.00006;
00313  
00314     int counter = 0;
00315     int initial0 = encoder0.getPulses();
00316     int initial1 = encoder1.getPulses();
00317  
00318     int desiredCount0 = initial0 + desiredCount180;
00319     int desiredCount1 = initial1 - desiredCount180;
00320  
00321     int count0 = initial0;
00322     int count1 = initial1;
00323  
00324     double error0 = count0 - desiredCount0;
00325     double error1 = count1 - desiredCount1;
00326  
00327     while(1) {
00328  
00329         if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
00330             count0 = encoder0.getPulses();
00331             count1 = encoder1.getPulses();
00332  
00333             error0 = desiredCount0 - count0;
00334             error1 = desiredCount1 - count1;
00335  
00336             right_motor.move(error1*kp);
00337             left_motor.move(error0*kp);
00338             counter = 0;
00339         } else {
00340             counter++;
00341             
00342             count0 = encoder0.getPulses();
00343             count1 = encoder1.getPulses();
00344  
00345             error0 = desiredCount0 - count0;
00346             error1 = desiredCount1 - count1;
00347             
00348             right_motor.brake();
00349             left_motor.brake();
00350         }
00351         if (counter > 150) {
00352             break;
00353         }
00354     }
00355     right_motor.brake();
00356     left_motor.brake();
00357     currDir += 2;
00358 }
00359 
00360 #endif