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@16:4e76b53cdef2, 2016-04-01 (annotated)

Committer:
j_j205
Date:
Fri Apr 01 22:21:27 2016 +0000
Revision:
16:4e76b53cdef2
Parent:
15:3967cb2d93f5 
stepper test 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"
Hellkite85 7:52c4be3bfc1b 4 #include "StepperDrive.h"
Hellkite85 7:52c4be3bfc1b 5 #include <stack>
j_j205 0:999bb8fcd0b2 6
j_j205 0:999bb8fcd0b2 7 // FUNCTION:
j_j205 0:999bb8fcd0b2 8 // Scanner()
j_j205 0:999bb8fcd0b2 9 // IN-PARAMETERS:
Hellkite85 7:52c4be3bfc1b 10 // Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
j_j205 2:b281034eda86 11 // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
j_j205 2:b281034eda86 12 // Gp2x &_longRangeR, float _period = 10e-3
j_j205 0:999bb8fcd0b2 13 // OUT-PARAMETERS:
j_j205 2:b281034eda86 14 // None
j_j205 0:999bb8fcd0b2 15 // DESCRIPTION:
j_j205 2:b281034eda86 16 // Constructor.
Hellkite85 7:52c4be3bfc1b 17 Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
j_j205 16:4e76b53cdef2 18 LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, float _period) : pc(pc1),
j_j205 16:4e76b53cdef2 19 drive(_drive), servoL(_servoL), servoR(_servoR), longRangeL(_longRangeL),
j_j205 16:4e76b53cdef2 20 longRangeR(_longRangeR), period(_period)
j_j205 0:999bb8fcd0b2 21 {
j_j205 0:999bb8fcd0b2 22 servoL.period(1.0/50.0);
j_j205 0:999bb8fcd0b2 23 servoR.period(1.0/50.0);
j_j205 2:b281034eda86 24
j_j205 6:5e24ff86b743 25 float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925,
j_j205 6:5e24ff86b743 26 0.1013, 0.1100}; // sub-micro
j_j205 2:b281034eda86 27
j_j205 6:5e24ff86b743 28 for (int i = 0; i < 7; i++)
j_j205 3:992558a021d7 29 DUTY[i] = temp[i];
j_j205 3:992558a021d7 30
j_j205 3:992558a021d7 31 obstacle = 0;
j_j205 3:992558a021d7 32 distLeft = 0.0;
j_j205 3:992558a021d7 33 distRight = 0.0;
j_j205 6:5e24ff86b743 34 distForwardL = 0.0;
j_j205 6:5e24ff86b743 35 distForwardR = 0.0;
j_j205 2:b281034eda86 36
j_j205 14:d327852dafd1 37 // initializing to localizeRight status
j_j205 14:d327852dafd1 38 avoidFlag = 0;
j_j205 3:992558a021d7 39 huntFlag = 0;
j_j205 16:4e76b53cdef2 40 localizeRightFlag = 0;
j_j205 14:d327852dafd1 41 localizeLeftFlag = 0;
j_j205 3:992558a021d7 42 invertL = -1;
j_j205 3:992558a021d7 43 invertR = 1;
j_j205 14:d327852dafd1 44 dutyL = MIN_DUTY;
j_j205 3:992558a021d7 45 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 46 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 47 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 48 wait(0.2);
j_j205 2:b281034eda86 49 pitEnable = 1;
j_j205 3:992558a021d7 50 scanPit.attach(this, &Scanner::scan, period);
j_j205 6:5e24ff86b743 51 pc.printf("Scanner created\n");
j_j205 0:999bb8fcd0b2 52 }
j_j205 0:999bb8fcd0b2 53
j_j205 0:999bb8fcd0b2 54 // FUNCTION:
j_j205 14:d327852dafd1 55 // void localizeLeftMode()
j_j205 14:d327852dafd1 56 // IN-PARAMETERS:
j_j205 14:d327852dafd1 57 // None
j_j205 14:d327852dafd1 58 // OUT-PARAMETERS:
j_j205 14:d327852dafd1 59 // None
j_j205 14:d327852dafd1 60 // DESCRIPTION:
j_j205 14:d327852dafd1 61 // This method initializes to localizeLeft()
j_j205 14:d327852dafd1 62 void Scanner::localizeLeftMode()
j_j205 14:d327852dafd1 63 {
j_j205 14:d327852dafd1 64 localizeRightFlag = 0;
j_j205 14:d327852dafd1 65
j_j205 14:d327852dafd1 66 dutyL = MAX_DUTY;
j_j205 14:d327852dafd1 67 dutyR = MAX_DUTY;
j_j205 14:d327852dafd1 68 servoL.write(DUTY[dutyL]);
j_j205 14:d327852dafd1 69 servoR.write(DUTY[dutyR]);
j_j205 14:d327852dafd1 70 wait(0.2);
j_j205 14:d327852dafd1 71
j_j205 14:d327852dafd1 72 localizeLeftFlag = 1;
j_j205 14:d327852dafd1 73 }
j_j205 14:d327852dafd1 74
j_j205 14:d327852dafd1 75 // FUNCTION:
j_j205 6:5e24ff86b743 76 // void localizeLeft()
j_j205 6:5e24ff86b743 77 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 78 // None
j_j205 6:5e24ff86b743 79 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 80 // None
j_j205 6:5e24ff86b743 81 // DESCRIPTION:
j_j205 6:5e24ff86b743 82 // Using sensor longRangeR this method will localize to the wall
j_j205 6:5e24ff86b743 83 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 84 // should remain xxx distance from the left wall.
j_j205 6:5e24ff86b743 85 void Scanner::localizeLeft()
Hellkite85 7:52c4be3bfc1b 86 {
j_j205 16:4e76b53cdef2 87 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 88 float distLocalR = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 89
j_j205 16:4e76b53cdef2 90 if (distLocalR >= 9.2)
Hellkite85 7:52c4be3bfc1b 91 {
Hellkite85 7:52c4be3bfc1b 92 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 93 drive.move(0.0, ((-5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 94 // wait for move to complete
j_j205 14:d327852dafd1 95 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 96 {
j_j205 14:d327852dafd1 97 wait(1e-6);
j_j205 14:d327852dafd1 98 }
Hellkite85 7:52c4be3bfc1b 99 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 100 }
j_j205 16:4e76b53cdef2 101 else if (distLocalR <= 8.2)
Hellkite85 7:52c4be3bfc1b 102 {
Hellkite85 7:52c4be3bfc1b 103 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 104 drive.move(0.0, ((5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 105 // wait for move to complete
j_j205 14:d327852dafd1 106 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 107 {
j_j205 14:d327852dafd1 108 wait(1e-6);
j_j205 14:d327852dafd1 109 }
Hellkite85 7:52c4be3bfc1b 110 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 111 }
j_j205 16:4e76b53cdef2 112
j_j205 16:4e76b53cdef2 113 // wait for move to complete
j_j205 16:4e76b53cdef2 114 while(!drive.isMoveDone())
j_j205 16:4e76b53cdef2 115 {
j_j205 16:4e76b53cdef2 116 wait(1e-6);
j_j205 16:4e76b53cdef2 117 }
j_j205 16:4e76b53cdef2 118 pitEnable = 1;
j_j205 6:5e24ff86b743 119 }
j_j205 6:5e24ff86b743 120
j_j205 6:5e24ff86b743 121 // FUNCTION:
j_j205 14:d327852dafd1 122 // void localizeRightMode()
j_j205 14:d327852dafd1 123 // IN-PARAMETERS:
j_j205 14:d327852dafd1 124 // None
j_j205 14:d327852dafd1 125 // OUT-PARAMETERS:
j_j205 14:d327852dafd1 126 // None
j_j205 14:d327852dafd1 127 // DESCRIPTION:
j_j205 14:d327852dafd1 128 // Initializes to localizeRight()
Hellkite85 15:3967cb2d93f5 129 void Scanner::localizeRightMode()
j_j205 14:d327852dafd1 130 {
j_j205 14:d327852dafd1 131 localizeLeftFlag = 0;
j_j205 14:d327852dafd1 132
j_j205 14:d327852dafd1 133 dutyL = MIN_DUTY;
j_j205 14:d327852dafd1 134 dutyR = MIN_DUTY;
j_j205 14:d327852dafd1 135 servoL.write(DUTY[dutyL]);
j_j205 14:d327852dafd1 136 servoR.write(DUTY[dutyR]);
j_j205 14:d327852dafd1 137 wait(0.2);
j_j205 14:d327852dafd1 138
j_j205 14:d327852dafd1 139 localizeRightFlag = 1;
j_j205 14:d327852dafd1 140 }
j_j205 14:d327852dafd1 141
j_j205 14:d327852dafd1 142 // FUNCTION:
j_j205 6:5e24ff86b743 143 // void localizeRight()
j_j205 6:5e24ff86b743 144 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 145 // None
j_j205 6:5e24ff86b743 146 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 147 // None
j_j205 6:5e24ff86b743 148 // DESCRIPTION:
j_j205 6:5e24ff86b743 149 // Using sensor longRangeL this method will localize to the wall
j_j205 6:5e24ff86b743 150 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 151 // should remain xxx distance from the left wall. This function
j_j205 6:5e24ff86b743 152 // will be called from scan when the localizeRightFlag is set.
Hellkite85 7:52c4be3bfc1b 153 void Scanner::localizeRight()
j_j205 6:5e24ff86b743 154 {
j_j205 16:4e76b53cdef2 155 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 156 float distLocalL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 157
j_j205 16:4e76b53cdef2 158 if (distLocalL >= 9.2)
Hellkite85 7:52c4be3bfc1b 159 {
Hellkite85 7:52c4be3bfc1b 160 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 161 drive.move(0, ((5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 162 // wait for move to complete
j_j205 14:d327852dafd1 163 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 164 {
j_j205 14:d327852dafd1 165 wait(1e-6);
j_j205 14:d327852dafd1 166 }
Hellkite85 7:52c4be3bfc1b 167 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 168 }
j_j205 16:4e76b53cdef2 169 else if (distLocalL <= 8.2)
Hellkite85 7:52c4be3bfc1b 170 {
Hellkite85 7:52c4be3bfc1b 171 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 172 drive.move(0, ((-5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 173 // wait for move to complete
j_j205 14:d327852dafd1 174 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 175 {
j_j205 14:d327852dafd1 176 wait(1e-6);
j_j205 14:d327852dafd1 177 }
Hellkite85 7:52c4be3bfc1b 178 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 179 }
j_j205 16:4e76b53cdef2 180
j_j205 16:4e76b53cdef2 181 // wait for move to complete
j_j205 16:4e76b53cdef2 182 while(!drive.isMoveDone())
j_j205 16:4e76b53cdef2 183 {
j_j205 16:4e76b53cdef2 184 wait(1e-6);
j_j205 16:4e76b53cdef2 185 }
j_j205 16:4e76b53cdef2 186 pitEnable = 1;
j_j205 6:5e24ff86b743 187 }
j_j205 6:5e24ff86b743 188
j_j205 6:5e24ff86b743 189 // FUNCTION:
j_j205 0:999bb8fcd0b2 190 // scan()
j_j205 0:999bb8fcd0b2 191 // IN-PARAMETERS:
j_j205 2:b281034eda86 192 // None
j_j205 0:999bb8fcd0b2 193 // OUT-PARAMETERS:
j_j205 2:b281034eda86 194 // None
j_j205 0:999bb8fcd0b2 195 // DESCRIPTION:
j_j205 2:b281034eda86 196 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 197 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 198 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 199 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 200 void Scanner::scan()
j_j205 0:999bb8fcd0b2 201 {
j_j205 2:b281034eda86 202 if (pitEnable == 1)
j_j205 2:b281034eda86 203 {
j_j205 13:8767be1c5cc2 204 if (localizeRightFlag == 1)
j_j205 13:8767be1c5cc2 205 localizeRight();
j_j205 13:8767be1c5cc2 206
j_j205 14:d327852dafd1 207 /*if (localizeLeftFlag == 1)
j_j205 14:d327852dafd1 208 localizeLeft();*/
j_j205 2:b281034eda86 209 }
j_j205 3:992558a021d7 210 }