3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
Diff: scanner.cpp
- Revision:
- 9:bae63bc84829
- Parent:
- 8:32a445ae1d72
- Child:
- 12:514544a4014f
diff -r 32a445ae1d72 -r bae63bc84829 scanner.cpp --- a/scanner.cpp Sat Mar 26 05:26:17 2016 +0000 +++ b/scanner.cpp Sat Mar 26 23:01:49 2016 +0000 @@ -3,6 +3,7 @@ #include "LongRangeSensor.h" #include "ShortRangeSensor.h" #include "StepperDrive.h" +#include "Gripper.h" #include <stack> // FUNCTION: @@ -17,10 +18,10 @@ // Constructor. Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, ShortRangeSensor &_shortRangeL, ShortRangeSensor &_shortRangeR, LongRangeSensor &_longRangeL, - LongRangeSensor &_longRangeR, float _period) : pc(pc1), drive(_drive), servoL(_servoL), + LongRangeSensor &_longRangeR, Gripper &_robotGrip, float _period) : pc(pc1), drive(_drive), servoL(_servoL), servoR(_servoR), shortRangeL(_shortRangeL), shortRangeR(_shortRangeR), longRangeL(_longRangeL), - longRangeR(_longRangeR), period(_period) + longRangeR(_longRangeR), grip(_robotGrip), period(_period) { servoL.period(1.0/50.0); servoR.period(1.0/50.0); @@ -100,71 +101,141 @@ // void Scanner::hunt() { + // This creates an instance of a structure + // call huntMove. This structure is used to + // store the distance value and angle value + // when implementing the Navigation drive + // code. + huntMove moveRobot; + // Used to store temporary distance // recorded from VL6180x short range // sensor. float tempDistShortL; float tempDistShortR; + + float distShortL; + float distShortR; // Used to temporary store the number // of stepper motor increments. int tempStepCount = 0; + // This variable is half of the available + // tolerance of the gripper with regards + // to the peg. + float tolerance = 0.1875; + + // These variables represent the tolerance + // and plus/minus half the diameter of the + // peg measured in inches. + float toleranceAHP = 0.9375; + float toleranceSHP = 0.5625; + pitEnable = 0; + // This positions both of the servos to + // be 60 degrees inwards. servoL.write(DUTY[4]); servoR.write(DUTY[2]); - // Update this code using the new VL6180x code - // from Victor. - // tempDistShortL = - // tempDistShortR = + // This initializes the temporary distance variables + // with the data collected from the VL6180X sensors. + tempDistShortL = shortRangeL.getRange(); + tempDistShortR = shortRangeR.getRange(); - // This if statement moves the robot forward in order - // to continue the hunt of the peg. -/* while(distShortL == 0 && distShortR == 0 && tempStepCount < 34) + // This code converts the measurements from the short + // range sensor from millimeters to inches. + distShortL = (tempDistShortL / 25.0); + distShortR = (tempDistShortR / 25.0); + + // This while loop statement moves the robot forward in order + // to continue the hunt of the peg. The equals to comparsion + // statements need to be updated to reflect what the short + // range sensors output when there is nothing within the + // range of the short distance sensors. + while(distShortL == 0.0 && distShortR == 0.0 && tempStepCount < 34) { drive.move(0.25, 0.0); - }*/ + + moveRobot.distance = 0.25; + moveRobot.angle = 0.0; + + myStack.push(moveRobot); + } // Adjust the comparsion for floats with - // regards to the sensor data. - // This while loop checks the short range + // regards to the sensor data. This + // while loop ideally checks the short range // sensors and if they are not equal to // each other, the code will center the - // robot. - while(tempDistShortL != tempDistShortR) + // robot according to the short range sensor. + while((distShortL <= distShortR + tolerance) && (distShortL >= distShortR + tolerance)) { // Adjust this comparsion for floats // with regards to the sensor data. - if(tempDistShortL > tempDistShortR) + if(distShortL > (distShortR + tolerance)) + { drive.move(0.0, -5.0); + + moveRobot.distance = 0.0; + moveRobot.angle = -5.0; + + myStack.push(moveRobot); + } else + { drive.move(0.0, 5.0); + + moveRobot.distance = 0.0; + moveRobot.angle = 5.0; + + myStack.push(moveRobot); + } } servoL.write(DUTY[dutyR]); servoR.write(DUTY[dutyL]); - // Update this code with the new VL6180x sensor - // code from Victor. - // tempDistShortL = - // tempDistShortR = + // This code reinitializes the short distance + // variable now that the short distance sensors + // are pointing inward. + distShortL = shortRangeL.getRange(); + distShortR = shortRangeR.getRange(); // This while loop is used to position the robot - // at the correct location in order to pick up - // the peg. - while(tempDistShortL == tempDistShortR) + // at the correct location vertically with regards + // to the peg in order to pick up the peg. The tolerance + while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR)) { - /* 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 - */ - } + drive.move(0.125, 0.0); + + moveRobot.distance = 0.125; + moveRobot.angle = 0.0; + + myStack.push(moveRobot); + } + + // This is the code to enable the gripper to grip the peg. + robotGrip.grip() + + /* pseudocode + implement the code for reading a value from the color sensor + store the value that is returned from the color sensor + */ + + // This is the code to enable the gripper to lift the peg. + robotGrip.lift(); + + // This code uses the stack to reverse all the movement + // of the robot during this function. + while(!myStack.empty()) + { + myStack.top(); + myStack.pop(); + + drive.move((-(moveRobot.distance)),(-(moveRobot.angle))); + } } // FUNCTION: