MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.h@44:85bf2c0cd518, 2017-06-02 (annotated)

Committer:
christine222
Date:
Fri Jun 02 18:50:51 2017 +0000
Revision:
44:85bf2c0cd518
Parent:
43:f22168a05c3e
Child:
46:b156ef445742
rngesus

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
kyleliangus 43:f22168a05c3e 175 const int desiredCount180 = 3120; // change accordingly to the terrain
christine222 44:85bf2c0cd518 176 const int desiredCountR = 1340;
christine222 44:85bf2c0cd518 177 const int desiredCountL = 1455;
vanshg 39:058fb32c24e0 178
kyleliangus 43:f22168a05c3e 179 const int oneCellCount = 5480;
christine222 44:85bf2c0cd518 180 const int oneCellCountMomentum = 4578;//4570 (.15) speed;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
sahilmgandhi 41:56a34315dd75 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
christine222 44:85bf2c0cd518 187 const double frontStop = 8.35;
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;
kyleliangus 43:f22168a05c3e 258 double kp = 0.00015;
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
christine222 44:85bf2c0cd518 275 if(!(abs(error0) < 4) && !(abs(error1) < 4)) {
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