Justin Jordan / scanner

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?

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 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 }