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-03-26
- Revision:
- 12:514544a4014f
- Parent:
- 9:bae63bc84829
- Child:
- 13:8767be1c5cc2
File content as of revision 12:514544a4014f:
#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 avoid status avoidFlag = 1; huntFlag = 0; invertL = -1; invertR = 1; dutyL = MAX_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); 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); 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]); // 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); 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: // 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 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() { float distLocalR = longRangeR.distInchesR(); 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: // 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() { float distLocalL = longRangeL.distInchesL(); 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: // 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) { /*// 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) { // add obstacle avoid code } // check hunt flag if (huntFlag == 1) { // add hunt code }*/ // increment/decrement servo position dutyL += invertL; dutyR += invertR; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) || ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY))) { invertL *= -1; invertR *= -1; } } }