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