3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
Diff: scanner.cpp
- Revision:
- 14:d327852dafd1
- Parent:
- 13:8767be1c5cc2
- Child:
- 15:3967cb2d93f5
--- a/scanner.cpp Sat Mar 26 23:28:14 2016 +0000 +++ b/scanner.cpp Fri Apr 01 15:55:52 2016 +0000 @@ -38,12 +38,14 @@ distForwardL = 0.0; distForwardR = 0.0; - // initializing to avoid status - avoidFlag = 1; + // initializing to localizeRight status + avoidFlag = 0; huntFlag = 0; + localizeRightFlag = 1; + localizeLeftFlag = 0; invertL = -1; invertR = 1; - dutyL = MAX_DUTY; + dutyL = MIN_DUTY; dutyR = MIN_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); @@ -267,6 +269,27 @@ } // FUNCTION: +// void localizeLeftMode() +// IN-PARAMETERS: +// None +// OUT-PARAMETERS: +// None +// DESCRIPTION: +// This method initializes to localizeLeft() +void Scanner::localizeLeftMode() +{ + localizeRightFlag = 0; + + dutyL = MAX_DUTY; + dutyR = MAX_DUTY; + servoL.write(DUTY[dutyL]); + servoR.write(DUTY[dutyR]); + wait(0.2); + + localizeLeftFlag = 1; +} + +// FUNCTION: // void localizeLeft() // IN-PARAMETERS: // None @@ -280,25 +303,56 @@ { float distLocalR = longRangeR.distInchesR(); - if (distLocalR > 8.5) + if (distLocalR >= 8.5) { pitEnable = 0; drive.pauseMove(); - drive.move(0.0, -1.0); + drive.move(0.0, ((-0.5)*(3.14159 / 180.0))); + // wait for move to complete + while(!drive.isMoveDone()) + { + wait(1e-6); + } drive.resumeMove(); pitEnable = 1; } - else if (distLocalR < 7.5) + else if (distLocalR <= 7.5) { pitEnable = 0; drive.pauseMove(); - drive.move(0.0, 1.0); + drive.move(0.0, ((0.5)*(3.14159 / 180.0))); + // wait for move to complete + while(!drive.isMoveDone()) + { + wait(1e-6); + } drive.resumeMove(); pitEnable = 1; } } // FUNCTION: +// void localizeRightMode() +// IN-PARAMETERS: +// None +// OUT-PARAMETERS: +// None +// DESCRIPTION: +// Initializes to localizeRight() +void Scanner::localizeRight() +{ + localizeLeftFlag = 0; + + dutyL = MIN_DUTY; + dutyR = MIN_DUTY; + servoL.write(DUTY[dutyL]); + servoR.write(DUTY[dutyR]); + wait(0.2); + + localizeRightFlag = 1; +} + +// FUNCTION: // void localizeRight() // IN-PARAMETERS: // None @@ -311,26 +365,42 @@ // will be called from scan when the localizeRightFlag is set. void Scanner::localizeRight() { + dutyL = MIN_DUTY; + dutyR = MIN_DUTY; + servoL.write(DUTY[dutyL]); + servoR.write(DUTY[dutyR]); float distLocalL = longRangeL.distInchesL(); - if (distLocalL > 8.5) + if (distLocalL >= 8.5) { pitEnable = 0; drive.pauseMove(); - drive.move(0.0, 1.0); + drive.move(0, ((2.0)*(3.14159 / 180.0))); + // wait for move to complete + while(!drive.isMoveDone()) + { + wait(1e-6); + } drive.resumeMove(); pitEnable = 1; } - else if (distLocalL < 7.5) + else if (distLocalL <= 7.5) { pitEnable = 0; drive.pauseMove(); - drive.move(0.0, -1.0); + drive.move(0, ((-2.0)*(3.14159 / 180.0))); + // wait for move to complete + while(!drive.isMoveDone()) + { + wait(1e-6); + } drive.resumeMove(); pitEnable = 1; } } +void Scanner + // FUNCTION: // scan() // IN-PARAMETERS: @@ -349,8 +419,8 @@ if (localizeRightFlag == 1) localizeRight(); - if (localizeLeftFlag == 1) - localizeLeft(); + /*if (localizeLeftFlag == 1) + localizeLeft();*/ /*// obtain distance measures from sensors distLeft = longRangeL.distInchesL();