Justin Jordan / scanner

Scanner code will include the following: obstacle avoidance, hunting for victims, and localization checks.

scanner.cpp@6:5e24ff86b743, 2016-03-25 (annotated)

Committer:
j_j205
Date:
Fri Mar 25 19:51:11 2016 +0000
Revision:
6:5e24ff86b743
Parent:
5:4d5601a9dffe 
3/25/16 2:50 JJ

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 0:999bb8fcd0b2 1 #include "scanner.h"
j_j205 0:999bb8fcd0b2 2 #include "mbed.h"
j_j205 4:6a8d80954323 3 #include "LongRangeSensor.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 2:b281034eda86 9 // Serial &pc1, PinName _servoL, PinName _servoR,
j_j205 2:b281034eda86 10 // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
j_j205 2:b281034eda86 11 // Gp2x &_longRangeR, float _period = 10e-3
j_j205 0:999bb8fcd0b2 12 // OUT-PARAMETERS:
j_j205 2:b281034eda86 13 // None
j_j205 0:999bb8fcd0b2 14 // DESCRIPTION:
j_j205 2:b281034eda86 15 // Constructor.
j_j205 2:b281034eda86 16 Scanner::Scanner(Serial &pc1, PinName _servoL, PinName _servoR,
j_j205 4:6a8d80954323 17 VL6180x &_shortRangeL, VL6180x &_shortRangeR, LongRangeSensor &_longRangeL,
j_j205 4:6a8d80954323 18 LongRangeSensor &_longRangeR, float _period) : pc(pc1), servoL(_servoL),
j_j205 2:b281034eda86 19 servoR(_servoR), shortRangeL(_shortRangeL),
j_j205 2:b281034eda86 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 2:b281034eda86 25
j_j205 6:5e24ff86b743 26 float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925,
j_j205 6:5e24ff86b743 27 0.1013, 0.1100}; // sub-micro
j_j205 2:b281034eda86 28
j_j205 6:5e24ff86b743 29 for (int i = 0; i < 7; i++)
j_j205 3:992558a021d7 30 DUTY[i] = temp[i];
j_j205 3:992558a021d7 31
j_j205 3:992558a021d7 32 obstacle = 0;
j_j205 3:992558a021d7 33 distLeft = 0.0;
j_j205 3:992558a021d7 34 distRight = 0.0;
j_j205 6:5e24ff86b743 35 distForwardL = 0.0;
j_j205 6:5e24ff86b743 36 distForwardR = 0.0;
j_j205 2:b281034eda86 37
j_j205 2:b281034eda86 38 // initializing to avoid status
j_j205 3:992558a021d7 39 avoidFlag = 1;
j_j205 3:992558a021d7 40 huntFlag = 0;
j_j205 3:992558a021d7 41 invertL = -1;
j_j205 3:992558a021d7 42 invertR = 1;
j_j205 3:992558a021d7 43 dutyL = MAX_DUTY;
j_j205 3:992558a021d7 44 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 45 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 46 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 47 wait(0.2);
j_j205 2:b281034eda86 48 pitEnable = 1;
j_j205 3:992558a021d7 49 scanPit.attach(this, &Scanner::scan, period);
j_j205 6:5e24ff86b743 50 pc.printf("Scanner created\n");
j_j205 0:999bb8fcd0b2 51 }
j_j205 0:999bb8fcd0b2 52
j_j205 0:999bb8fcd0b2 53 // FUNCTION:
j_j205 3:992558a021d7 54 // huntMode()
j_j205 0:999bb8fcd0b2 55 // IN-PARAMETERS:
j_j205 2:b281034eda86 56 // None
j_j205 2:b281034eda86 57 // None
j_j205 0:999bb8fcd0b2 58 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 59 // Hunts for victim.
j_j205 3:992558a021d7 60 void Scanner::huntMode()
j_j205 0:999bb8fcd0b2 61 {
j_j205 2:b281034eda86 62 pitEnable = 0;
j_j205 3:992558a021d7 63 avoidFlag = 0;
j_j205 3:992558a021d7 64 huntFlag = 1;
j_j205 3:992558a021d7 65 invertL = -1;
j_j205 3:992558a021d7 66 invertR = 1;
j_j205 6:5e24ff86b743 67 dutyL = MAX_DUTY;
j_j205 6:5e24ff86b743 68 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 69 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 70 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 71 wait(0.1);
j_j205 2:b281034eda86 72 pitEnable = 1;
j_j205 0:999bb8fcd0b2 73 }
j_j205 0:999bb8fcd0b2 74
j_j205 0:999bb8fcd0b2 75 // FUNCTION:
j_j205 3:992558a021d7 76 // avoidMode()
j_j205 0:999bb8fcd0b2 77 // IN-PARAMETERS:
j_j205 2:b281034eda86 78 // None
j_j205 0:999bb8fcd0b2 79 // OUT-PARAMETERS:
j_j205 2:b281034eda86 80 // None
j_j205 0:999bb8fcd0b2 81 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 82 // Scans to avoid obstacles.
j_j205 3:992558a021d7 83 void Scanner::avoidMode()
j_j205 0:999bb8fcd0b2 84 {
j_j205 2:b281034eda86 85 pitEnable = 0;
j_j205 3:992558a021d7 86 avoidFlag = 1;
j_j205 3:992558a021d7 87 huntFlag = 0;
j_j205 3:992558a021d7 88 invertL = -1;
j_j205 3:992558a021d7 89 invertR = 1;
j_j205 3:992558a021d7 90 dutyL = MAX_DUTY;
j_j205 3:992558a021d7 91 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 92 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 93 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 94 wait(0.1);
j_j205 2:b281034eda86 95 pitEnable = 1;
j_j205 0:999bb8fcd0b2 96 }
j_j205 0:999bb8fcd0b2 97
j_j205 0:999bb8fcd0b2 98 // FUNCTION:
j_j205 0:999bb8fcd0b2 99 // localize()
j_j205 0:999bb8fcd0b2 100 // IN-PARAMETERS:
j_j205 2:b281034eda86 101 // None
j_j205 0:999bb8fcd0b2 102 // OUT-PARAMETERS:
j_j205 2:b281034eda86 103 // None
j_j205 0:999bb8fcd0b2 104 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 105 // Checks localization points.
j_j205 3:992558a021d7 106 void Scanner::localize()
j_j205 0:999bb8fcd0b2 107 {
j_j205 3:992558a021d7 108 pitEnable = 0;
j_j205 3:992558a021d7 109 dutyL = MIN_DUTY;
j_j205 3:992558a021d7 110 dutyR = MAX_DUTY;
j_j205 3:992558a021d7 111 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 112 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 113 wait(0.1);
j_j205 5:4d5601a9dffe 114 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 115 distRight = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 116 dutyL = MAX_DUTY;
j_j205 6:5e24ff86b743 117 dutyR = MIN_DUTY;
j_j205 3:992558a021d7 118 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 119 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 120 wait(0.1);
j_j205 6:5e24ff86b743 121 distForwardL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 122 distForwardR = longRangeR.distInchesR();
j_j205 3:992558a021d7 123 pitEnable = 1;
j_j205 0:999bb8fcd0b2 124 }
j_j205 0:999bb8fcd0b2 125
j_j205 0:999bb8fcd0b2 126 // FUNCTION:
j_j205 6:5e24ff86b743 127 // void localizeLeft()
j_j205 6:5e24ff86b743 128 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 129 // None
j_j205 6:5e24ff86b743 130 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 131 // None
j_j205 6:5e24ff86b743 132 // DESCRIPTION:
j_j205 6:5e24ff86b743 133 // Using sensor longRangeR this method will localize to the wall
j_j205 6:5e24ff86b743 134 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 135 // should remain xxx distance from the left wall.
j_j205 6:5e24ff86b743 136 void Scanner::localizeLeft()
j_j205 6:5e24ff86b743 137 {
j_j205 6:5e24ff86b743 138 /****** pseudocode *****
j_j205 6:5e24ff86b743 139
j_j205 6:5e24ff86b743 140 dist = longRangeR dist measure
j_j205 6:5e24ff86b743 141
j_j205 6:5e24ff86b743 142 if (dist > xxx dist)
j_j205 6:5e24ff86b743 143 pitEnable = 0
j_j205 6:5e24ff86b743 144 pause current route
j_j205 6:5e24ff86b743 145 adjust angle to left
j_j205 6:5e24ff86b743 146 unpause route
j_j205 6:5e24ff86b743 147 pitEnable = 1
j_j205 6:5e24ff86b743 148
j_j205 6:5e24ff86b743 149
j_j205 6:5e24ff86b743 150 else if (dist < xxx dist)
j_j205 6:5e24ff86b743 151 pitEnable = 0
j_j205 6:5e24ff86b743 152 pause current route
j_j205 6:5e24ff86b743 153 adjust angle to right
j_j205 6:5e24ff86b743 154 unpause route
j_j205 6:5e24ff86b743 155 pitEnable = 1
j_j205 6:5e24ff86b743 156 */
j_j205 6:5e24ff86b743 157 }
j_j205 6:5e24ff86b743 158
j_j205 6:5e24ff86b743 159 // FUNCTION:
j_j205 6:5e24ff86b743 160 // void localizeRight()
j_j205 6:5e24ff86b743 161 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 162 // None
j_j205 6:5e24ff86b743 163 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 164 // None
j_j205 6:5e24ff86b743 165 // DESCRIPTION:
j_j205 6:5e24ff86b743 166 // Using sensor longRangeL this method will localize to the wall
j_j205 6:5e24ff86b743 167 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 168 // should remain xxx distance from the left wall. This function
j_j205 6:5e24ff86b743 169 // will be called from scan when the localizeRightFlag is set.
j_j205 6:5e24ff86b743 170 void localizeRight()
j_j205 6:5e24ff86b743 171 {
j_j205 6:5e24ff86b743 172 /****** pseudocode *****
j_j205 6:5e24ff86b743 173
j_j205 6:5e24ff86b743 174 dist = longRangeL dist measure
j_j205 6:5e24ff86b743 175
j_j205 6:5e24ff86b743 176 if (dist > xxx dist)
j_j205 6:5e24ff86b743 177 pitEnable = 0
j_j205 6:5e24ff86b743 178 pause current route
j_j205 6:5e24ff86b743 179 adjust angle to right
j_j205 6:5e24ff86b743 180 unpause route
j_j205 6:5e24ff86b743 181 pitEnable = 1
j_j205 6:5e24ff86b743 182
j_j205 6:5e24ff86b743 183 else if (dist < xxx dist)
j_j205 6:5e24ff86b743 184 pitEnable = 0
j_j205 6:5e24ff86b743 185 pause current route
j_j205 6:5e24ff86b743 186 adjust angle to left
j_j205 6:5e24ff86b743 187 unpause route
j_j205 6:5e24ff86b743 188 pitEnable = 1
j_j205 6:5e24ff86b743 189 */
j_j205 6:5e24ff86b743 190 }
j_j205 6:5e24ff86b743 191
j_j205 6:5e24ff86b743 192 // FUNCTION:
j_j205 0:999bb8fcd0b2 193 // scan()
j_j205 0:999bb8fcd0b2 194 // IN-PARAMETERS:
j_j205 2:b281034eda86 195 // None
j_j205 0:999bb8fcd0b2 196 // OUT-PARAMETERS:
j_j205 2:b281034eda86 197 // None
j_j205 0:999bb8fcd0b2 198 // DESCRIPTION:
j_j205 2:b281034eda86 199 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 200 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 201 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 202 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 203 void Scanner::scan()
j_j205 0:999bb8fcd0b2 204 {
j_j205 2:b281034eda86 205 if (pitEnable == 1)
j_j205 2:b281034eda86 206 {
j_j205 6:5e24ff86b743 207 /*// obtain distance measures from sensors
j_j205 5:4d5601a9dffe 208 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 209 distRight = longRangeR.distInchesR();
j_j205 5:4d5601a9dffe 210
j_j205 6:5e24ff86b743 211 //check localize flags and make necessary method calls
j_j205 6:5e24ff86b743 212
j_j205 3:992558a021d7 213 // check avoid flag
j_j205 5:4d5601a9dffe 214 if (avoidFlag == 1)
j_j205 5:4d5601a9dffe 215 {
j_j205 5:4d5601a9dffe 216 // add obstacle avoid code
j_j205 5:4d5601a9dffe 217 }
j_j205 5:4d5601a9dffe 218
j_j205 3:992558a021d7 219 // check hunt flag
j_j205 6:5e24ff86b743 220 if (huntFlag == 1)
j_j205 5:4d5601a9dffe 221 {
j_j205 5:4d5601a9dffe 222 // add hunt code
j_j205 6:5e24ff86b743 223 }*/
j_j205 3:992558a021d7 224
j_j205 3:992558a021d7 225 // increment/decrement servo position
j_j205 2:b281034eda86 226 dutyL += invertL;
j_j205 2:b281034eda86 227 dutyR += invertR;
j_j205 2:b281034eda86 228 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 229 servoR.write(DUTY[dutyR]);
j_j205 6:5e24ff86b743 230 if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
j_j205 2:b281034eda86 231 ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
j_j205 2:b281034eda86 232 {
j_j205 2:b281034eda86 233 invertL *= -1;
j_j205 2:b281034eda86 234 invertR *= -1;
j_j205 2:b281034eda86 235 }
j_j205 2:b281034eda86 236 }
j_j205 3:992558a021d7 237 }