MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.h@32:69acb14778ea, 2017-05-26 (annotated)

Committer:
kyleliangus
Date:
Fri May 26 03:46:03 2017 +0000
Revision:
32:69acb14778ea
Parent:
31:9b71b44e0867
Child:
33:68ce1f74ab5f
PID values still wonky. IR PID vs Encoder PID also wonky

Who changed what in which revision?

UserRevisionLine numberNew 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
sahilmgandhi 29:ec2c5a69acd6 173
kyleliangus 32:69acb14778ea 174 #define IP_CONSTANT 0.25
kyleliangus 32:69acb14778ea 175 #define II_CONSTANT 0
kyleliangus 32:69acb14778ea 176 #define ID_CONSTANT 0.00
kyleliangus 32:69acb14778ea 177
sahilmgandhi 29:ec2c5a69acd6 178 const int desiredCount180 = 2870;
sahilmgandhi 29:ec2c5a69acd6 179 const int desiredCountR = 1700;
sahilmgandhi 29:ec2c5a69acd6 180 const int desiredCountL = 1700;
sahilmgandhi 29:ec2c5a69acd6 181
sahilmgandhi 29:ec2c5a69acd6 182 const int oneCellCount = 5400;
sahilmgandhi 29:ec2c5a69acd6 183 const int oneCellCountMomentum = 4550;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
sahilmgandhi 29:ec2c5a69acd6 184
sahilmgandhi 29:ec2c5a69acd6 185 float receiverOneReading = 0.0;
sahilmgandhi 29:ec2c5a69acd6 186 float receiverTwoReading = 0.0;
sahilmgandhi 29:ec2c5a69acd6 187 float receiverThreeReading = 0.0;
sahilmgandhi 29:ec2c5a69acd6 188 float receiverFourReading = 0.0;
sahilmgandhi 29:ec2c5a69acd6 189
sahilmgandhi 29:ec2c5a69acd6 190 float ir1base = 0.0;
sahilmgandhi 29:ec2c5a69acd6 191 float ir2base = 0.0;
sahilmgandhi 29:ec2c5a69acd6 192
sahilmgandhi 29:ec2c5a69acd6 193 float ir3base = 0.0;
sahilmgandhi 29:ec2c5a69acd6 194
sahilmgandhi 29:ec2c5a69acd6 195 float ir4base = 0.0;
sahilmgandhi 29:ec2c5a69acd6 196
sahilmgandhi 29:ec2c5a69acd6 197 float initAverageL = 8.25;
sahilmgandhi 29:ec2c5a69acd6 198 float averageDivL = 27.8; //blue
sahilmgandhi 29:ec2c5a69acd6 199 float initAverageR = 8.75; //4.5
sahilmgandhi 29:ec2c5a69acd6 200 float averageDivR = 28.5; //red
sahilmgandhi 29:ec2c5a69acd6 201 float averageDivUpper = 0.5;
sahilmgandhi 29:ec2c5a69acd6 202
sahilmgandhi 29:ec2c5a69acd6 203 float noWallR = 0.007;
sahilmgandhi 29:ec2c5a69acd6 204 float noWallL = 0.007;
sahilmgandhi 29:ec2c5a69acd6 205
sahilmgandhi 29:ec2c5a69acd6 206 inline void turnLeft()
sahilmgandhi 29:ec2c5a69acd6 207 {
sahilmgandhi 29:ec2c5a69acd6 208 double speed0 = 0.11;
sahilmgandhi 29:ec2c5a69acd6 209 double speed1 = -0.13;
sahilmgandhi 29:ec2c5a69acd6 210
sahilmgandhi 29:ec2c5a69acd6 211 int counter = 0;
sahilmgandhi 29:ec2c5a69acd6 212 int initial0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 213 int initial1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 214
sahilmgandhi 29:ec2c5a69acd6 215 int desiredCount0 = initial0 - desiredCountL;
sahilmgandhi 29:ec2c5a69acd6 216 int desiredCount1 = initial1 + desiredCountL;
sahilmgandhi 29:ec2c5a69acd6 217
sahilmgandhi 29:ec2c5a69acd6 218 int count0 = initial0;
sahilmgandhi 29:ec2c5a69acd6 219 int count1 = initial1;
sahilmgandhi 29:ec2c5a69acd6 220
sahilmgandhi 29:ec2c5a69acd6 221 double error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 222 double error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 223
sahilmgandhi 29:ec2c5a69acd6 224 while(1) {
sahilmgandhi 29:ec2c5a69acd6 225
sahilmgandhi 29:ec2c5a69acd6 226 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
sahilmgandhi 29:ec2c5a69acd6 227 count0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 228 count1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 229
sahilmgandhi 29:ec2c5a69acd6 230 error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 231 error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 232
sahilmgandhi 29:ec2c5a69acd6 233 right_motor.move(speed0);
sahilmgandhi 29:ec2c5a69acd6 234 left_motor.move(speed1);
sahilmgandhi 29:ec2c5a69acd6 235 counter = 0;
sahilmgandhi 29:ec2c5a69acd6 236 } else {
sahilmgandhi 29:ec2c5a69acd6 237 counter++;
sahilmgandhi 29:ec2c5a69acd6 238 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 239 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 240 }
sahilmgandhi 29:ec2c5a69acd6 241
sahilmgandhi 29:ec2c5a69acd6 242 if (counter > 60) {
sahilmgandhi 29:ec2c5a69acd6 243 break;
sahilmgandhi 29:ec2c5a69acd6 244 }
sahilmgandhi 29:ec2c5a69acd6 245 }
sahilmgandhi 29:ec2c5a69acd6 246
sahilmgandhi 29:ec2c5a69acd6 247 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 248 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 249 turnFlag = 0; // zeroing out the flags!
sahilmgandhi 29:ec2c5a69acd6 250 currDir -= 1;
sahilmgandhi 29:ec2c5a69acd6 251 }
sahilmgandhi 29:ec2c5a69acd6 252
sahilmgandhi 29:ec2c5a69acd6 253
sahilmgandhi 29:ec2c5a69acd6 254 inline void turnRight()
sahilmgandhi 29:ec2c5a69acd6 255 {
sahilmgandhi 29:ec2c5a69acd6 256 double speed0 = -0.11;
sahilmgandhi 29:ec2c5a69acd6 257 double speed1 = 0.13;
sahilmgandhi 29:ec2c5a69acd6 258
sahilmgandhi 29:ec2c5a69acd6 259 int counter = 0;
sahilmgandhi 29:ec2c5a69acd6 260 int initial0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 261 int initial1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 262
sahilmgandhi 29:ec2c5a69acd6 263 int desiredCount0 = initial0 + desiredCountR;
sahilmgandhi 29:ec2c5a69acd6 264 int desiredCount1 = initial1 - desiredCountR;
sahilmgandhi 29:ec2c5a69acd6 265
sahilmgandhi 29:ec2c5a69acd6 266 int count0 = initial0;
sahilmgandhi 29:ec2c5a69acd6 267 int count1 = initial1;
sahilmgandhi 29:ec2c5a69acd6 268
sahilmgandhi 29:ec2c5a69acd6 269 double error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 270 double error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 271
sahilmgandhi 29:ec2c5a69acd6 272 while(1) {
sahilmgandhi 29:ec2c5a69acd6 273
sahilmgandhi 29:ec2c5a69acd6 274 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
sahilmgandhi 29:ec2c5a69acd6 275 count0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 276 count1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 277
sahilmgandhi 29:ec2c5a69acd6 278 error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 279 error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 280
sahilmgandhi 29:ec2c5a69acd6 281 right_motor.move(speed0);
sahilmgandhi 29:ec2c5a69acd6 282 left_motor.move(speed1);
sahilmgandhi 29:ec2c5a69acd6 283 counter = 0;
sahilmgandhi 29:ec2c5a69acd6 284 } else {
sahilmgandhi 29:ec2c5a69acd6 285 counter++;
sahilmgandhi 29:ec2c5a69acd6 286 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 287 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 288 }
sahilmgandhi 29:ec2c5a69acd6 289
sahilmgandhi 29:ec2c5a69acd6 290 if (counter > 60) {
sahilmgandhi 29:ec2c5a69acd6 291 break;
sahilmgandhi 29:ec2c5a69acd6 292 }
sahilmgandhi 29:ec2c5a69acd6 293 }
sahilmgandhi 29:ec2c5a69acd6 294
sahilmgandhi 29:ec2c5a69acd6 295 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 296 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 297 turnFlag = 0;
sahilmgandhi 29:ec2c5a69acd6 298 currDir += 1;
sahilmgandhi 29:ec2c5a69acd6 299 }
sahilmgandhi 29:ec2c5a69acd6 300
sahilmgandhi 29:ec2c5a69acd6 301 inline void turnLeft180()
sahilmgandhi 29:ec2c5a69acd6 302 {
sahilmgandhi 29:ec2c5a69acd6 303 double speed0 = 0.15;
sahilmgandhi 29:ec2c5a69acd6 304 double speed1 = -0.15;
sahilmgandhi 29:ec2c5a69acd6 305
sahilmgandhi 29:ec2c5a69acd6 306 int counter = 0;
sahilmgandhi 29:ec2c5a69acd6 307 int initial0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 308 int initial1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 309
sahilmgandhi 29:ec2c5a69acd6 310 int desiredCount0 = initial0 - desiredCountL*2;
sahilmgandhi 29:ec2c5a69acd6 311 int desiredCount1 = initial1 + desiredCountL*2;
sahilmgandhi 29:ec2c5a69acd6 312
sahilmgandhi 29:ec2c5a69acd6 313 int count0 = initial0;
sahilmgandhi 29:ec2c5a69acd6 314 int count1 = initial1;
sahilmgandhi 29:ec2c5a69acd6 315
sahilmgandhi 29:ec2c5a69acd6 316 double error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 317 double error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 318
sahilmgandhi 29:ec2c5a69acd6 319
sahilmgandhi 29:ec2c5a69acd6 320 while(1) {
sahilmgandhi 29:ec2c5a69acd6 321
sahilmgandhi 29:ec2c5a69acd6 322 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
sahilmgandhi 29:ec2c5a69acd6 323 count0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 324 count1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 325
sahilmgandhi 29:ec2c5a69acd6 326 error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 327 error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 328
sahilmgandhi 29:ec2c5a69acd6 329 right_motor.move(speed0);
sahilmgandhi 29:ec2c5a69acd6 330 left_motor.move(speed1);
sahilmgandhi 29:ec2c5a69acd6 331 counter = 0;
sahilmgandhi 29:ec2c5a69acd6 332 } else {
sahilmgandhi 29:ec2c5a69acd6 333 counter++;
sahilmgandhi 29:ec2c5a69acd6 334 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 335 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 336 }
sahilmgandhi 29:ec2c5a69acd6 337
sahilmgandhi 29:ec2c5a69acd6 338 if (counter > 60) {
sahilmgandhi 29:ec2c5a69acd6 339 break;
sahilmgandhi 29:ec2c5a69acd6 340 }
sahilmgandhi 29:ec2c5a69acd6 341 }
sahilmgandhi 29:ec2c5a69acd6 342
sahilmgandhi 29:ec2c5a69acd6 343 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 344 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 345 currDir -= 2;
sahilmgandhi 29:ec2c5a69acd6 346 }
sahilmgandhi 29:ec2c5a69acd6 347
sahilmgandhi 29:ec2c5a69acd6 348 inline void turnRight180()
sahilmgandhi 29:ec2c5a69acd6 349 {
sahilmgandhi 29:ec2c5a69acd6 350 double speed0 = -0.16;
sahilmgandhi 29:ec2c5a69acd6 351 double speed1 = 0.16;
sahilmgandhi 29:ec2c5a69acd6 352
sahilmgandhi 29:ec2c5a69acd6 353 int counter = 0;
sahilmgandhi 29:ec2c5a69acd6 354 int initial0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 355 int initial1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 356
sahilmgandhi 29:ec2c5a69acd6 357 int desiredCount0 = initial0 + desiredCount180;
sahilmgandhi 29:ec2c5a69acd6 358 int desiredCount1 = initial1 - desiredCount180;
sahilmgandhi 29:ec2c5a69acd6 359
sahilmgandhi 29:ec2c5a69acd6 360 int count0 = initial0;
sahilmgandhi 29:ec2c5a69acd6 361 int count1 = initial1;
sahilmgandhi 29:ec2c5a69acd6 362
sahilmgandhi 29:ec2c5a69acd6 363 double error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 364 double error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 365
sahilmgandhi 29:ec2c5a69acd6 366
sahilmgandhi 29:ec2c5a69acd6 367 while(1) {
sahilmgandhi 29:ec2c5a69acd6 368
sahilmgandhi 29:ec2c5a69acd6 369 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
sahilmgandhi 29:ec2c5a69acd6 370 count0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 371 count1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 372
sahilmgandhi 29:ec2c5a69acd6 373 error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 374 error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 375
sahilmgandhi 29:ec2c5a69acd6 376 right_motor.move(speed0);
sahilmgandhi 29:ec2c5a69acd6 377 left_motor.move(speed1);
sahilmgandhi 29:ec2c5a69acd6 378 counter = 0;
sahilmgandhi 29:ec2c5a69acd6 379 } else {
sahilmgandhi 29:ec2c5a69acd6 380 counter++;
sahilmgandhi 29:ec2c5a69acd6 381 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 382 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 383 }
sahilmgandhi 29:ec2c5a69acd6 384
sahilmgandhi 29:ec2c5a69acd6 385 if (counter > 60) {
sahilmgandhi 29:ec2c5a69acd6 386 break;
sahilmgandhi 29:ec2c5a69acd6 387 }
sahilmgandhi 29:ec2c5a69acd6 388 }
sahilmgandhi 29:ec2c5a69acd6 389 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 390 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 391 currDir += 2;
sahilmgandhi 29:ec2c5a69acd6 392 }
sahilmgandhi 29:ec2c5a69acd6 393
kyleliangus 4:b5b7836ca2b0 394 #endif