Mouse code for the MacroRat
main.h
- Committer:
- sahilmgandhi
- Date:
- 2017-06-03
- Revision:
- 46:b156ef445742
- Parent:
- 44:85bf2c0cd518
File content as of revision 46:b156ef445742:
#ifndef MAIN_H #define MAIN_H #include "mbed.h" #include "ITG3200.h" #include "motor.h" #include "QEI.h" #include <stack> // std::stack #include <utility> // std::pair, std::make_pair #define PULSES 3520 #define SAMPLE_NUM 40 #define WHEEL_SPEED 0.12 // Motors /* PwmOut left1(PB_7); PwmOut left2(PB_8); PwmOut right1(PA_10); PwmOut right2(PA_11); DigitalOut enableLeftMotor(PB_4); DigitalOut enableRightMotor(PB_5); */ // RGB LED DigitalOut redLed(PC_0); DigitalOut blueLed(PC_1); DigitalOut greenLed(PC_2); // IRPairs IRPair IRP_4( PB_13, PC_4 ); // swapped 4 and 3 here so that we do not have to flip it everywhere else IRPair IRP_3( PB_1, PC_5); IRPair IRP_2( PB_14, PA_7 ); // swapped 2 and 1 here so we do not have to flip it everywhere else! IRPair IRP_1( PB_0, PA_6 ); Motor left_motor( PB_8, PB_7, PB_4 ); // forward, backwards, enable Motor right_motor( PA_11, PA_10, PB_5 ); // forward, backwards, enable /* DigitalOut IR_1(PB_1); DigitalOut IR_2(PB_13); DigitalOut IR_3(PB_0); DigitalOut IR_4(PB_14); // Receivers AnalogIn Rec_1(PC_5); AnalogIn Rec_2(PC_4); AnalogIn Rec_3(PA_6); AnalogIn Rec_4(PA_7); */ // Doing DEBUGGING #define DEBUGGING 0 Serial serial(PC_6, PC_7); // Gyro ITG3200 gyro(PC_9, PA_8); volatile double reading = 0; int gyroX = 0; int gyroY = 0; int gyroZ = 0; InterruptIn dipButton1(PB_15); InterruptIn dipButton2(PB_10); InterruptIn dipButton3(PB_9); InterruptIn dipButton4(PB_12); void enableButton1(); void enableButton2(); void enableButton3(); void enableButton4(); void disableButton1(); void disableButton2(); void disableButton3(); void disableButton4(); bool isWallInFront(int x, int y); bool isWallInBack(int x, int y); bool isWallOnRight(int x, int y); bool isWallOnLeft(int x, int y); int chooseNextMovement(); void changeManhattanDistance(bool headCenter); bool hasVisited(int x, int y); volatile int dipFlags = 0; #define BUTTON1_FLAG 0x1 #define BUTTON2_FLAG 0x2 #define BUTTON3_FLAG 0x4 #define BUTTON4_FLAG 0x8 int turnFlag = 0; #define LEFT_FLAG 0x1 #define RIGHT_FLAG 0x2 QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING ); QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); #define F_WALL 0x1 #define L_WALL 0x2 #define R_WALL 0x4 #define B_WALL 0x8 #define MAZE_LEN 16 int mouseX = 0; int mouseY = 0; bool justTurned = false; bool goingToCenter = true; stack< pair<int, int> > cellsToVisit; int currDir = 100; // modulo this to keep track of the current direction of the mouse! // 0 = forward, 1 = right, 2 = down, 3 = left int wallArray[16][16] = {0}; // array to keep track of the walls int visitedCells[16][16] = {0}; // array to keep track of the mouse's current location int manhattanDistances[16][16] = { {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12}, {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11}, {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10}, {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9}, {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8}, {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7}, {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7}, {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8}, {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9}, {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10}, {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11}, {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12}, {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, }; int distanceToCenter[16][16] = { {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12}, {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11}, {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10}, {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9}, {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8}, {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7}, {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7}, {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8}, {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9}, {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10}, {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11}, {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12}, {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, }; int distanceToStart[16][16] = {0}; /* Constants for when HIGH_PWM_VOLTAGE = 0.2 #define IP_CONSTANT 6 #define II_CONSTANT 0 #define ID_CONSTANT 1 */ // Constants for when HIGH_PWM_VOLTAGE = 0.1 // #define IP_CONSTANT 8.85 // #define II_CONSTANT 0.005 // #define ID_CONSTANT 3.15 //#define IP_CONSTANT 8.2 //#define II_CONSTANT 0.06 //#define ID_CONSTANT 7.55 const int desiredCount180 = 3120; // change accordingly to the terrain const int desiredCountR = 1340; const int desiredCountL = 1455; const int oneCellCount = 5480; const int oneCellCountMomentum = 4578;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! double receiverOneReading = 0.0; double receiverTwoReading = 0.0; double receiverThreeReading = 0.0; double receiverFourReading = 0.0; const double frontStop = 8.35; const double LRAvg = 3.5; float ir1base = 0.0; float ir2base = 0.0; float ir3base = 0.0; float ir4base = 0.0; float averageDivUpper = 0.5; inline void turnLeft() { double speed0 = 0.11; double speed1 = -0.12; // change back to 0.13 if turns stop working, testing something out! //double kp = 0.000082; double kp = 0.00010; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 - desiredCountL; // left wheel int desiredCount1 = initial1 + desiredCountL; // right wheel int count0 = initial0; int count1 = initial1; double error0 = desiredCount0 - count0; // is negative double error1 = desiredCount1 - count1; // is positive while(1) { if(!(abs(error0) < 4) && !(abs(error1) < 4)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = desiredCount0 - count0; // is negative error1 = desiredCount1 - count1; // is positive right_motor.move(error1*kp); left_motor.move(error0*kp); counter = 0; } else { counter++; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = desiredCount0 - count0; // is negative error1 = desiredCount1 - count1; // is positive right_motor.brake(); left_motor.brake(); } if (counter > 100) { break; } } right_motor.brake(); left_motor.brake(); turnFlag = 0; // zeroing out the flags! currDir -= 1; } inline void turnRight() { double speed0 = -0.11; double speed1 = 0.12; // change back to 0.13 if turns stop working, testing something out! // double kp = 0.00009; double kp = 0.00015; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 + desiredCountR; // left wheel int desiredCount1 = initial1 - desiredCountR; // right wheel int count0 = initial0; int count1 = initial1; double error0 = desiredCount0 - count0; // is positive double error1 = desiredCount1 - count1; // is negative while(1) { if(!(abs(error0) < 4) && !(abs(error1) < 4)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = desiredCount0 - count0; // is positive error1 = desiredCount1 - count1; // is negative right_motor.move(error1*kp); left_motor.move(error0*kp); counter = 0; } else { counter++; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = desiredCount0 - count0; // is positive error1 = desiredCount1 - count1; // is negative right_motor.brake(); left_motor.brake(); } if (counter > 100) { break; } } right_motor.brake(); left_motor.brake(); turnFlag = 0; currDir += 1; } inline void turn180() { double speed0 = -0.10; double speed1 = 0.11; // double kp = 0.000055; double kp = 0.00006; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 + desiredCount180; int desiredCount1 = initial1 - desiredCount180; int count0 = initial0; int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; while(1) { if(!(abs(error0) < 3) && !(abs(error1) < 3)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = desiredCount0 - count0; error1 = desiredCount1 - count1; right_motor.move(error1*kp); left_motor.move(error0*kp); counter = 0; } else { counter++; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = desiredCount0 - count0; error1 = desiredCount1 - count1; right_motor.brake(); left_motor.brake(); } if (counter > 150) { break; } } right_motor.brake(); left_motor.brake(); currDir += 2; } #endif