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-04-01
- Revision:
- 16:4e76b53cdef2
- Parent:
- 15:3967cb2d93f5
File content as of revision 16:4e76b53cdef2:
#include "scanner.h" #include "mbed.h" #include "LongRangeSensor.h" #include "StepperDrive.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, LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, float _period) : pc(pc1), drive(_drive), servoL(_servoL), servoR(_servoR), 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 localizeRight status avoidFlag = 0; huntFlag = 0; localizeRightFlag = 0; localizeLeftFlag = 0; invertL = -1; invertR = 1; dutyL = MIN_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: // void localizeLeftMode() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // This method initializes to localizeLeft() void Scanner::localizeLeftMode() { localizeRightFlag = 0; dutyL = MAX_DUTY; dutyR = MAX_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.2); localizeLeftFlag = 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() { pitEnable = 0; float distLocalR = longRangeR.distInchesR(); if (distLocalR >= 9.2) { drive.pauseMove(); drive.move(0.0, ((-5.0)*(3.14159 / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } drive.resumeMove(); } else if (distLocalR <= 8.2) { drive.pauseMove(); drive.move(0.0, ((5.0)*(3.14159 / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } drive.resumeMove(); } // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pitEnable = 1; } // FUNCTION: // void localizeRightMode() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Initializes to localizeRight() void Scanner::localizeRightMode() { localizeLeftFlag = 0; dutyL = MIN_DUTY; dutyR = MIN_DUTY; servoL.write(DUTY[dutyL]); servoR.write(DUTY[dutyR]); wait(0.2); localizeRightFlag = 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() { pitEnable = 0; float distLocalL = longRangeL.distInchesL(); if (distLocalL >= 9.2) { drive.pauseMove(); drive.move(0, ((5.0)*(3.14159 / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } drive.resumeMove(); } else if (distLocalL <= 8.2) { drive.pauseMove(); drive.move(0, ((-5.0)*(3.14159 / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } drive.resumeMove(); } // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } 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) { if (localizeRightFlag == 1) localizeRight(); /*if (localizeLeftFlag == 1) localizeLeft();*/ } }