3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
Diff: scanner.cpp
- Revision:
- 1:2714d60284ef
- Parent:
- 0:999bb8fcd0b2
diff -r 999bb8fcd0b2 -r 2714d60284ef scanner.cpp --- a/scanner.cpp Wed Feb 17 16:47:55 2016 +0000 +++ b/scanner.cpp Tue Feb 23 23:30:43 2016 +0000 @@ -6,11 +6,13 @@ // 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: -// Default constructor. +// Constructor. Scanner::Scanner(Serial &pc1, PinName _servoL, PinName _servoR, VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL, Gp2x &_longRangeR, float _period) : pc(pc1), servoL(_servoL), @@ -18,59 +20,113 @@ shortRangeR(_shortRangeR), longRangeL(_longRangeL), longRangeR(_longRangeR), period(_period) { - pit.attach(this, &Scanner::scan, 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. -int Scanner::hunt() +void Scanner::hunt() { - return 0; + 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. -int Scanner::avoid() +void Scanner::avoid() { - return 0; + 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: -// Checks localization points. +// 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; + } + } } \ No newline at end of file