Scanner code will include the following: obstacle avoidance, hunting for victims, and localization checks.
scanner.cpp
- Committer:
- j_j205
- Date:
- 2016-03-25
- Revision:
- 6:5e24ff86b743
- Parent:
- 5:4d5601a9dffe
File content as of revision 6:5e24ff86b743:
#include "scanner.h" #include "mbed.h" #include "LongRangeSensor.h" #include "VL6180x.h" // FUNCTION: // Scanner() // IN-PARAMETERS: // Serial &pc1, 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, VL6180x &_shortRangeL, VL6180x &_shortRangeR, LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, float _period) : pc(pc1), servoL(_servoL), servoR(_servoR), shortRangeL(_shortRangeL), shortRangeR(_shortRangeR), longRangeL(_longRangeL), longRangeR(_longRangeR), 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: // 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() { /****** 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 // 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; } } }