3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
Diff: scanner.cpp
- Revision:
- 6:5e24ff86b743
- Parent:
- 5:4d5601a9dffe
- Child:
- 7:52c4be3bfc1b
diff -r 4d5601a9dffe -r 5e24ff86b743 scanner.cpp --- a/scanner.cpp Tue Mar 22 18:11:36 2016 +0000 +++ b/scanner.cpp Fri Mar 25 19:51:11 2016 +0000 @@ -23,21 +23,17 @@ servoL.period(1.0/50.0); servoR.period(1.0/50.0); - float temp[13] = {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708, - 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958, - 0.1000}; + float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925, + 0.1013, 0.1100}; // sub-micro - for (int i = 0; i < 13; i++) + for (int i = 0; i < 7; i++) DUTY[i] = temp[i]; - // 0.05 = min duty cycle - // {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708, - // 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958, - // 0.1000}; obstacle = 0; distLeft = 0.0; distRight = 0.0; - distForward = 0.0; + distForwardL = 0.0; + distForwardR = 0.0; // initializing to avoid status avoidFlag = 1; @@ -51,6 +47,7 @@ wait(0.2); pitEnable = 1; scanPit.attach(this, &Scanner::scan, period); +pc.printf("Scanner created\n"); } // FUNCTION: @@ -67,8 +64,8 @@ huntFlag = 1; invertL = -1; invertR = 1; - dutyL = HALF_DUTY; - dutyR = HALF_DUTY; + dutyL = MAX_DUTY; + dutyR = MIN_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.1); @@ -116,16 +113,83 @@ wait(0.1); distLeft = longRangeL.distInchesL(); distRight = longRangeR.distInchesR(); - dutyL = HALF_DUTY; - dutyR = HALF_DUTY; + dutyL = MAX_DUTY; + dutyR = MIN_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.1); - distForward = ( longRangeL.distInchesL() + longRangeR.distInchesR() )/2.0; + distForwardL = longRangeL.distInchesL(); + distForwardR = longRangeR.distInchesR(); pitEnable = 1; } // FUNCTION: +// void localizeLeft() +// IN-PARAMETERS: +// None +// OUT-PARAMETERS: +// None +// DESCRIPTION: +// Using sensor longRangeR this method will localize to the wall +// to the left during stretches of forward motion where robot +// should remain xxx distance from the left wall. +void Scanner::localizeLeft() +{ + /****** pseudocode ***** + + dist = longRangeR dist measure + + if (dist > xxx dist) + pitEnable = 0 + pause current route + adjust angle to left + unpause route + pitEnable = 1 + + + else if (dist < xxx dist) + pitEnable = 0 + pause current route + adjust angle to right + unpause route + pitEnable = 1 + */ +} + +// FUNCTION: +// void localizeRight() +// IN-PARAMETERS: +// None +// OUT-PARAMETERS: +// None +// DESCRIPTION: +// Using sensor longRangeL this method will localize to the wall +// to the left during stretches of forward motion where robot +// should remain xxx distance from the left wall. This function +// will be called from scan when the localizeRightFlag is set. +void localizeRight() +{ + /****** pseudocode ***** + + dist = longRangeL dist measure + + if (dist > xxx dist) + pitEnable = 0 + pause current route + adjust angle to right + unpause route + pitEnable = 1 + + else if (dist < xxx dist) + pitEnable = 0 + pause current route + adjust angle to left + unpause route + pitEnable = 1 + */ +} + +// FUNCTION: // scan() // IN-PARAMETERS: // None @@ -140,10 +204,12 @@ { if (pitEnable == 1) { - // obtain distance measures from sensors + /*// obtain distance measures from sensors distLeft = longRangeL.distInchesL(); distRight = longRangeR.distInchesR(); + //check localize flags and make necessary method calls + // check avoid flag if (avoidFlag == 1) { @@ -151,18 +217,17 @@ } // check hunt flag - else if (huntFlag == 1) + if (huntFlag == 1) { // add hunt code - } + }*/ // increment/decrement servo position dutyL += invertL; dutyR += invertR; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); - if(((dutyL == HALF_DUTY) && (dutyR == HALF_DUTY)) || - ((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) || + if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) || ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY))) { invertL *= -1;