3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
scanner.cpp
- Committer:
- j_j205
- Date:
- 2016-04-06
- Revision:
- 17:ec04d60f38bd
- Parent:
- 15:3967cb2d93f5
- Child:
- 18:f5c46fb2cd31
File content as of revision 17:ec04d60f38bd:
#include "scanner.h" #include "mbed.h" #include "LongRangeSensor.h" #include "ShortRangeSensor.h" #include "StepperDrive.h" #include "Gripper.h" #include <stack> // FUNCTION: // Scanner() // IN-PARAMETERS: // 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, StepperDrive &_drive, PinName _servoL, PinName _servoR, ShortRangeSensor &_shortRangeL, ShortRangeSensor &_shortRangeR, LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, Gripper &_robotGrip, float _period) : pc(pc1), drive(_drive), servoL(_servoL), servoR(_servoR), shortRangeL(_shortRangeL), shortRangeR(_shortRangeR), longRangeL(_longRangeL), longRangeR(_longRangeR), robotGrip(_robotGrip), period(_period) { servoL.period(1.0/50.0); servoR.period(1.0/50.0); float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925, 0.1013, 0.1100}; // sub-micro for (int i = 0; i < 7; i++) DUTY[i] = temp[i]; obstacle = 0; distLeft = 0.0; distRight = 0.0; distForwardL = 0.0; distForwardR = 0.0; // initializing to localizeRight status avoidFlag = 0; huntFlag = 0; localizeRightFlag = 1; localizeLeftFlag = 0; invertL = -1; invertR = 1; dutyL = MIN_DUTY; dutyR = MIN_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.2); pitEnable = 1; scanPit.attach(this, &Scanner::scan, period); pc.printf("Scanner created\n"); } // FUNCTION: // huntMode() // IN-PARAMETERS: // None // None // DESCRIPTION: // Hunts for victim. void Scanner::huntMode() { pitEnable = 0; avoidFlag = 0; huntFlag = 1; invertL = -1; invertR = 1; dutyL = MAX_DUTY; dutyR = MIN_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.1); pitEnable = 1; } // FUNCTION: // avoidMode() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Scans to avoid obstacles. void Scanner::avoidMode() { pitEnable = 0; avoidFlag = 1; huntFlag = 0; invertL = -1; invertR = 1; dutyL = MAX_DUTY; dutyR = MIN_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.1); pitEnable = 1; } // FUNCTION: // 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]); // This initializes the temporary distance variables // with the data collected from the VL6180X sensors. tempDistShortL = shortRangeL.getRange(); tempDistShortR = shortRangeR.getRange(); // 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); while(!drive.isMoveDone()) { wait(1e-6); } 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 ideally checks the short range // sensors and if they are not equal to // each other, the code will center the // 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(distShortL > (distShortR + tolerance)) { drive.move(0.0, (-5.0)*(3.14159 / 180.0)); while(!drive.isMoveDone()) { wait(1e-6); } moveRobot.distance = 0.0; moveRobot.angle = ((-5.0)*(3.14159 / 180.0)); myStack.push(moveRobot); } else { drive.move(0.0, (5.0*(3.14159 / 180.0))); while(!drive.isMoveDone()) { wait(1e-6); } moveRobot.distance = 0.0; moveRobot.angle = ((5.0)*(3.14159 / 180.0)); myStack.push(moveRobot); } } servoL.write(DUTY[dutyR]); servoR.write(DUTY[dutyL]); // 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 vertically with regards // to the peg in order to pick up the peg. The tolerance while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR)) { drive.move(0.125, 0.0); while(!drive.isMoveDone()) { wait(1e-6); } 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))); while(!drive.isMoveDone()) { wait(1e-6); } } } // FUNCTION: // localize() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Checks localization points. void Scanner::localize() { pitEnable = 0; dutyL = MIN_DUTY; dutyR = MAX_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.1); distLeft = longRangeL.distInchesL(); distRight = longRangeR.distInchesR(); dutyL = MAX_DUTY; dutyR = MIN_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.1); distForwardL = longRangeL.distInchesL(); distForwardR = longRangeR.distInchesR(); pitEnable = 1; } // 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 // 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() { pitEnable = 0; float distLocalR = longRangeR.distInchesR(); if (distLocalR >= 9.2) { drive.pauseMove(); drive.move(0.0, ((-5.0)*(3.14159 / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } drive.resumeMove(); } else if (distLocalR <= 8.2) { drive.pauseMove(); drive.move(0.0, ((5.0)*(3.14159 / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } drive.resumeMove(); } // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pitEnable = 1; } // FUNCTION: // void localizeRightMode() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Initializes to localizeRight() void Scanner::localizeRightMode() { 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 // 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 Scanner::localizeRight() { pitEnable = 0; float distLocalL = longRangeL.distInchesL(); if (distLocalL >= 9.2) { drive.pauseMove(); drive.move(0, ((5.0)*(3.14159 / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } drive.resumeMove(); } else if (distLocalL <= 8.2) { drive.pauseMove(); drive.move(0, ((-5.0)*(3.14159 / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } drive.resumeMove(); } // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pitEnable = 1; } // FUNCTION: // scan() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Ticker function to make servos scan either the inward-facing // 90 degrees or the outward-facing 90 degrees. Distance // measures are obtained from both the short range and long // range sensors at 15 degree intervals. void Scanner::scan() { if (pitEnable == 1) { if (localizeRightFlag == 1) localizeRight(); /*if (localizeLeftFlag == 1) localizeLeft();*/ } }