3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
scanner.cpp
- Committer:
- Hellkite85
- Date:
- 2016-03-26
- Revision:
- 7:52c4be3bfc1b
- Parent:
- 6:5e24ff86b743
- Child:
- 8:32a445ae1d72
File content as of revision 7:52c4be3bfc1b:
#include "scanner.h" #include "mbed.h" #include "LongRangeSensor.h" #include "VL6180x.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, VL6180x &_shortRangeL, VL6180x &_shortRangeR, LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, float _period) : pc(pc1), drive(_drive), 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: // void Scanner::hunt() { // Used to store temporary distance // recorded from VL6180x short range // sensor. float tempDistShortL; float tempDistShortR; // Used to temporary store the number // of stepper motor increments. int tempStepCount = 0; pitEnable = 0; servoL.write(DUTY[4]); servoR.write(DUTY[2]); // Update this code using the new VL6180x code // from Victor. // tempDistShortL = // tempDistShortR = // This if statement moves the robot forward in order // to continue the hunt of the peg. while(distShortL == 0 && distShortR == 0 && tempStepCount < 34) { drive.move(0.25, 0.0); } // Adjust the comparsion for floats with // regards to the sensor data. // This while loop checks the short range // sensors and if they are not equal to // each other, the code will center the // robot. while(tempDistShortL != tempDistShortR) { // Adjust this comparsion for floats // with regards to the sensor data. if(tempDistShortL > tempDistShortR) drive.move(0.0, -5.0); else drive.move(0.0, 5.0); } servoL.write(DUTY[dutyR]); servoR.write(DUTY[dutyL]); // Update this code with the new VL6180x sensor // code from Victor. // tempDistShortL = // tempDistShortR = // This while loop is used to position the robot // at the correct location in order to pick up // the peg. while(tempDistShortL == tempDistShortR) { /* pseudocode implement the code for the gripper to pick up the peg implement the code for reading a value from the color sensor store the value that is returned from the color sensor implement the code for the gripper to angle up pop the stack and reverse the moves stored in the stack return a done value */ } } // 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() { float distLocalR = longRangeR.distInchesR(); if (distLocalR > 8.5) { pitEnable = 0; drive.pauseMove(); drive.move(0.0, -1.0); drive.resumeMove(); pitEnable = 1; } else if (distLocalR < 7.5) { pitEnable = 0; drive.pauseMove(); drive.move(0.0, 1.0); drive.resumeMove(); 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 Scanner::localizeRight() { float distLocalL = longRangeL.distInchesL(); if (distLocalL > 8.5) { pitEnable = 0; drive.pauseMove(); drive.move(0.0, 1.0); drive.resumeMove(); pitEnable = 1; } else if (distLocalL < 7.5) { pitEnable = 0; drive.pauseMove(); drive.move(0.0, -1.0); drive.resumeMove(); 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; } } }