Justin Jordan / scanner

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

scanner.cpp@2:b281034eda86, 2016-02-23 (annotated)

Committer:
j_j205
Date:
Tue Feb 23 23:35:41 2016 +0000
Revision:
2:b281034eda86
Parent:
0:999bb8fcd0b2
Child:
3:992558a021d7
2/23/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 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 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 0:999bb8fcd0b2 17 VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
j_j205 2:b281034eda86 18 Gp2x &_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 2:b281034eda86 26 for (int i = 0; i < 13; i++)
j_j205 2:b281034eda86 27 DUTY[i] = 0.0300 + (i * 0.0075); // for radioshack servo
j_j205 2:b281034eda86 28
j_j205 2:b281034eda86 29 /*for (int i = 0; i < 13; i++)
j_j205 2:b281034eda86 30 DUTY[i] = 0.0500 + (i * DELTA_DUTY); // 0.05 = min duty cycle
j_j205 2:b281034eda86 31 // {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708,
j_j205 2:b281034eda86 32 // 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958,
j_j205 2:b281034eda86 33 // 0.1000};*/
j_j205 2:b281034eda86 34
j_j205 2:b281034eda86 35 // initializing to avoid status
j_j205 2:b281034eda86 36 invertL = 1;
j_j205 2:b281034eda86 37 invertR = -1;
j_j205 2:b281034eda86 38 dutyL = MIN_DUTY;
j_j205 2:b281034eda86 39 dutyR = MAX_DUTY;
j_j205 2:b281034eda86 40 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 41 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 42 wait(0.2);
j_j205 2:b281034eda86 43 pitEnable = 1;
j_j205 2:b281034eda86 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 2:b281034eda86 50 // None
j_j205 0:999bb8fcd0b2 51 // OUT-PARAMETERS:
j_j205 2:b281034eda86 52 // None
j_j205 0:999bb8fcd0b2 53 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 54 // Hunts for victim.
j_j205 2:b281034eda86 55 void Scanner::hunt()
j_j205 0:999bb8fcd0b2 56 {
j_j205 2:b281034eda86 57 pitEnable = 0;
j_j205 2:b281034eda86 58 invertL = 1;
j_j205 2:b281034eda86 59 invertR = -1;
j_j205 2:b281034eda86 60 dutyL = HALF_DUTY;
j_j205 2:b281034eda86 61 dutyR = HALF_DUTY;
j_j205 2:b281034eda86 62 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 63 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 64 wait(0.1);
j_j205 2:b281034eda86 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 2:b281034eda86 71 // None
j_j205 0:999bb8fcd0b2 72 // OUT-PARAMETERS:
j_j205 2:b281034eda86 73 // None
j_j205 0:999bb8fcd0b2 74 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 75 // Scans to avoid obstacles.
j_j205 2:b281034eda86 76 void Scanner::avoid()
j_j205 0:999bb8fcd0b2 77 {
j_j205 2:b281034eda86 78 pitEnable = 0;
j_j205 2:b281034eda86 79 invertL = 1;
j_j205 2:b281034eda86 80 invertR = -1;
j_j205 2:b281034eda86 81 dutyL = MIN_DUTY;
j_j205 2:b281034eda86 82 dutyR = MAX_DUTY;
j_j205 2:b281034eda86 83 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 84 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 85 wait(0.1);
j_j205 2:b281034eda86 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 2:b281034eda86 92 // None
j_j205 0:999bb8fcd0b2 93 // OUT-PARAMETERS:
j_j205 2:b281034eda86 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 2:b281034eda86 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 2:b281034eda86 106 // None
j_j205 0:999bb8fcd0b2 107 // OUT-PARAMETERS:
j_j205 2:b281034eda86 108 // None
j_j205 0:999bb8fcd0b2 109 // DESCRIPTION:
j_j205 2:b281034eda86 110 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 111 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 112 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 113 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 114 void Scanner::scan()
j_j205 0:999bb8fcd0b2 115 {
j_j205 2:b281034eda86 116 if (pitEnable == 1)
j_j205 2:b281034eda86 117 {
j_j205 2:b281034eda86 118 // obtain distance measures from sensors
j_j205 2:b281034eda86 119 dutyL += invertL;
j_j205 2:b281034eda86 120 dutyR += invertR;
j_j205 2:b281034eda86 121 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 122 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 123
j_j205 2:b281034eda86 124 if(((dutyL == HALF_DUTY) && (dutyR == HALF_DUTY)) ||
j_j205 2:b281034eda86 125 ((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
j_j205 2:b281034eda86 126 ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
j_j205 2:b281034eda86 127 {
j_j205 2:b281034eda86 128 invertL *= -1;
j_j205 2:b281034eda86 129 invertR *= -1;
j_j205 2:b281034eda86 130 }
j_j205 2:b281034eda86 131 }
j_j205 0:999bb8fcd0b2 132 }