Texas State IEEE / scanner

3/26/16 12:25 am JJ

Dependents:   steppertest R5 2016 Robotics Team 1

Fork of scanner by David Vasquez

scanner.cpp@5:4d5601a9dffe, 2016-03-22 (annotated)

Committer:
j_j205
Date:
Tue Mar 22 18:11:36 2016 +0000
Revision:
5:4d5601a9dffe
Parent:
4:6a8d80954323
Child:
6:5e24ff86b743
3/22/16 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 3:992558a021d7 26 float temp[13] = {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708,
j_j205 3:992558a021d7 27 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958,
j_j205 3:992558a021d7 28 0.1000};
j_j205 2:b281034eda86 29
j_j205 3:992558a021d7 30 for (int i = 0; i < 13; i++)
j_j205 3:992558a021d7 31 DUTY[i] = temp[i];
j_j205 3:992558a021d7 32 // 0.05 = min duty cycle
j_j205 2:b281034eda86 33 // {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708,
j_j205 2:b281034eda86 34 // 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958,
j_j205 3:992558a021d7 35 // 0.1000};
j_j205 3:992558a021d7 36
j_j205 3:992558a021d7 37 obstacle = 0;
j_j205 3:992558a021d7 38 distLeft = 0.0;
j_j205 3:992558a021d7 39 distRight = 0.0;
j_j205 3:992558a021d7 40 distForward = 0.0;
j_j205 2:b281034eda86 41
j_j205 2:b281034eda86 42 // initializing to avoid status
j_j205 3:992558a021d7 43 avoidFlag = 1;
j_j205 3:992558a021d7 44 huntFlag = 0;
j_j205 3:992558a021d7 45 invertL = -1;
j_j205 3:992558a021d7 46 invertR = 1;
j_j205 3:992558a021d7 47 dutyL = MAX_DUTY;
j_j205 3:992558a021d7 48 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 49 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 50 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 51 wait(0.2);
j_j205 2:b281034eda86 52 pitEnable = 1;
j_j205 3:992558a021d7 53 scanPit.attach(this, &Scanner::scan, period);
j_j205 0:999bb8fcd0b2 54 }
j_j205 0:999bb8fcd0b2 55
j_j205 0:999bb8fcd0b2 56 // FUNCTION:
j_j205 3:992558a021d7 57 // huntMode()
j_j205 0:999bb8fcd0b2 58 // IN-PARAMETERS:
j_j205 2:b281034eda86 59 // None
j_j205 2:b281034eda86 60 // None
j_j205 0:999bb8fcd0b2 61 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 62 // Hunts for victim.
j_j205 3:992558a021d7 63 void Scanner::huntMode()
j_j205 0:999bb8fcd0b2 64 {
j_j205 2:b281034eda86 65 pitEnable = 0;
j_j205 3:992558a021d7 66 avoidFlag = 0;
j_j205 3:992558a021d7 67 huntFlag = 1;
j_j205 3:992558a021d7 68 invertL = -1;
j_j205 3:992558a021d7 69 invertR = 1;
j_j205 2:b281034eda86 70 dutyL = HALF_DUTY;
j_j205 2:b281034eda86 71 dutyR = HALF_DUTY;
j_j205 2:b281034eda86 72 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 73 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 74 wait(0.1);
j_j205 2:b281034eda86 75 pitEnable = 1;
j_j205 0:999bb8fcd0b2 76 }
j_j205 0:999bb8fcd0b2 77
j_j205 0:999bb8fcd0b2 78 // FUNCTION:
j_j205 3:992558a021d7 79 // avoidMode()
j_j205 0:999bb8fcd0b2 80 // IN-PARAMETERS:
j_j205 2:b281034eda86 81 // None
j_j205 0:999bb8fcd0b2 82 // OUT-PARAMETERS:
j_j205 2:b281034eda86 83 // None
j_j205 0:999bb8fcd0b2 84 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 85 // Scans to avoid obstacles.
j_j205 3:992558a021d7 86 void Scanner::avoidMode()
j_j205 0:999bb8fcd0b2 87 {
j_j205 2:b281034eda86 88 pitEnable = 0;
j_j205 3:992558a021d7 89 avoidFlag = 1;
j_j205 3:992558a021d7 90 huntFlag = 0;
j_j205 3:992558a021d7 91 invertL = -1;
j_j205 3:992558a021d7 92 invertR = 1;
j_j205 3:992558a021d7 93 dutyL = MAX_DUTY;
j_j205 3:992558a021d7 94 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 95 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 96 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 97 wait(0.1);
j_j205 2:b281034eda86 98 pitEnable = 1;
j_j205 0:999bb8fcd0b2 99 }
j_j205 0:999bb8fcd0b2 100
j_j205 0:999bb8fcd0b2 101 // FUNCTION:
j_j205 0:999bb8fcd0b2 102 // localize()
j_j205 0:999bb8fcd0b2 103 // IN-PARAMETERS:
j_j205 2:b281034eda86 104 // None
j_j205 0:999bb8fcd0b2 105 // OUT-PARAMETERS:
j_j205 2:b281034eda86 106 // None
j_j205 0:999bb8fcd0b2 107 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 108 // Checks localization points.
j_j205 3:992558a021d7 109 void Scanner::localize()
j_j205 0:999bb8fcd0b2 110 {
j_j205 3:992558a021d7 111 pitEnable = 0;
j_j205 3:992558a021d7 112 dutyL = MIN_DUTY;
j_j205 3:992558a021d7 113 dutyR = MAX_DUTY;
j_j205 3:992558a021d7 114 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 115 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 116 wait(0.1);
j_j205 5:4d5601a9dffe 117 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 118 distRight = longRangeR.distInchesR();
j_j205 3:992558a021d7 119 dutyL = HALF_DUTY;
j_j205 3:992558a021d7 120 dutyR = HALF_DUTY;
j_j205 3:992558a021d7 121 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 122 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 123 wait(0.1);
j_j205 5:4d5601a9dffe 124 distForward = ( longRangeL.distInchesL() + longRangeR.distInchesR() )/2.0;
j_j205 3:992558a021d7 125 pitEnable = 1;
j_j205 0:999bb8fcd0b2 126 }
j_j205 0:999bb8fcd0b2 127
j_j205 0:999bb8fcd0b2 128 // FUNCTION:
j_j205 0:999bb8fcd0b2 129 // scan()
j_j205 0:999bb8fcd0b2 130 // IN-PARAMETERS:
j_j205 2:b281034eda86 131 // None
j_j205 0:999bb8fcd0b2 132 // OUT-PARAMETERS:
j_j205 2:b281034eda86 133 // None
j_j205 0:999bb8fcd0b2 134 // DESCRIPTION:
j_j205 2:b281034eda86 135 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 136 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 137 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 138 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 139 void Scanner::scan()
j_j205 0:999bb8fcd0b2 140 {
j_j205 2:b281034eda86 141 if (pitEnable == 1)
j_j205 2:b281034eda86 142 {
j_j205 2:b281034eda86 143 // obtain distance measures from sensors
j_j205 5:4d5601a9dffe 144 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 145 distRight = longRangeR.distInchesR();
j_j205 5:4d5601a9dffe 146
j_j205 3:992558a021d7 147 // check avoid flag
j_j205 5:4d5601a9dffe 148 if (avoidFlag == 1)
j_j205 5:4d5601a9dffe 149 {
j_j205 5:4d5601a9dffe 150 // add obstacle avoid code
j_j205 5:4d5601a9dffe 151 }
j_j205 5:4d5601a9dffe 152
j_j205 3:992558a021d7 153 // check hunt flag
j_j205 5:4d5601a9dffe 154 else if (huntFlag == 1)
j_j205 5:4d5601a9dffe 155 {
j_j205 5:4d5601a9dffe 156 // add hunt code
j_j205 5:4d5601a9dffe 157 }
j_j205 3:992558a021d7 158
j_j205 3:992558a021d7 159 // increment/decrement servo position
j_j205 2:b281034eda86 160 dutyL += invertL;
j_j205 2:b281034eda86 161 dutyR += invertR;
j_j205 2:b281034eda86 162 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 163 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 164 if(((dutyL == HALF_DUTY) && (dutyR == HALF_DUTY)) ||
j_j205 2:b281034eda86 165 ((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
j_j205 2:b281034eda86 166 ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
j_j205 2:b281034eda86 167 {
j_j205 2:b281034eda86 168 invertL *= -1;
j_j205 2:b281034eda86 169 invertR *= -1;
j_j205 2:b281034eda86 170 }
j_j205 2:b281034eda86 171 }
j_j205 3:992558a021d7 172 }