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-22
- Revision:
- 5:4d5601a9dffe
- Parent:
- 4:6a8d80954323
- Child:
- 6:5e24ff86b743
File content as of revision 5:4d5601a9dffe:
#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[13] = {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708, 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958, 0.1000}; for (int i = 0; i < 13; i++) DUTY[i] = temp[i]; // 0.05 = min duty cycle // {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708, // 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958, // 0.1000}; obstacle = 0; distLeft = 0.0; distRight = 0.0; distForward = 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); } // FUNCTION: // huntMode() // IN-PARAMETERS: // None // None // DESCRIPTION: // Hunts for victim. void Scanner::huntMode() { pitEnable = 0; avoidFlag = 0; huntFlag = 1; invertL = -1; invertR = 1; dutyL = HALF_DUTY; dutyR = HALF_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 = HALF_DUTY; dutyR = HALF_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.1); distForward = ( longRangeL.distInchesL() + longRangeR.distInchesR() )/2.0; 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 avoid flag if (avoidFlag == 1) { // add obstacle avoid code } // check hunt flag else if (huntFlag == 1) { // add hunt code } // increment/decrement servo position dutyL += invertL; dutyR += invertR; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); if(((dutyL == HALF_DUTY) && (dutyR == HALF_DUTY)) || ((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) || ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY))) { invertL *= -1; invertR *= -1; } } }