MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.h@37:3dcc95e9321c, 2017-05-27 (annotated)

Committer:
sahilmgandhi
Date:
Sat May 27 03:37:24 2017 +0000
Revision:
37:3dcc95e9321c
Parent:
36:9c4cc9944b69
Child:
38:fe05f93009a2
Need to get centered more before turns (so after moving forward). Also fix when both walls are missing!

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 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
sahilmgandhi 37:3dcc95e9321c 175 const int desiredCount180 = 3400; // change accordingly to the terrain
kyleliangus 36:9c4cc9944b69 176 const int desiredCountR = 1600;
kyleliangus 36:9c4cc9944b69 177 const int desiredCountL = 1590;
sahilmgandhi 29:ec2c5a69acd6 178
sahilmgandhi 29:ec2c5a69acd6 179 const int oneCellCount = 5400;
sahilmgandhi 37:3dcc95e9321c 180 const int oneCellCountMomentum = 4900;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
sahilmgandhi 29:ec2c5a69acd6 181
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
sahilmgandhi 29:ec2c5a69acd6 187 float ir1base = 0.0;
sahilmgandhi 29:ec2c5a69acd6 188 float ir2base = 0.0;
sahilmgandhi 29:ec2c5a69acd6 189
sahilmgandhi 29:ec2c5a69acd6 190 float ir3base = 0.0;
sahilmgandhi 29:ec2c5a69acd6 191
sahilmgandhi 29:ec2c5a69acd6 192 float ir4base = 0.0;
sahilmgandhi 29:ec2c5a69acd6 193
sahilmgandhi 29:ec2c5a69acd6 194 float averageDivUpper = 0.5;
sahilmgandhi 29:ec2c5a69acd6 195
sahilmgandhi 29:ec2c5a69acd6 196 inline void turnLeft()
sahilmgandhi 29:ec2c5a69acd6 197 {
sahilmgandhi 29:ec2c5a69acd6 198 double speed0 = 0.11;
sahilmgandhi 33:68ce1f74ab5f 199 double speed1 = -0.12; // change back to 0.13 if turns stop working, testing something out!
sahilmgandhi 29:ec2c5a69acd6 200
sahilmgandhi 37:3dcc95e9321c 201 double kp = 0.000080;
kyleliangus 36:9c4cc9944b69 202
sahilmgandhi 29:ec2c5a69acd6 203 int counter = 0;
sahilmgandhi 29:ec2c5a69acd6 204 int initial0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 205 int initial1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 206
kyleliangus 36:9c4cc9944b69 207 int desiredCount0 = initial0 - desiredCountL; // left wheel
kyleliangus 36:9c4cc9944b69 208 int desiredCount1 = initial1 + desiredCountL; // right wheel
sahilmgandhi 29:ec2c5a69acd6 209
sahilmgandhi 29:ec2c5a69acd6 210 int count0 = initial0;
sahilmgandhi 29:ec2c5a69acd6 211 int count1 = initial1;
sahilmgandhi 29:ec2c5a69acd6 212
kyleliangus 36:9c4cc9944b69 213 double error0 = desiredCount0 - count0; // is negative
kyleliangus 36:9c4cc9944b69 214 double error1 = desiredCount1 - count1; // is positive
sahilmgandhi 29:ec2c5a69acd6 215
sahilmgandhi 29:ec2c5a69acd6 216 while(1) {
sahilmgandhi 29:ec2c5a69acd6 217
kyleliangus 36:9c4cc9944b69 218 if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
sahilmgandhi 29:ec2c5a69acd6 219 count0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 220 count1 = encoder1.getPulses();
kyleliangus 36:9c4cc9944b69 221
kyleliangus 36:9c4cc9944b69 222 error0 = desiredCount0 - count0; // is negative
kyleliangus 36:9c4cc9944b69 223 error1 = desiredCount1 - count1; // is positive
sahilmgandhi 37:3dcc95e9321c 224
kyleliangus 36:9c4cc9944b69 225 right_motor.move(error1*kp);
kyleliangus 36:9c4cc9944b69 226 left_motor.move(error0*kp);
sahilmgandhi 29:ec2c5a69acd6 227 counter = 0;
sahilmgandhi 29:ec2c5a69acd6 228 } else {
sahilmgandhi 29:ec2c5a69acd6 229 counter++;
sahilmgandhi 29:ec2c5a69acd6 230 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 231 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 232 }
sahilmgandhi 29:ec2c5a69acd6 233 if (counter > 60) {
sahilmgandhi 29:ec2c5a69acd6 234 break;
sahilmgandhi 29:ec2c5a69acd6 235 }
sahilmgandhi 29:ec2c5a69acd6 236 }
sahilmgandhi 29:ec2c5a69acd6 237
sahilmgandhi 29:ec2c5a69acd6 238 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 239 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 240 turnFlag = 0; // zeroing out the flags!
sahilmgandhi 29:ec2c5a69acd6 241 currDir -= 1;
sahilmgandhi 29:ec2c5a69acd6 242 }
sahilmgandhi 29:ec2c5a69acd6 243
sahilmgandhi 29:ec2c5a69acd6 244
sahilmgandhi 29:ec2c5a69acd6 245 inline void turnRight()
sahilmgandhi 29:ec2c5a69acd6 246 {
sahilmgandhi 29:ec2c5a69acd6 247 double speed0 = -0.11;
sahilmgandhi 33:68ce1f74ab5f 248 double speed1 = 0.12; // change back to 0.13 if turns stop working, testing something out!
sahilmgandhi 29:ec2c5a69acd6 249
kyleliangus 36:9c4cc9944b69 250 double kp = 0.00009;
kyleliangus 36:9c4cc9944b69 251
sahilmgandhi 29:ec2c5a69acd6 252 int counter = 0;
sahilmgandhi 29:ec2c5a69acd6 253 int initial0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 254 int initial1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 255
kyleliangus 36:9c4cc9944b69 256 int desiredCount0 = initial0 + desiredCountR; // left wheel
kyleliangus 36:9c4cc9944b69 257 int desiredCount1 = initial1 - desiredCountR; // right wheel
sahilmgandhi 29:ec2c5a69acd6 258
sahilmgandhi 29:ec2c5a69acd6 259 int count0 = initial0;
sahilmgandhi 29:ec2c5a69acd6 260 int count1 = initial1;
sahilmgandhi 29:ec2c5a69acd6 261
kyleliangus 36:9c4cc9944b69 262 double error0 = desiredCount0 - count0; // is positive
kyleliangus 36:9c4cc9944b69 263 double error1 = desiredCount1 - count1; // is negative
sahilmgandhi 29:ec2c5a69acd6 264
sahilmgandhi 29:ec2c5a69acd6 265 while(1) {
sahilmgandhi 29:ec2c5a69acd6 266
kyleliangus 36:9c4cc9944b69 267 if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
sahilmgandhi 29:ec2c5a69acd6 268 count0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 269 count1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 270
kyleliangus 36:9c4cc9944b69 271 error0 = desiredCount0 - count0; // is positive
kyleliangus 36:9c4cc9944b69 272 error1 = desiredCount1 - count1; // is negative
sahilmgandhi 29:ec2c5a69acd6 273
kyleliangus 36:9c4cc9944b69 274 right_motor.move(error1*kp);
kyleliangus 36:9c4cc9944b69 275 left_motor.move(error0*kp);
sahilmgandhi 29:ec2c5a69acd6 276 counter = 0;
sahilmgandhi 29:ec2c5a69acd6 277 } else {
sahilmgandhi 29:ec2c5a69acd6 278 counter++;
sahilmgandhi 29:ec2c5a69acd6 279 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 280 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 281 }
sahilmgandhi 29:ec2c5a69acd6 282 if (counter > 60) {
sahilmgandhi 29:ec2c5a69acd6 283 break;
sahilmgandhi 29:ec2c5a69acd6 284 }
sahilmgandhi 29:ec2c5a69acd6 285 }
sahilmgandhi 29:ec2c5a69acd6 286
sahilmgandhi 29:ec2c5a69acd6 287 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 288 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 289 turnFlag = 0;
sahilmgandhi 29:ec2c5a69acd6 290 currDir += 1;
sahilmgandhi 29:ec2c5a69acd6 291 }
sahilmgandhi 33:68ce1f74ab5f 292
kyleliangus 36:9c4cc9944b69 293 inline void turn180()
sahilmgandhi 29:ec2c5a69acd6 294 {
sahilmgandhi 37:3dcc95e9321c 295 double speed0 = -0.10;
sahilmgandhi 37:3dcc95e9321c 296 double speed1 = 0.11;
sahilmgandhi 37:3dcc95e9321c 297
sahilmgandhi 37:3dcc95e9321c 298 double kp = 0.000055;
sahilmgandhi 29:ec2c5a69acd6 299
sahilmgandhi 29:ec2c5a69acd6 300 int counter = 0;
sahilmgandhi 29:ec2c5a69acd6 301 int initial0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 302 int initial1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 303
sahilmgandhi 29:ec2c5a69acd6 304 int desiredCount0 = initial0 + desiredCount180;
sahilmgandhi 29:ec2c5a69acd6 305 int desiredCount1 = initial1 - desiredCount180;
sahilmgandhi 29:ec2c5a69acd6 306
sahilmgandhi 29:ec2c5a69acd6 307 int count0 = initial0;
sahilmgandhi 29:ec2c5a69acd6 308 int count1 = initial1;
sahilmgandhi 29:ec2c5a69acd6 309
sahilmgandhi 29:ec2c5a69acd6 310 double error0 = count0 - desiredCount0;
sahilmgandhi 29:ec2c5a69acd6 311 double error1 = count1 - desiredCount1;
sahilmgandhi 29:ec2c5a69acd6 312
sahilmgandhi 29:ec2c5a69acd6 313 while(1) {
sahilmgandhi 29:ec2c5a69acd6 314
sahilmgandhi 37:3dcc95e9321c 315 if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
sahilmgandhi 29:ec2c5a69acd6 316 count0 = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 317 count1 = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 318
sahilmgandhi 37:3dcc95e9321c 319 error0 = desiredCount0 - count0;
sahilmgandhi 37:3dcc95e9321c 320 error1 = desiredCount1 - count1;
sahilmgandhi 29:ec2c5a69acd6 321
sahilmgandhi 37:3dcc95e9321c 322 right_motor.move(error1*kp);
sahilmgandhi 37:3dcc95e9321c 323 left_motor.move(error0*kp);
sahilmgandhi 29:ec2c5a69acd6 324 counter = 0;
sahilmgandhi 29:ec2c5a69acd6 325 } else {
sahilmgandhi 29:ec2c5a69acd6 326 counter++;
sahilmgandhi 29:ec2c5a69acd6 327 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 328 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 329 }
sahilmgandhi 29:ec2c5a69acd6 330 if (counter > 60) {
sahilmgandhi 29:ec2c5a69acd6 331 break;
sahilmgandhi 29:ec2c5a69acd6 332 }
sahilmgandhi 29:ec2c5a69acd6 333 }
sahilmgandhi 29:ec2c5a69acd6 334 right_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 335 left_motor.brake();
sahilmgandhi 29:ec2c5a69acd6 336 currDir += 2;
sahilmgandhi 29:ec2c5a69acd6 337 }
sahilmgandhi 29:ec2c5a69acd6 338
kyleliangus 4:b5b7836ca2b0 339 #endif