MacroRat
Mouse code for the MacroRat
Revision 29:ec2c5a69acd6, committed 2017-05-24
- Comitter:
- sahilmgandhi
- Date:
- Wed May 24 01:57:01 2017 +0000
- Parent:
- 28:8126a4d620e8
- Child:
- 30:11f4316a5ba7
- Commit message:
- Need to change ir2-ir3 to now be ir1 - ir4
Changed in this revision
--- a/irpair.cpp Sun May 21 13:04:21 2017 +0000
+++ b/irpair.cpp Wed May 24 01:57:01 2017 +0000
@@ -1,9 +1,12 @@
#include "irpair.h"
#include "mbed.h"
+Ticker toggleIr;
+
void IRPair::calibrateSensor() {
ir.write( 1 );
+ wait_us(70);
for (int i = 0; i < samplesToTake; ++i)
sensorAvg += recv.read();
@@ -16,6 +19,8 @@
{
float z = 0;
ir.write( 1 );
+ wait_us(70);
+
for( int i = 0; i < samples; ++i )
z += recv.read();
ir.write( 0 );
--- a/main.cpp Sun May 21 13:04:21 2017 +0000
+++ b/main.cpp Wed May 24 01:57:01 2017 +0000
@@ -12,44 +12,7 @@
#include "stm32f4xx.h"
#include "QEI.h"
-/* Constants for when HIGH_PWM_VOLTAGE = 0.2
-#define IP_CONSTANT 6
-#define II_CONSTANT 0
-#define ID_CONSTANT 1
-*/
-
-// Constants for when HIGH_PWM_VOLTAGE = 0.1
-// #define IP_CONSTANT 8.85
-// #define II_CONSTANT 0.005
-// #define ID_CONSTANT 3.15
-#define IP_CONSTANT 8.15
-#define II_CONSTANT 0.06
-#define ID_CONSTANT 7.5
-
-const int desiredCount180 = 2700;
-const int desiredCountR = 1575;
-const int desiredCountL = 1650;
-
-const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4450;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
-
-float receiverOneReading = 0.0;
-float receiverTwoReading = 0.0;
-float receiverThreeReading = 0.0;
-float receiverFourReading = 0.0;
-float ir1base = 0.0;
-float ir2base = 0.0;
-
-float ir3base = 0.0;
-
-float ir4base = 0.0;
-
-float initAverageL = 8.25;
-float averageDivL = 27.8; //blue
-float initAverageR = 8.75; //4.5
-float averageDivR = 28.5; //red
-float averageDivUpper = 0.5;
//IRSAvg= >: 0.143701, 0.128285
@@ -70,200 +33,9 @@
//0.003795,
//0.005297, 0.007064
-
-float noWallR = 0.007;
-float noWallL = 0.007;
-
+
void pidOnEncoders();
-
-
-void turnLeft()
-{
- double speed0 = 0.11;
- double speed1 = -0.13;
-
- int counter = 0;
- int initial0 = encoder0.getPulses();
- int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 - desiredCountL;
- int desiredCount1 = initial1 + desiredCountL;
-
- int count0 = initial0;
- int count1 = initial1;
-
- double error0 = count0 - desiredCount0;
- double error1 = count1 - desiredCount1;
-
- while(1) {
-
- if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
-
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
- counter = 0;
- } else {
- counter++;
- right_motor.brake();
- left_motor.brake();
- }
-
- if (counter > 60) {
- break;
- }
- }
-
- right_motor.brake();
- left_motor.brake();
- turnFlag = 0; // zeroing out the flags!
- currDir -= 1;
-}
-
-void turnRight()
-{
- double speed0 = -0.11;
- double speed1 = 0.13;
-
- int counter = 0;
- int initial0 = encoder0.getPulses();
- int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 + desiredCountR;
- int desiredCount1 = initial1 - desiredCountR;
-
- int count0 = initial0;
- int count1 = initial1;
-
- double error0 = count0 - desiredCount0;
- double error1 = count1 - desiredCount1;
-
- while(1) {
-
- if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
-
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
- counter = 0;
- } else {
- counter++;
- right_motor.brake();
- left_motor.brake();
- }
-
- if (counter > 60) {
- break;
- }
- }
-
- right_motor.brake();
- left_motor.brake();
- turnFlag = 0;
- currDir += 1;
-}
-
-void turnLeft180()
-{
- double speed0 = 0.15;
- double speed1 = -0.15;
-
- int counter = 0;
- int initial0 = encoder0.getPulses();
- int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 - desiredCountL*2;
- int desiredCount1 = initial1 + desiredCountL*2;
-
- int count0 = initial0;
- int count1 = initial1;
-
- double error0 = count0 - desiredCount0;
- double error1 = count1 - desiredCount1;
-
-
- while(1) {
-
- if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
-
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
- counter = 0;
- } else {
- counter++;
- right_motor.brake();
- left_motor.brake();
- }
-
- if (counter > 60) {
- break;
- }
- }
-
- right_motor.brake();
- left_motor.brake();
- currDir -= 2;
-}
-
-void turnRight180()
-{
- double speed0 = -0.16;
- double speed1 = 0.16;
-
- int counter = 0;
- int initial0 = encoder0.getPulses();
- int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 + desiredCount180;
- int desiredCount1 = initial1 - desiredCount180;
-
- int count0 = initial0;
- int count1 = initial1;
-
- double error0 = count0 - desiredCount0;
- double error1 = count1 - desiredCount1;
-
-
- while(1) {
-
- if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
-
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
- counter = 0;
- } else {
- counter++;
- right_motor.brake();
- left_motor.brake();
- }
-
- if (counter > 60) {
- break;
- }
- }
- right_motor.brake();
- left_motor.brake();
- currDir += 2;
-}
-
+
void moveForwardCellEncoder(double cellNum){
int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
@@ -583,15 +355,17 @@
double derivError = 0;
double sumError = 0;
- double HIGH_PWM_VOLTAGE = 0.12;
- double rightSpeed = 0.12;
- double leftSpeed = 0.12;
+ double HIGH_PWM_VOLTAGE_R = 0.15;
+ double HIGH_PWM_VOLTAGE_L = 0.16;
+
+ double rightSpeed = 0.15;
+ double leftSpeed = 0.16;
int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
- left_motor.forward(0.12);
- right_motor.forward(0.10);
+ left_motor.forward(0.16);
+ right_motor.forward(0.15);
float ir2 = IRP_2.getSamples( SAMPLE_NUM );
float ir3 = IRP_3.getSamples( SAMPLE_NUM );
@@ -602,10 +376,10 @@
int state = 0;
while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
- receiverTwoReading = IRP_2.getSamples(50);
- receiverThreeReading = IRP_3.getSamples(50);
- receiverOneReading = IRP_1.getSamples(50);
- receiverFourReading = IRP_4.getSamples(50);
+ receiverTwoReading = IRP_2.getSamples(100);
+ receiverThreeReading = IRP_3.getSamples(100);
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverFourReading = IRP_4.getSamples(100);
if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){
if (currDir % 4 == 0){
@@ -715,11 +489,11 @@
derivError = currentError - previousError;
double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
- rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
- leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
+ rightSpeed = HIGH_PWM_VOLTAGE_R - abs(PIDSum*HIGH_PWM_VOLTAGE_R);
+ leftSpeed = HIGH_PWM_VOLTAGE_L + abs(PIDSum*HIGH_PWM_VOLTAGE_L);
} else { // r is faster than L. speed up l, slow down r
- rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
- leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
+ rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
+ leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
}
if (leftSpeed > 0.30) leftSpeed = 0.30;
if (leftSpeed < 0) leftSpeed = 0;
@@ -746,6 +520,7 @@
mouseX -= 1;
}
}
+
left_motor.brake();
right_motor.brake();
@@ -987,55 +762,59 @@
void changeManhattanDistance(bool headCenter){
if (headCenter){
- for (int i = 0; i < MAZE_LEN / 2; i++) {
- for (int j = 0; j < MAZE_LEN / 2; j++) {
- manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(7 - i);
+ for (int i = 0; i < MAZE_LEN; i++) {
+ for (int j = 0; j < MAZE_LEN; j++) {
+ distanceToCenter[i][j] = manhattanDistances[i][j];
}
}
- for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
- for (int j = 0; j < MAZE_LEN / 2; j++) {
- manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(8 - i);
- }
- }
- for (int i = 0; i < MAZE_LEN / 2; i++) {
- for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
- manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(7 - i);
- }
- }
- for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
- for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
- manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(8 - i);
+ for (int i = 0; i < MAZE_LEN; i++) {
+ for (int j = 0; j < MAZE_LEN; j++) {
+ manhattanDistances[i][j] = distanceToStart[i][j];
}
}
}
else {
- for (int i = 0; i < MAZE_LEN / 2; i++) {
+ for (int i = 0; i < MAZE_LEN; i++) {
+ for (int j = 0; j < MAZE_LEN; j++) {
+ distanceToStart[i][j] = manhattanDistances[i][j];
+ }
+ }
+ for (int i = 0; i < MAZE_LEN; i++) {
+ for (int j = 0; j < MAZE_LEN; j++) {
+ manhattanDistances[i][j] = distanceToCenter[i][j];
+ }
+ }
+ }
+}
+
+void initializeDistances(){
+ for (int i = 0; i < MAZE_LEN / 2; i++) {
for (int j = 0; j < MAZE_LEN / 2; j++) {
- manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
+ distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
}
}
for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
for (int j = 0; j < MAZE_LEN / 2; j++) {
- manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
+ distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
}
}
for (int i = 0; i < MAZE_LEN / 2; i++) {
for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
- manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
+ distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
}
}
for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
- manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
+ distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
}
}
- }
}
-
+
int main()
{
//Set highest bandwidth.
//gyro.setLpBandwidth(LPFBW_42HZ);
+ initializeDistances();
serial.baud(9600);
//serial.printf("The gyro's address is %s", gyro.getWhoAmI());
@@ -1119,49 +898,49 @@
//wait_ms(1500);
int nextMovement = 0;
while (1) {
- prevEncoder0Count = encoder0.getPulses();
- prevEncoder1Count = encoder1.getPulses();
-
- if (!overrideFloodFill){
- nextMovement = chooseNextMovement();
- if (nextMovement == 0){
- nCellEncoderAndIR(1);
- }
- else if (nextMovement == 1){
- justTurned = true;
- turnRight();
- }
- else if (nextMovement == 2){
- justTurned = true;
- turnLeft();
- }
- else if (nextMovement == 3){
- nCellEncoderAndIR(1);
- }
- else if (nextMovement == 4){
- justTurned = true;
- turnRight180();
- }
- }
- else{
- overrideFloodFill = false;
- if ((rand()%2 + 1) == 1){
- justTurned = true;
- turnLeft();
- }
- else{
- justTurned = true;
- turnRight();
- }
- }
- currEncoder0Count = encoder0.getPulses();
- currEncoder1Count = encoder1.getPulses();
-
- if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
- overrideFloodFill = true;
- }
-
- wait_ms(1000); // reduce this once we fine tune this!
+ // prevEncoder0Count = encoder0.getPulses();
+// prevEncoder1Count = encoder1.getPulses();
+//
+// if (!overrideFloodFill){
+// nextMovement = chooseNextMovement();
+// if (nextMovement == 0){
+// nCellEncoderAndIR(1);
+// }
+// else if (nextMovement == 1){
+// justTurned = true;
+// turnRight();
+// }
+// else if (nextMovement == 2){
+// justTurned = true;
+// turnLeft();
+// }
+// else if (nextMovement == 3){
+// nCellEncoderAndIR(1);
+// }
+// else if (nextMovement == 4){
+// justTurned = true;
+// turnRight180();
+// }
+// }
+// else{
+// overrideFloodFill = false;
+// if ((rand()%2 + 1) == 1){
+// justTurned = true;
+// turnLeft();
+// }
+// else{
+// justTurned = true;
+// turnRight();
+// }
+// }
+// currEncoder0Count = encoder0.getPulses();
+// currEncoder1Count = encoder1.getPulses();
+//
+// if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
+// overrideFloodFill = true;
+// }
+//
+// wait_ms(300); // reduce this once we fine tune this!
//wait_ms(1500);
//turnRight();
@@ -1263,10 +1042,9 @@
//break;
//pidOnEncoders();
// moveForwardUntilWallIr();
- //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
//serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
- // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
- //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
+ double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
+ serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
//reading = Rec_4.read();
// serial.printf("reading: %f\n", reading);
--- a/main.h Sun May 21 13:04:21 2017 +0000
+++ b/main.h Wed May 24 01:57:01 2017 +0000
@@ -134,4 +134,257 @@
{14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
};
+int distanceToCenter[16][16] = {
+ {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
+ {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
+ {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
+ {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
+ {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
+ {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
+ {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
+ {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
+ {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
+ {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
+ {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
+ {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
+ {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
+ {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
+ {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
+ {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
+ };
+
+int distanceToStart[16][16] = {0};
+
+
+
+/* Constants for when HIGH_PWM_VOLTAGE = 0.2
+#define IP_CONSTANT 6
+#define II_CONSTANT 0
+#define ID_CONSTANT 1
+*/
+
+// Constants for when HIGH_PWM_VOLTAGE = 0.1
+// #define IP_CONSTANT 8.85
+// #define II_CONSTANT 0.005
+// #define ID_CONSTANT 3.15
+#define IP_CONSTANT 8.2
+#define II_CONSTANT 0.06
+#define ID_CONSTANT 7.55
+
+const int desiredCount180 = 2870;
+const int desiredCountR = 1700;
+const int desiredCountL = 1700;
+
+const int oneCellCount = 5400;
+const int oneCellCountMomentum = 4550;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
+
+float receiverOneReading = 0.0;
+float receiverTwoReading = 0.0;
+float receiverThreeReading = 0.0;
+float receiverFourReading = 0.0;
+
+float ir1base = 0.0;
+float ir2base = 0.0;
+
+float ir3base = 0.0;
+
+float ir4base = 0.0;
+
+float initAverageL = 8.25;
+float averageDivL = 27.8; //blue
+float initAverageR = 8.75; //4.5
+float averageDivR = 28.5; //red
+float averageDivUpper = 0.5;
+
+float noWallR = 0.007;
+float noWallL = 0.007;
+
+inline void turnLeft()
+{
+ double speed0 = 0.11;
+ double speed1 = -0.13;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ int desiredCount0 = initial0 - desiredCountL;
+ int desiredCount1 = initial1 + desiredCountL;
+
+ int count0 = initial0;
+ int count1 = initial1;
+
+ double error0 = count0 - desiredCount0;
+ double error1 = count1 - desiredCount1;
+
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
+
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ } else {
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
+
+ if (counter > 60) {
+ break;
+ }
+ }
+
+ right_motor.brake();
+ left_motor.brake();
+ turnFlag = 0; // zeroing out the flags!
+ currDir -= 1;
+}
+
+
+inline void turnRight()
+{
+ double speed0 = -0.11;
+ double speed1 = 0.13;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ int desiredCount0 = initial0 + desiredCountR;
+ int desiredCount1 = initial1 - desiredCountR;
+
+ int count0 = initial0;
+ int count1 = initial1;
+
+ double error0 = count0 - desiredCount0;
+ double error1 = count1 - desiredCount1;
+
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
+
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ } else {
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
+
+ if (counter > 60) {
+ break;
+ }
+ }
+
+ right_motor.brake();
+ left_motor.brake();
+ turnFlag = 0;
+ currDir += 1;
+}
+
+inline void turnLeft180()
+{
+ double speed0 = 0.15;
+ double speed1 = -0.15;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ int desiredCount0 = initial0 - desiredCountL*2;
+ int desiredCount1 = initial1 + desiredCountL*2;
+
+ int count0 = initial0;
+ int count1 = initial1;
+
+ double error0 = count0 - desiredCount0;
+ double error1 = count1 - desiredCount1;
+
+
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
+
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ } else {
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
+
+ if (counter > 60) {
+ break;
+ }
+ }
+
+ right_motor.brake();
+ left_motor.brake();
+ currDir -= 2;
+}
+
+inline void turnRight180()
+{
+ double speed0 = -0.16;
+ double speed1 = 0.16;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ int desiredCount0 = initial0 + desiredCount180;
+ int desiredCount1 = initial1 - desiredCount180;
+
+ int count0 = initial0;
+ int count1 = initial1;
+
+ double error0 = count0 - desiredCount0;
+ double error1 = count1 - desiredCount1;
+
+
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
+
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ } else {
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
+
+ if (counter > 60) {
+ break;
+ }
+ }
+ right_motor.brake();
+ left_motor.brake();
+ currDir += 2;
+}
+
#endif
\ No newline at end of file