3-26-2015 DSV
Fork of scanner by
scanner.cpp
- Committer:
- j_j205
- Date:
- 2016-02-23
- Revision:
- 1:2714d60284ef
- Parent:
- 0:999bb8fcd0b2
File content as of revision 1:2714d60284ef:
#include "scanner.h" #include "mbed.h" #include "Gp2x.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, Gp2x &_longRangeL, Gp2x &_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); for (int i = 0; i < 13; i++) DUTY[i] = 0.0300 + (i * 0.0075); // for radioshack servo /*for (int i = 0; i < 13; i++) DUTY[i] = 0.0500 + (i * DELTA_DUTY); // 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};*/ // initializing to avoid status invertL = 1; invertR = -1; dutyL = MIN_DUTY; dutyR = MAX_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.2); pitEnable = 1; pit.attach(this, &Scanner::scan, period); } // FUNCTION: // hunt() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Hunts for victim. void Scanner::hunt() { pitEnable = 0; invertL = 1; invertR = -1; dutyL = HALF_DUTY; dutyR = HALF_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.1); pitEnable = 1; } // FUNCTION: // avoid() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Scans to avoid obstacles. void Scanner::avoid() { pitEnable = 0; invertL = 1; invertR = -1; dutyL = MIN_DUTY; dutyR = MAX_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. int Scanner::localize() { /* need to implement */ return 0; } // 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 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; } } }