Mouse code for the MacroRat
main.h@33:68ce1f74ab5f, 2017-05-26 (annotated)
- Committer:
- sahilmgandhi
- Date:
- Fri May 26 06:23:19 2017 +0000
- Revision:
- 33:68ce1f74ab5f
- Parent:
- 32:69acb14778ea
- Child:
- 35:a5bd9ef82210
PID working with the new IRs now ... need to tune it a bit though.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kyleliangus | 4:b5b7836ca2b0 | 1 | #ifndef MAIN_H |
kyleliangus | 4:b5b7836ca2b0 | 2 | #define MAIN_H |
kyleliangus | 4:b5b7836ca2b0 | 3 | |
kyleliangus | 4:b5b7836ca2b0 | 4 | #include "mbed.h" |
kyleliangus | 4:b5b7836ca2b0 | 5 | #include "ITG3200.h" |
kyleliangus | 9:1d8e4da058cd | 6 | #include "motor.h" |
vanshg | 11:8fc2b703086b | 7 | #include "QEI.h" |
sahilmgandhi | 26:d20f1adac2d3 | 8 | #include <stack> // std::stack |
sahilmgandhi | 26:d20f1adac2d3 | 9 | #include <utility> // std::pair, std::make_pair |
sahilmgandhi | 26:d20f1adac2d3 | 10 | |
vanshg | 11:8fc2b703086b | 11 | #define PULSES 3520 |
kyleliangus | 32:69acb14778ea | 12 | #define SAMPLE_NUM 40 |
kyleliangus | 4:b5b7836ca2b0 | 13 | |
kyleliangus | 4:b5b7836ca2b0 | 14 | // Motors |
kyleliangus | 8:a0760acdc59e | 15 | /* |
kyleliangus | 4:b5b7836ca2b0 | 16 | PwmOut left1(PB_7); |
kyleliangus | 4:b5b7836ca2b0 | 17 | PwmOut left2(PB_8); |
kyleliangus | 4:b5b7836ca2b0 | 18 | PwmOut right1(PA_10); |
kyleliangus | 4:b5b7836ca2b0 | 19 | PwmOut right2(PA_11); |
kyleliangus | 4:b5b7836ca2b0 | 20 | |
sahilmgandhi | 7:6f5cb6377bd4 | 21 | DigitalOut enableLeftMotor(PB_4); |
sahilmgandhi | 7:6f5cb6377bd4 | 22 | DigitalOut enableRightMotor(PB_5); |
kyleliangus | 8:a0760acdc59e | 23 | */ |
sahilmgandhi | 7:6f5cb6377bd4 | 24 | |
kyleliangus | 4:b5b7836ca2b0 | 25 | // RGB LED |
kyleliangus | 4:b5b7836ca2b0 | 26 | DigitalOut redLed(PC_0); |
kyleliangus | 4:b5b7836ca2b0 | 27 | DigitalOut blueLed(PC_1); |
kyleliangus | 4:b5b7836ca2b0 | 28 | DigitalOut greenLed(PC_2); |
kyleliangus | 4:b5b7836ca2b0 | 29 | |
kyleliangus | 9:1d8e4da058cd | 30 | // IRPairs |
sahilmgandhi | 30:11f4316a5ba7 | 31 | IRPair IRP_4( PB_13, PC_4 ); // swapped 4 and 3 here so that we do not have to flip it everywhere else |
sahilmgandhi | 30:11f4316a5ba7 | 32 | IRPair IRP_3( PB_1, PC_5); |
sahilmgandhi | 30:11f4316a5ba7 | 33 | IRPair IRP_2( PB_14, PA_7 ); // swapped 2 and 1 here so we do not have to flip it everywhere else! |
sahilmgandhi | 30:11f4316a5ba7 | 34 | IRPair IRP_1( PB_0, PA_6 ); |
kyleliangus | 9:1d8e4da058cd | 35 | |
kyleliangus | 15:b80555a4a8b9 | 36 | Motor left_motor( PB_8, PB_7, PB_4 ); // forward, backwards, enable |
kyleliangus | 15:b80555a4a8b9 | 37 | Motor right_motor( PA_11, PA_10, PB_5 ); // forward, backwards, enable |
kyleliangus | 9:1d8e4da058cd | 38 | |
kyleliangus | 9:1d8e4da058cd | 39 | /* |
kyleliangus | 4:b5b7836ca2b0 | 40 | DigitalOut IR_1(PB_1); |
kyleliangus | 4:b5b7836ca2b0 | 41 | DigitalOut IR_2(PB_13); |
kyleliangus | 4:b5b7836ca2b0 | 42 | DigitalOut IR_3(PB_0); |
kyleliangus | 4:b5b7836ca2b0 | 43 | DigitalOut IR_4(PB_14); |
kyleliangus | 4:b5b7836ca2b0 | 44 | // Receivers |
kyleliangus | 4:b5b7836ca2b0 | 45 | AnalogIn Rec_1(PC_5); |
kyleliangus | 4:b5b7836ca2b0 | 46 | AnalogIn Rec_2(PC_4); |
kyleliangus | 4:b5b7836ca2b0 | 47 | AnalogIn Rec_3(PA_6); |
kyleliangus | 4:b5b7836ca2b0 | 48 | AnalogIn Rec_4(PA_7); |
kyleliangus | 9:1d8e4da058cd | 49 | */ |
kyleliangus | 4:b5b7836ca2b0 | 50 | |
kyleliangus | 4:b5b7836ca2b0 | 51 | // Doing DEBUGGING |
kyleliangus | 4:b5b7836ca2b0 | 52 | #define DEBUGGING 1 |
kyleliangus | 4:b5b7836ca2b0 | 53 | Serial serial(PC_6, PC_7); |
kyleliangus | 4:b5b7836ca2b0 | 54 | |
kyleliangus | 4:b5b7836ca2b0 | 55 | // Gyro |
kyleliangus | 4:b5b7836ca2b0 | 56 | ITG3200 gyro(PC_9, PA_8); |
kyleliangus | 4:b5b7836ca2b0 | 57 | |
kyleliangus | 4:b5b7836ca2b0 | 58 | volatile double reading = 0; |
kyleliangus | 4:b5b7836ca2b0 | 59 | |
kyleliangus | 4:b5b7836ca2b0 | 60 | int gyroX = 0; |
kyleliangus | 4:b5b7836ca2b0 | 61 | int gyroY = 0; |
kyleliangus | 4:b5b7836ca2b0 | 62 | int gyroZ = 0; |
kyleliangus | 4:b5b7836ca2b0 | 63 | |
vanshg | 10:810d1849da9d | 64 | InterruptIn dipButton1(PB_15); |
vanshg | 10:810d1849da9d | 65 | InterruptIn dipButton2(PB_10); |
vanshg | 10:810d1849da9d | 66 | InterruptIn dipButton3(PB_9); |
vanshg | 10:810d1849da9d | 67 | InterruptIn dipButton4(PB_12); |
vanshg | 10:810d1849da9d | 68 | |
vanshg | 10:810d1849da9d | 69 | void enableButton1(); |
vanshg | 10:810d1849da9d | 70 | void enableButton2(); |
vanshg | 10:810d1849da9d | 71 | void enableButton3(); |
vanshg | 10:810d1849da9d | 72 | void enableButton4(); |
vanshg | 10:810d1849da9d | 73 | void disableButton1(); |
vanshg | 10:810d1849da9d | 74 | void disableButton2(); |
vanshg | 10:810d1849da9d | 75 | void disableButton3(); |
vanshg | 10:810d1849da9d | 76 | void disableButton4(); |
vanshg | 10:810d1849da9d | 77 | |
sahilmgandhi | 26:d20f1adac2d3 | 78 | bool isWallInFront(int x, int y); |
sahilmgandhi | 26:d20f1adac2d3 | 79 | bool isWallInBack(int x, int y); |
sahilmgandhi | 26:d20f1adac2d3 | 80 | bool isWallOnRight(int x, int y); |
sahilmgandhi | 26:d20f1adac2d3 | 81 | bool isWallOnLeft(int x, int y); |
sahilmgandhi | 26:d20f1adac2d3 | 82 | |
sahilmgandhi | 26:d20f1adac2d3 | 83 | int chooseNextMovement(); |
sahilmgandhi | 26:d20f1adac2d3 | 84 | void changeManhattanDistance(bool headCenter); |
sahilmgandhi | 26:d20f1adac2d3 | 85 | bool hasVisited(int x, int y); |
sahilmgandhi | 26:d20f1adac2d3 | 86 | |
vanshg | 10:810d1849da9d | 87 | int dipFlags = 0; |
vanshg | 11:8fc2b703086b | 88 | #define BUTTON1_FLAG 0x1 |
vanshg | 11:8fc2b703086b | 89 | #define BUTTON2_FLAG 0x2 |
vanshg | 11:8fc2b703086b | 90 | #define BUTTON3_FLAG 0x4 |
vanshg | 11:8fc2b703086b | 91 | #define BUTTON4_FLAG 0x8 |
vanshg | 11:8fc2b703086b | 92 | |
sahilmgandhi | 17:f713758f6238 | 93 | int turnFlag = 0; |
sahilmgandhi | 17:f713758f6238 | 94 | #define LEFT_FLAG 0x1 |
sahilmgandhi | 17:f713758f6238 | 95 | #define RIGHT_FLAG 0x2 |
sahilmgandhi | 17:f713758f6238 | 96 | |
vanshg | 11:8fc2b703086b | 97 | QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING ); |
vanshg | 11:8fc2b703086b | 98 | QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); |
vanshg | 10:810d1849da9d | 99 | |
sahilmgandhi | 20:82836745332e | 100 | #define F_WALL 0x1 |
sahilmgandhi | 20:82836745332e | 101 | #define L_WALL 0x2 |
sahilmgandhi | 20:82836745332e | 102 | #define R_WALL 0x4 |
sahilmgandhi | 20:82836745332e | 103 | #define B_WALL 0x8 |
sahilmgandhi | 20:82836745332e | 104 | |
sahilmgandhi | 26:d20f1adac2d3 | 105 | #define MAZE_LEN 16 |
sahilmgandhi | 26:d20f1adac2d3 | 106 | |
sahilmgandhi | 26:d20f1adac2d3 | 107 | int mouseX = 0; |
sahilmgandhi | 26:d20f1adac2d3 | 108 | int mouseY = 0; |
sahilmgandhi | 26:d20f1adac2d3 | 109 | bool justTurned = false; |
sahilmgandhi | 26:d20f1adac2d3 | 110 | bool goingToCenter = true; |
sahilmgandhi | 26:d20f1adac2d3 | 111 | |
sahilmgandhi | 26:d20f1adac2d3 | 112 | stack< pair<int, int> > cellsToVisit; |
sahilmgandhi | 26:d20f1adac2d3 | 113 | |
sahilmgandhi | 31:9b71b44e0867 | 114 | int currDir = 100; // modulo this to keep track of the current direction of the mouse! |
sahilmgandhi | 26:d20f1adac2d3 | 115 | // 0 = forward, 1 = right, 2 = down, 3 = left |
sahilmgandhi | 20:82836745332e | 116 | int wallArray[16][16] = {0}; // array to keep track of the walls |
sahilmgandhi | 26:d20f1adac2d3 | 117 | int visitedCells[16][16] = {0}; // array to keep track of the mouse's current location |
sahilmgandhi | 26:d20f1adac2d3 | 118 | int manhattanDistances[16][16] = { |
sahilmgandhi | 20:82836745332e | 119 | {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, |
sahilmgandhi | 20:82836745332e | 120 | {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, |
sahilmgandhi | 20:82836745332e | 121 | {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12}, |
sahilmgandhi | 20:82836745332e | 122 | {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11}, |
sahilmgandhi | 20:82836745332e | 123 | {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10}, |
sahilmgandhi | 20:82836745332e | 124 | {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9}, |
sahilmgandhi | 20:82836745332e | 125 | {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8}, |
sahilmgandhi | 20:82836745332e | 126 | {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7}, |
sahilmgandhi | 20:82836745332e | 127 | {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7}, |
sahilmgandhi | 20:82836745332e | 128 | {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8}, |
sahilmgandhi | 20:82836745332e | 129 | {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9}, |
sahilmgandhi | 20:82836745332e | 130 | {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10}, |
sahilmgandhi | 20:82836745332e | 131 | {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11}, |
sahilmgandhi | 20:82836745332e | 132 | {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12}, |
sahilmgandhi | 20:82836745332e | 133 | {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, |
sahilmgandhi | 20:82836745332e | 134 | {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, |
sahilmgandhi | 20:82836745332e | 135 | }; |
sahilmgandhi | 20:82836745332e | 136 | |
sahilmgandhi | 29:ec2c5a69acd6 | 137 | int distanceToCenter[16][16] = { |
sahilmgandhi | 29:ec2c5a69acd6 | 138 | {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, |
sahilmgandhi | 29:ec2c5a69acd6 | 139 | {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, |
sahilmgandhi | 29:ec2c5a69acd6 | 140 | {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12}, |
sahilmgandhi | 29:ec2c5a69acd6 | 141 | {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11}, |
sahilmgandhi | 29:ec2c5a69acd6 | 142 | {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10}, |
sahilmgandhi | 29:ec2c5a69acd6 | 143 | {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9}, |
sahilmgandhi | 29:ec2c5a69acd6 | 144 | {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8}, |
sahilmgandhi | 29:ec2c5a69acd6 | 145 | {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7}, |
sahilmgandhi | 29:ec2c5a69acd6 | 146 | {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7}, |
sahilmgandhi | 29:ec2c5a69acd6 | 147 | {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8}, |
sahilmgandhi | 29:ec2c5a69acd6 | 148 | {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9}, |
sahilmgandhi | 29:ec2c5a69acd6 | 149 | {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10}, |
sahilmgandhi | 29:ec2c5a69acd6 | 150 | {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11}, |
sahilmgandhi | 29:ec2c5a69acd6 | 151 | {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12}, |
sahilmgandhi | 29:ec2c5a69acd6 | 152 | {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, |
sahilmgandhi | 29:ec2c5a69acd6 | 153 | {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, |
sahilmgandhi | 29:ec2c5a69acd6 | 154 | }; |
sahilmgandhi | 29:ec2c5a69acd6 | 155 | |
sahilmgandhi | 29:ec2c5a69acd6 | 156 | int distanceToStart[16][16] = {0}; |
sahilmgandhi | 29:ec2c5a69acd6 | 157 | |
sahilmgandhi | 29:ec2c5a69acd6 | 158 | |
sahilmgandhi | 29:ec2c5a69acd6 | 159 | |
sahilmgandhi | 29:ec2c5a69acd6 | 160 | /* Constants for when HIGH_PWM_VOLTAGE = 0.2 |
sahilmgandhi | 29:ec2c5a69acd6 | 161 | #define IP_CONSTANT 6 |
sahilmgandhi | 29:ec2c5a69acd6 | 162 | #define II_CONSTANT 0 |
sahilmgandhi | 29:ec2c5a69acd6 | 163 | #define ID_CONSTANT 1 |
sahilmgandhi | 29:ec2c5a69acd6 | 164 | */ |
sahilmgandhi | 29:ec2c5a69acd6 | 165 | |
sahilmgandhi | 29:ec2c5a69acd6 | 166 | // Constants for when HIGH_PWM_VOLTAGE = 0.1 |
sahilmgandhi | 29:ec2c5a69acd6 | 167 | // #define IP_CONSTANT 8.85 |
sahilmgandhi | 29:ec2c5a69acd6 | 168 | // #define II_CONSTANT 0.005 |
sahilmgandhi | 29:ec2c5a69acd6 | 169 | // #define ID_CONSTANT 3.15 |
kyleliangus | 32:69acb14778ea | 170 | //#define IP_CONSTANT 8.2 |
kyleliangus | 32:69acb14778ea | 171 | //#define II_CONSTANT 0.06 |
kyleliangus | 32:69acb14778ea | 172 | //#define ID_CONSTANT 7.55 |
kyleliangus | 32:69acb14778ea | 173 | |
sahilmgandhi | 33:68ce1f74ab5f | 174 | const int desiredCount180 = 3000; // change accordingly to the terrain |
sahilmgandhi | 29:ec2c5a69acd6 | 175 | const int desiredCountR = 1700; |
sahilmgandhi | 29:ec2c5a69acd6 | 176 | const int desiredCountL = 1700; |
sahilmgandhi | 29:ec2c5a69acd6 | 177 | |
sahilmgandhi | 29:ec2c5a69acd6 | 178 | const int oneCellCount = 5400; |
sahilmgandhi | 33:68ce1f74ab5f | 179 | const int oneCellCountMomentum = 4570;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! |
sahilmgandhi | 29:ec2c5a69acd6 | 180 | |
sahilmgandhi | 33:68ce1f74ab5f | 181 | double receiverOneReading = 0.0; |
sahilmgandhi | 33:68ce1f74ab5f | 182 | double receiverTwoReading = 0.0; |
sahilmgandhi | 33:68ce1f74ab5f | 183 | double receiverThreeReading = 0.0; |
sahilmgandhi | 33:68ce1f74ab5f | 184 | double receiverFourReading = 0.0; |
sahilmgandhi | 29:ec2c5a69acd6 | 185 | |
sahilmgandhi | 29:ec2c5a69acd6 | 186 | float ir1base = 0.0; |
sahilmgandhi | 29:ec2c5a69acd6 | 187 | float ir2base = 0.0; |
sahilmgandhi | 29:ec2c5a69acd6 | 188 | |
sahilmgandhi | 29:ec2c5a69acd6 | 189 | float ir3base = 0.0; |
sahilmgandhi | 29:ec2c5a69acd6 | 190 | |
sahilmgandhi | 29:ec2c5a69acd6 | 191 | float ir4base = 0.0; |
sahilmgandhi | 29:ec2c5a69acd6 | 192 | |
sahilmgandhi | 29:ec2c5a69acd6 | 193 | float initAverageL = 8.25; |
sahilmgandhi | 29:ec2c5a69acd6 | 194 | float averageDivL = 27.8; //blue |
sahilmgandhi | 29:ec2c5a69acd6 | 195 | float initAverageR = 8.75; //4.5 |
sahilmgandhi | 29:ec2c5a69acd6 | 196 | float averageDivR = 28.5; //red |
sahilmgandhi | 29:ec2c5a69acd6 | 197 | float averageDivUpper = 0.5; |
sahilmgandhi | 29:ec2c5a69acd6 | 198 | |
sahilmgandhi | 29:ec2c5a69acd6 | 199 | float noWallR = 0.007; |
sahilmgandhi | 29:ec2c5a69acd6 | 200 | float noWallL = 0.007; |
sahilmgandhi | 29:ec2c5a69acd6 | 201 | |
sahilmgandhi | 29:ec2c5a69acd6 | 202 | inline void turnLeft() |
sahilmgandhi | 29:ec2c5a69acd6 | 203 | { |
sahilmgandhi | 29:ec2c5a69acd6 | 204 | double speed0 = 0.11; |
sahilmgandhi | 33:68ce1f74ab5f | 205 | double speed1 = -0.12; // change back to 0.13 if turns stop working, testing something out! |
sahilmgandhi | 29:ec2c5a69acd6 | 206 | |
sahilmgandhi | 29:ec2c5a69acd6 | 207 | int counter = 0; |
sahilmgandhi | 29:ec2c5a69acd6 | 208 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 209 | int initial1 = encoder1.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 210 | |
sahilmgandhi | 29:ec2c5a69acd6 | 211 | int desiredCount0 = initial0 - desiredCountL; |
sahilmgandhi | 29:ec2c5a69acd6 | 212 | int desiredCount1 = initial1 + desiredCountL; |
sahilmgandhi | 29:ec2c5a69acd6 | 213 | |
sahilmgandhi | 29:ec2c5a69acd6 | 214 | int count0 = initial0; |
sahilmgandhi | 29:ec2c5a69acd6 | 215 | int count1 = initial1; |
sahilmgandhi | 29:ec2c5a69acd6 | 216 | |
sahilmgandhi | 29:ec2c5a69acd6 | 217 | double error0 = count0 - desiredCount0; |
sahilmgandhi | 29:ec2c5a69acd6 | 218 | double error1 = count1 - desiredCount1; |
sahilmgandhi | 29:ec2c5a69acd6 | 219 | |
sahilmgandhi | 29:ec2c5a69acd6 | 220 | while(1) { |
sahilmgandhi | 29:ec2c5a69acd6 | 221 | |
sahilmgandhi | 29:ec2c5a69acd6 | 222 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
sahilmgandhi | 29:ec2c5a69acd6 | 223 | count0 = encoder0.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 224 | count1 = encoder1.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 225 | |
sahilmgandhi | 29:ec2c5a69acd6 | 226 | error0 = count0 - desiredCount0; |
sahilmgandhi | 29:ec2c5a69acd6 | 227 | error1 = count1 - desiredCount1; |
sahilmgandhi | 29:ec2c5a69acd6 | 228 | |
sahilmgandhi | 29:ec2c5a69acd6 | 229 | right_motor.move(speed0); |
sahilmgandhi | 29:ec2c5a69acd6 | 230 | left_motor.move(speed1); |
sahilmgandhi | 29:ec2c5a69acd6 | 231 | counter = 0; |
sahilmgandhi | 29:ec2c5a69acd6 | 232 | } else { |
sahilmgandhi | 29:ec2c5a69acd6 | 233 | counter++; |
sahilmgandhi | 29:ec2c5a69acd6 | 234 | right_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 235 | left_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 236 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 237 | |
sahilmgandhi | 29:ec2c5a69acd6 | 238 | if (counter > 60) { |
sahilmgandhi | 29:ec2c5a69acd6 | 239 | break; |
sahilmgandhi | 29:ec2c5a69acd6 | 240 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 241 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 242 | |
sahilmgandhi | 29:ec2c5a69acd6 | 243 | right_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 244 | left_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 245 | turnFlag = 0; // zeroing out the flags! |
sahilmgandhi | 29:ec2c5a69acd6 | 246 | currDir -= 1; |
sahilmgandhi | 29:ec2c5a69acd6 | 247 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 248 | |
sahilmgandhi | 29:ec2c5a69acd6 | 249 | |
sahilmgandhi | 29:ec2c5a69acd6 | 250 | inline void turnRight() |
sahilmgandhi | 29:ec2c5a69acd6 | 251 | { |
sahilmgandhi | 29:ec2c5a69acd6 | 252 | double speed0 = -0.11; |
sahilmgandhi | 33:68ce1f74ab5f | 253 | double speed1 = 0.12; // change back to 0.13 if turns stop working, testing something out! |
sahilmgandhi | 29:ec2c5a69acd6 | 254 | |
sahilmgandhi | 29:ec2c5a69acd6 | 255 | int counter = 0; |
sahilmgandhi | 29:ec2c5a69acd6 | 256 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 257 | int initial1 = encoder1.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 258 | |
sahilmgandhi | 29:ec2c5a69acd6 | 259 | int desiredCount0 = initial0 + desiredCountR; |
sahilmgandhi | 29:ec2c5a69acd6 | 260 | int desiredCount1 = initial1 - desiredCountR; |
sahilmgandhi | 29:ec2c5a69acd6 | 261 | |
sahilmgandhi | 29:ec2c5a69acd6 | 262 | int count0 = initial0; |
sahilmgandhi | 29:ec2c5a69acd6 | 263 | int count1 = initial1; |
sahilmgandhi | 29:ec2c5a69acd6 | 264 | |
sahilmgandhi | 29:ec2c5a69acd6 | 265 | double error0 = count0 - desiredCount0; |
sahilmgandhi | 29:ec2c5a69acd6 | 266 | double error1 = count1 - desiredCount1; |
sahilmgandhi | 29:ec2c5a69acd6 | 267 | |
sahilmgandhi | 29:ec2c5a69acd6 | 268 | while(1) { |
sahilmgandhi | 29:ec2c5a69acd6 | 269 | |
sahilmgandhi | 29:ec2c5a69acd6 | 270 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
sahilmgandhi | 29:ec2c5a69acd6 | 271 | count0 = encoder0.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 272 | count1 = encoder1.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 273 | |
sahilmgandhi | 29:ec2c5a69acd6 | 274 | error0 = count0 - desiredCount0; |
sahilmgandhi | 29:ec2c5a69acd6 | 275 | error1 = count1 - desiredCount1; |
sahilmgandhi | 29:ec2c5a69acd6 | 276 | |
sahilmgandhi | 29:ec2c5a69acd6 | 277 | right_motor.move(speed0); |
sahilmgandhi | 29:ec2c5a69acd6 | 278 | left_motor.move(speed1); |
sahilmgandhi | 29:ec2c5a69acd6 | 279 | counter = 0; |
sahilmgandhi | 29:ec2c5a69acd6 | 280 | } else { |
sahilmgandhi | 29:ec2c5a69acd6 | 281 | counter++; |
sahilmgandhi | 29:ec2c5a69acd6 | 282 | right_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 283 | left_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 284 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 285 | |
sahilmgandhi | 29:ec2c5a69acd6 | 286 | if (counter > 60) { |
sahilmgandhi | 29:ec2c5a69acd6 | 287 | break; |
sahilmgandhi | 29:ec2c5a69acd6 | 288 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 289 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 290 | |
sahilmgandhi | 29:ec2c5a69acd6 | 291 | right_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 292 | left_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 293 | turnFlag = 0; |
sahilmgandhi | 29:ec2c5a69acd6 | 294 | currDir += 1; |
sahilmgandhi | 29:ec2c5a69acd6 | 295 | } |
sahilmgandhi | 33:68ce1f74ab5f | 296 | |
sahilmgandhi | 29:ec2c5a69acd6 | 297 | inline void turnRight180() |
sahilmgandhi | 29:ec2c5a69acd6 | 298 | { |
sahilmgandhi | 33:68ce1f74ab5f | 299 | double speed0 = -0.15; |
sahilmgandhi | 29:ec2c5a69acd6 | 300 | double speed1 = 0.16; |
sahilmgandhi | 29:ec2c5a69acd6 | 301 | |
sahilmgandhi | 29:ec2c5a69acd6 | 302 | int counter = 0; |
sahilmgandhi | 29:ec2c5a69acd6 | 303 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 304 | int initial1 = encoder1.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 305 | |
sahilmgandhi | 29:ec2c5a69acd6 | 306 | int desiredCount0 = initial0 + desiredCount180; |
sahilmgandhi | 29:ec2c5a69acd6 | 307 | int desiredCount1 = initial1 - desiredCount180; |
sahilmgandhi | 29:ec2c5a69acd6 | 308 | |
sahilmgandhi | 29:ec2c5a69acd6 | 309 | int count0 = initial0; |
sahilmgandhi | 29:ec2c5a69acd6 | 310 | int count1 = initial1; |
sahilmgandhi | 29:ec2c5a69acd6 | 311 | |
sahilmgandhi | 29:ec2c5a69acd6 | 312 | double error0 = count0 - desiredCount0; |
sahilmgandhi | 29:ec2c5a69acd6 | 313 | double error1 = count1 - desiredCount1; |
sahilmgandhi | 29:ec2c5a69acd6 | 314 | |
sahilmgandhi | 29:ec2c5a69acd6 | 315 | |
sahilmgandhi | 29:ec2c5a69acd6 | 316 | while(1) { |
sahilmgandhi | 29:ec2c5a69acd6 | 317 | |
sahilmgandhi | 29:ec2c5a69acd6 | 318 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
sahilmgandhi | 29:ec2c5a69acd6 | 319 | count0 = encoder0.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 320 | count1 = encoder1.getPulses(); |
sahilmgandhi | 29:ec2c5a69acd6 | 321 | |
sahilmgandhi | 29:ec2c5a69acd6 | 322 | error0 = count0 - desiredCount0; |
sahilmgandhi | 29:ec2c5a69acd6 | 323 | error1 = count1 - desiredCount1; |
sahilmgandhi | 29:ec2c5a69acd6 | 324 | |
sahilmgandhi | 29:ec2c5a69acd6 | 325 | right_motor.move(speed0); |
sahilmgandhi | 29:ec2c5a69acd6 | 326 | left_motor.move(speed1); |
sahilmgandhi | 29:ec2c5a69acd6 | 327 | counter = 0; |
sahilmgandhi | 29:ec2c5a69acd6 | 328 | } else { |
sahilmgandhi | 29:ec2c5a69acd6 | 329 | counter++; |
sahilmgandhi | 29:ec2c5a69acd6 | 330 | right_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 331 | left_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 332 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 333 | |
sahilmgandhi | 29:ec2c5a69acd6 | 334 | if (counter > 60) { |
sahilmgandhi | 29:ec2c5a69acd6 | 335 | break; |
sahilmgandhi | 29:ec2c5a69acd6 | 336 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 337 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 338 | right_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 339 | left_motor.brake(); |
sahilmgandhi | 29:ec2c5a69acd6 | 340 | currDir += 2; |
sahilmgandhi | 29:ec2c5a69acd6 | 341 | } |
sahilmgandhi | 29:ec2c5a69acd6 | 342 | |
kyleliangus | 4:b5b7836ca2b0 | 343 | #endif |