Scanner code will include the following: obstacle avoidance, hunting for victims, and localization checks.
scanner.cpp@1:2714d60284ef, 2016-02-23 (annotated)
- Committer:
- j_j205
- Date:
- Tue Feb 23 23:30:43 2016 +0000
- Revision:
- 1:2714d60284ef
- Parent:
- 0:999bb8fcd0b2
2/23/16 JJ
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
j_j205 | 0:999bb8fcd0b2 | 1 | #include "scanner.h" |
j_j205 | 0:999bb8fcd0b2 | 2 | #include "mbed.h" |
j_j205 | 0:999bb8fcd0b2 | 3 | #include "Gp2x.h" |
j_j205 | 0:999bb8fcd0b2 | 4 | #include "VL6180x.h" |
j_j205 | 0:999bb8fcd0b2 | 5 | |
j_j205 | 0:999bb8fcd0b2 | 6 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 7 | // Scanner() |
j_j205 | 0:999bb8fcd0b2 | 8 | // IN-PARAMETERS: |
j_j205 | 1:2714d60284ef | 9 | // Serial &pc1, PinName _servoL, PinName _servoR, |
j_j205 | 1:2714d60284ef | 10 | // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL, |
j_j205 | 1:2714d60284ef | 11 | // Gp2x &_longRangeR, float _period = 10e-3 |
j_j205 | 0:999bb8fcd0b2 | 12 | // OUT-PARAMETERS: |
j_j205 | 1:2714d60284ef | 13 | // None |
j_j205 | 0:999bb8fcd0b2 | 14 | // DESCRIPTION: |
j_j205 | 1:2714d60284ef | 15 | // Constructor. |
j_j205 | 0:999bb8fcd0b2 | 16 | Scanner::Scanner(Serial &pc1, PinName _servoL, PinName _servoR, |
j_j205 | 0:999bb8fcd0b2 | 17 | VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL, |
j_j205 | 0:999bb8fcd0b2 | 18 | Gp2x &_longRangeR, float _period) : pc(pc1), servoL(_servoL), |
j_j205 | 0:999bb8fcd0b2 | 19 | servoR(_servoR), shortRangeL(_shortRangeL), |
j_j205 | 0:999bb8fcd0b2 | 20 | shortRangeR(_shortRangeR), longRangeL(_longRangeL), |
j_j205 | 0:999bb8fcd0b2 | 21 | longRangeR(_longRangeR), period(_period) |
j_j205 | 0:999bb8fcd0b2 | 22 | { |
j_j205 | 0:999bb8fcd0b2 | 23 | servoL.period(1.0/50.0); |
j_j205 | 0:999bb8fcd0b2 | 24 | servoR.period(1.0/50.0); |
j_j205 | 1:2714d60284ef | 25 | |
j_j205 | 1:2714d60284ef | 26 | for (int i = 0; i < 13; i++) |
j_j205 | 1:2714d60284ef | 27 | DUTY[i] = 0.0300 + (i * 0.0075); // for radioshack servo |
j_j205 | 1:2714d60284ef | 28 | |
j_j205 | 1:2714d60284ef | 29 | /*for (int i = 0; i < 13; i++) |
j_j205 | 1:2714d60284ef | 30 | DUTY[i] = 0.0500 + (i * DELTA_DUTY); // 0.05 = min duty cycle |
j_j205 | 1:2714d60284ef | 31 | // {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708, |
j_j205 | 1:2714d60284ef | 32 | // 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958, |
j_j205 | 1:2714d60284ef | 33 | // 0.1000};*/ |
j_j205 | 1:2714d60284ef | 34 | |
j_j205 | 1:2714d60284ef | 35 | // initializing to avoid status |
j_j205 | 1:2714d60284ef | 36 | invertL = 1; |
j_j205 | 1:2714d60284ef | 37 | invertR = -1; |
j_j205 | 1:2714d60284ef | 38 | dutyL = MIN_DUTY; |
j_j205 | 1:2714d60284ef | 39 | dutyR = MAX_DUTY; |
j_j205 | 1:2714d60284ef | 40 | servoL.write(DUTY[dutyL]); |
j_j205 | 1:2714d60284ef | 41 | servoR.write(DUTY[dutyR]); |
j_j205 | 1:2714d60284ef | 42 | wait(0.2); |
j_j205 | 1:2714d60284ef | 43 | pitEnable = 1; |
j_j205 | 1:2714d60284ef | 44 | pit.attach(this, &Scanner::scan, period); |
j_j205 | 0:999bb8fcd0b2 | 45 | } |
j_j205 | 0:999bb8fcd0b2 | 46 | |
j_j205 | 0:999bb8fcd0b2 | 47 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 48 | // hunt() |
j_j205 | 0:999bb8fcd0b2 | 49 | // IN-PARAMETERS: |
j_j205 | 1:2714d60284ef | 50 | // None |
j_j205 | 0:999bb8fcd0b2 | 51 | // OUT-PARAMETERS: |
j_j205 | 1:2714d60284ef | 52 | // None |
j_j205 | 0:999bb8fcd0b2 | 53 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 54 | // Hunts for victim. |
j_j205 | 1:2714d60284ef | 55 | void Scanner::hunt() |
j_j205 | 0:999bb8fcd0b2 | 56 | { |
j_j205 | 1:2714d60284ef | 57 | pitEnable = 0; |
j_j205 | 1:2714d60284ef | 58 | invertL = 1; |
j_j205 | 1:2714d60284ef | 59 | invertR = -1; |
j_j205 | 1:2714d60284ef | 60 | dutyL = HALF_DUTY; |
j_j205 | 1:2714d60284ef | 61 | dutyR = HALF_DUTY; |
j_j205 | 1:2714d60284ef | 62 | servoL.write(DUTY[dutyL]); |
j_j205 | 1:2714d60284ef | 63 | servoR.write(DUTY[dutyR]); |
j_j205 | 1:2714d60284ef | 64 | wait(0.1); |
j_j205 | 1:2714d60284ef | 65 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 66 | } |
j_j205 | 0:999bb8fcd0b2 | 67 | |
j_j205 | 0:999bb8fcd0b2 | 68 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 69 | // avoid() |
j_j205 | 0:999bb8fcd0b2 | 70 | // IN-PARAMETERS: |
j_j205 | 1:2714d60284ef | 71 | // None |
j_j205 | 0:999bb8fcd0b2 | 72 | // OUT-PARAMETERS: |
j_j205 | 1:2714d60284ef | 73 | // None |
j_j205 | 0:999bb8fcd0b2 | 74 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 75 | // Scans to avoid obstacles. |
j_j205 | 1:2714d60284ef | 76 | void Scanner::avoid() |
j_j205 | 0:999bb8fcd0b2 | 77 | { |
j_j205 | 1:2714d60284ef | 78 | pitEnable = 0; |
j_j205 | 1:2714d60284ef | 79 | invertL = 1; |
j_j205 | 1:2714d60284ef | 80 | invertR = -1; |
j_j205 | 1:2714d60284ef | 81 | dutyL = MIN_DUTY; |
j_j205 | 1:2714d60284ef | 82 | dutyR = MAX_DUTY; |
j_j205 | 1:2714d60284ef | 83 | servoL.write(DUTY[dutyL]); |
j_j205 | 1:2714d60284ef | 84 | servoR.write(DUTY[dutyR]); |
j_j205 | 1:2714d60284ef | 85 | wait(0.1); |
j_j205 | 1:2714d60284ef | 86 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 87 | } |
j_j205 | 0:999bb8fcd0b2 | 88 | |
j_j205 | 0:999bb8fcd0b2 | 89 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 90 | // localize() |
j_j205 | 0:999bb8fcd0b2 | 91 | // IN-PARAMETERS: |
j_j205 | 1:2714d60284ef | 92 | // None |
j_j205 | 0:999bb8fcd0b2 | 93 | // OUT-PARAMETERS: |
j_j205 | 1:2714d60284ef | 94 | // None |
j_j205 | 0:999bb8fcd0b2 | 95 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 96 | // Checks localization points. |
j_j205 | 0:999bb8fcd0b2 | 97 | int Scanner::localize() |
j_j205 | 0:999bb8fcd0b2 | 98 | { |
j_j205 | 1:2714d60284ef | 99 | /* need to implement */ |
j_j205 | 0:999bb8fcd0b2 | 100 | return 0; |
j_j205 | 0:999bb8fcd0b2 | 101 | } |
j_j205 | 0:999bb8fcd0b2 | 102 | |
j_j205 | 0:999bb8fcd0b2 | 103 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 104 | // scan() |
j_j205 | 0:999bb8fcd0b2 | 105 | // IN-PARAMETERS: |
j_j205 | 1:2714d60284ef | 106 | // None |
j_j205 | 0:999bb8fcd0b2 | 107 | // OUT-PARAMETERS: |
j_j205 | 1:2714d60284ef | 108 | // None |
j_j205 | 0:999bb8fcd0b2 | 109 | // DESCRIPTION: |
j_j205 | 1:2714d60284ef | 110 | // Ticker function to make servos scan either the inward-facing |
j_j205 | 1:2714d60284ef | 111 | // 90 degrees or the outward-facing 90 degrees. Distance |
j_j205 | 1:2714d60284ef | 112 | // measures are obtained from both the short range and long |
j_j205 | 1:2714d60284ef | 113 | // range sensors at 15 degree intervals. |
j_j205 | 0:999bb8fcd0b2 | 114 | void Scanner::scan() |
j_j205 | 0:999bb8fcd0b2 | 115 | { |
j_j205 | 1:2714d60284ef | 116 | if (pitEnable == 1) |
j_j205 | 1:2714d60284ef | 117 | { |
j_j205 | 1:2714d60284ef | 118 | // obtain distance measures from sensors |
j_j205 | 1:2714d60284ef | 119 | dutyL += invertL; |
j_j205 | 1:2714d60284ef | 120 | dutyR += invertR; |
j_j205 | 1:2714d60284ef | 121 | servoL.write(DUTY[dutyL]); |
j_j205 | 1:2714d60284ef | 122 | servoR.write(DUTY[dutyR]); |
j_j205 | 0:999bb8fcd0b2 | 123 | |
j_j205 | 1:2714d60284ef | 124 | if(((dutyL == HALF_DUTY) && (dutyR == HALF_DUTY)) || |
j_j205 | 1:2714d60284ef | 125 | ((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) || |
j_j205 | 1:2714d60284ef | 126 | ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY))) |
j_j205 | 1:2714d60284ef | 127 | { |
j_j205 | 1:2714d60284ef | 128 | invertL *= -1; |
j_j205 | 1:2714d60284ef | 129 | invertR *= -1; |
j_j205 | 1:2714d60284ef | 130 | } |
j_j205 | 1:2714d60284ef | 131 | } |
j_j205 | 0:999bb8fcd0b2 | 132 | } |