3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
Diff: scanner.cpp
- Revision:
- 3:992558a021d7
- Parent:
- 2:b281034eda86
- Child:
- 4:6a8d80954323
diff -r b281034eda86 -r 992558a021d7 scanner.cpp --- a/scanner.cpp Tue Feb 23 23:35:41 2016 +0000 +++ b/scanner.cpp Mon Mar 21 01:32:09 2016 +0000 @@ -23,40 +23,50 @@ 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 + 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] = 0.0500 + (i * DELTA_DUTY); // 0.05 = min duty cycle + 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};*/ + // 0.1000}; + + obstacle = 0; + distLeft = 0.0; + distRight = 0.0; + distForward = 0.0; // initializing to avoid status - invertL = 1; - invertR = -1; - dutyL = MIN_DUTY; - dutyR = MAX_DUTY; + 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; - pit.attach(this, &Scanner::scan, period); + scanPit.attach(this, &Scanner::scan, period); } // FUNCTION: -// hunt() +// huntMode() // IN-PARAMETERS: // None -// OUT-PARAMETERS: // None // DESCRIPTION: // Hunts for victim. -void Scanner::hunt() +void Scanner::huntMode() { pitEnable = 0; - invertL = 1; - invertR = -1; + avoidFlag = 0; + huntFlag = 1; + invertL = -1; + invertR = 1; dutyL = HALF_DUTY; dutyR = HALF_DUTY; servoL.write(DUTY[dutyL]); @@ -66,20 +76,22 @@ } // FUNCTION: -// avoid() +// avoidMode() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Scans to avoid obstacles. -void Scanner::avoid() +void Scanner::avoidMode() { pitEnable = 0; - invertL = 1; - invertR = -1; - dutyL = MIN_DUTY; - dutyR = MAX_DUTY; + 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); @@ -94,10 +106,23 @@ // None // DESCRIPTION: // Checks localization points. -int Scanner::localize() +void Scanner::localize() { - /* need to implement */ - return 0; + pitEnable = 0; + dutyL = MIN_DUTY; + dutyR = MAX_DUTY; + servoL.write(DUTY[dutyL]); + servoR.write(DUTY[dutyR]); + wait(0.1); + /* distLeft = longRangL.read(); + distRight = longRangeR.read(); */ + dutyL = HALF_DUTY; + dutyR = HALF_DUTY; + servoL.write(DUTY[dutyL]); + servoR.write(DUTY[dutyR]); + wait(0.1); + /* distForward = ( longRangeL.read() + longRangeR.read() )/2.0 */ + pitEnable = 1; } // FUNCTION: @@ -116,11 +141,16 @@ if (pitEnable == 1) { // obtain distance measures from sensors + // check avoid flag + // add obstacle avoid code + // check hunt flag + // 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))) @@ -129,4 +159,4 @@ invertR *= -1; } } -} \ No newline at end of file +}