3-26-2015 DSV
Fork of scanner by
Diff: scanner.cpp
- Revision:
- 7:52c4be3bfc1b
- Parent:
- 6:5e24ff86b743
diff -r 5e24ff86b743 -r 52c4be3bfc1b scanner.cpp --- a/scanner.cpp Fri Mar 25 19:51:11 2016 +0000 +++ b/scanner.cpp Sat Mar 26 05:09:29 2016 +0000 @@ -2,20 +2,22 @@ #include "mbed.h" #include "LongRangeSensor.h" #include "VL6180x.h" +#include "StepperDrive.h" +#include <stack> // FUNCTION: // Scanner() // IN-PARAMETERS: -// Serial &pc1, PinName _servoL, PinName _servoR, +// Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL, // Gp2x &_longRangeR, float _period = 10e-3 // OUT-PARAMETERS: // None // DESCRIPTION: // Constructor. -Scanner::Scanner(Serial &pc1, PinName _servoL, PinName _servoR, +Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, VL6180x &_shortRangeL, VL6180x &_shortRangeR, LongRangeSensor &_longRangeL, - LongRangeSensor &_longRangeR, float _period) : pc(pc1), servoL(_servoL), + LongRangeSensor &_longRangeR, float _period) : pc(pc1), drive(_drive), servoL(_servoL), servoR(_servoR), shortRangeL(_shortRangeL), shortRangeR(_shortRangeR), longRangeL(_longRangeL), longRangeR(_longRangeR), period(_period) @@ -94,6 +96,76 @@ wait(0.1); pitEnable = 1; } +// FUNCTION: +// +void Scanner::hunt() +{ + // Used to store temporary distance + // recorded from VL6180x short range + // sensor. + float tempDistShortL; + float tempDistShortR; + + // Used to temporary store the number + // of stepper motor increments. + int tempStepCount = 0; + + pitEnable = 0; + + servoL.write(DUTY[4]); + servoR.write(DUTY[2]); + + // Update this code using the new VL6180x code + // from Victor. + // tempDistShortL = + // tempDistShortR = + + // This if statement moves the robot forward in order + // to continue the hunt of the peg. + while(distShortL == 0 && distShortR == 0 && tempStepCount < 34) + { + drive.move(0.25, 0.0); + } + + // Adjust the comparsion for floats with + // regards to the sensor data. + // This while loop checks the short range + // sensors and if they are not equal to + // each other, the code will center the + // robot. + while(tempDistShortL != tempDistShortR) + { + // Adjust this comparsion for floats + // with regards to the sensor data. + if(tempDistShortL > tempDistShortR) + drive.move(0.0, -5.0); + else + drive.move(0.0, 5.0); + } + + servoL.write(DUTY[dutyR]); + servoR.write(DUTY[dutyL]); + + // Update this code with the new VL6180x sensor + // code from Victor. + // tempDistShortL = + // tempDistShortR = + + // This while loop is used to position the robot + // at the correct location in order to pick up + // the peg. + while(tempDistShortL == tempDistShortR) + { + /* pseudocode + implement the code for the gripper to pick up the peg + implement the code for reading a value from the color sensor + store the value that is returned from the color sensor + implement the code for the gripper to angle up + pop the stack and reverse the moves stored in the stack + return a done value + */ + } +} // FUNCTION: // localize() @@ -134,26 +206,25 @@ // 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 +{ + float distLocalR = longRangeR.distInchesR(); - 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 - */ + if (distLocalR > 8.5) + { + pitEnable = 0; + drive.pauseMove(); + drive.move(0.0, -1.0); + drive.resumeMove(); + pitEnable = 1; + } + else if (distLocalR < 7.5) + { + pitEnable = 0; + drive.pauseMove(); + drive.move(0.0, 1.0); + drive.resumeMove(); + pitEnable = 1; + } } // FUNCTION: @@ -167,26 +238,26 @@ // 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() +void Scanner::localizeRight() { - /****** pseudocode ***** - - dist = longRangeL dist measure + float distLocalL = longRangeL.distInchesL(); - 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 - */ + if (distLocalL > 8.5) + { + pitEnable = 0; + drive.pauseMove(); + drive.move(0.0, 1.0); + drive.resumeMove(); + pitEnable = 1; + } + else if (distLocalL < 7.5) + { + pitEnable = 0; + drive.pauseMove(); + drive.move(0.0, -1.0); + drive.resumeMove(); + pitEnable = 1; + } } // FUNCTION: