Mouse code for the MacroRat
main.h
- Committer:
- sahilmgandhi
- Date:
- 2017-05-24
- Revision:
- 29:ec2c5a69acd6
- Parent:
- 26:d20f1adac2d3
- Child:
- 30:11f4316a5ba7
File content as of revision 29:ec2c5a69acd6:
#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 100 // 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_1, PC_5 ); IRPair IRP_3( PB_13, PC_4 ); IRPair IRP_2( PB_0, PA_6 ); IRPair IRP_1( PB_14, PA_7 ); 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 1 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); 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 = 102; // 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 = 2870; const int desiredCountR = 1700; const int desiredCountL = 1700; const int oneCellCount = 5400; const int oneCellCountMomentum = 4550;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! float receiverOneReading = 0.0; float receiverTwoReading = 0.0; float receiverThreeReading = 0.0; float receiverFourReading = 0.0; float ir1base = 0.0; float ir2base = 0.0; float ir3base = 0.0; float ir4base = 0.0; float initAverageL = 8.25; float averageDivL = 27.8; //blue float initAverageR = 8.75; //4.5 float averageDivR = 28.5; //red float averageDivUpper = 0.5; float noWallR = 0.007; float noWallL = 0.007; inline void turnLeft() { double speed0 = 0.11; double speed1 = -0.13; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 - desiredCountL; int desiredCount1 = initial1 + desiredCountL; int count0 = initial0; int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; while(1) { if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } else { counter++; right_motor.brake(); left_motor.brake(); } if (counter > 60) { 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.13; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 + desiredCountR; int desiredCount1 = initial1 - desiredCountR; int count0 = initial0; int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; while(1) { if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } else { counter++; right_motor.brake(); left_motor.brake(); } if (counter > 60) { break; } } right_motor.brake(); left_motor.brake(); turnFlag = 0; currDir += 1; } inline void turnLeft180() { double speed0 = 0.15; double speed1 = -0.15; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 - desiredCountL*2; int desiredCount1 = initial1 + desiredCountL*2; int count0 = initial0; int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; while(1) { if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } else { counter++; right_motor.brake(); left_motor.brake(); } if (counter > 60) { break; } } right_motor.brake(); left_motor.brake(); currDir -= 2; } inline void turnRight180() { double speed0 = -0.16; double speed1 = 0.16; 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) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } else { counter++; right_motor.brake(); left_motor.brake(); } if (counter > 60) { break; } } right_motor.brake(); left_motor.brake(); currDir += 2; } #endif