Tan Pham / Mbed 2 deprecated binbot

VNG bot battle

Dependencies:   BLE_API mbed nRF51822

SensorController.cpp@0:122d7bf3bbf0, 2016-09-05 (annotated)

Committer:
bintech91
Date:
Mon Sep 05 19:55:23 2016 +0000
Revision:
0:122d7bf3bbf0
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bintech91 0:122d7bf3bbf0 1 /*
bintech91 0:122d7bf3bbf0 2 * SensorController.cpp
bintech91 0:122d7bf3bbf0 3 *
bintech91 0:122d7bf3bbf0 4 * Created on: Aug 26, 2016
bintech91 0:122d7bf3bbf0 5 * Author: tanpt
bintech91 0:122d7bf3bbf0 6 */
bintech91 0:122d7bf3bbf0 7
bintech91 0:122d7bf3bbf0 8 #include "SensorController.h"
bintech91 0:122d7bf3bbf0 9
bintech91 0:122d7bf3bbf0 10 AnalogIn SensorController::sensorDistanceFront_(p1);
bintech91 0:122d7bf3bbf0 11 DigitalIn SensorController::sensorFrontRight_(p2);
bintech91 0:122d7bf3bbf0 12 DigitalIn SensorController::sensorFrontLeft_(p3);
bintech91 0:122d7bf3bbf0 13 DigitalIn SensorController::sensorBack_(p4);
bintech91 0:122d7bf3bbf0 14 AnalogIn SensorController::sensorDistanceRight_(p5);
bintech91 0:122d7bf3bbf0 15 AnalogIn SensorController::sensorDistanceLeft_(p6);
bintech91 0:122d7bf3bbf0 16
bintech91 0:122d7bf3bbf0 17 SensorController::SensorController() {
bintech91 0:122d7bf3bbf0 18
bintech91 0:122d7bf3bbf0 19 }
bintech91 0:122d7bf3bbf0 20
bintech91 0:122d7bf3bbf0 21 SensorController::~SensorController() {
bintech91 0:122d7bf3bbf0 22
bintech91 0:122d7bf3bbf0 23 }
bintech91 0:122d7bf3bbf0 24
bintech91 0:122d7bf3bbf0 25 void SensorController::init() {
bintech91 0:122d7bf3bbf0 26 sensorFrontLeft_.mode(PullUp);
bintech91 0:122d7bf3bbf0 27 sensorFrontRight_.mode(PullUp);
bintech91 0:122d7bf3bbf0 28 sensorBack_.mode(PullUp);
bintech91 0:122d7bf3bbf0 29 sensorIR_.mode(PullUp);
bintech91 0:122d7bf3bbf0 30 }
bintech91 0:122d7bf3bbf0 31 uint16_t SensorController::distanceSensorRight() {
bintech91 0:122d7bf3bbf0 32 uint16_t adc;
bintech91 0:122d7bf3bbf0 33 uint16_t cm;
bintech91 0:122d7bf3bbf0 34 uint16_t sensor = 0, i;
bintech91 0:122d7bf3bbf0 35
bintech91 0:122d7bf3bbf0 36 for (i = 0; i < SENSOR_NOISE; i++) {
bintech91 0:122d7bf3bbf0 37 adc = sensorFrontRight_.read_u16();
bintech91 0:122d7bf3bbf0 38 adc = 750 - adc;
bintech91 0:122d7bf3bbf0 39 if (adc > 60000)
bintech91 0:122d7bf3bbf0 40 cm = 1;
bintech91 0:122d7bf3bbf0 41 else if (adc > 600)
bintech91 0:122d7bf3bbf0 42 cm = 0;
bintech91 0:122d7bf3bbf0 43 else if (adc > 550)
bintech91 0:122d7bf3bbf0 44 cm = adc / 8;
bintech91 0:122d7bf3bbf0 45 else if (adc > 500)
bintech91 0:122d7bf3bbf0 46 cm = adc / 10;
bintech91 0:122d7bf3bbf0 47 else if (adc > 450)
bintech91 0:122d7bf3bbf0 48 cm = adc / 12;
bintech91 0:122d7bf3bbf0 49 else if (adc > 400)
bintech91 0:122d7bf3bbf0 50 cm = adc / 14;
bintech91 0:122d7bf3bbf0 51 else if (adc > 350)
bintech91 0:122d7bf3bbf0 52 cm = adc / 16;
bintech91 0:122d7bf3bbf0 53 else if (adc > 300)
bintech91 0:122d7bf3bbf0 54 cm = adc / 18;
bintech91 0:122d7bf3bbf0 55 else if (adc > 200)
bintech91 0:122d7bf3bbf0 56 cm = adc / 16;
bintech91 0:122d7bf3bbf0 57 else if (adc > 200)
bintech91 0:122d7bf3bbf0 58 cm = adc / 14;
bintech91 0:122d7bf3bbf0 59 else if (adc > 150)
bintech91 0:122d7bf3bbf0 60 cm = adc / 12;
bintech91 0:122d7bf3bbf0 61 else if (adc > 100)
bintech91 0:122d7bf3bbf0 62 cm = adc / 10;
bintech91 0:122d7bf3bbf0 63 else if (adc > 60)
bintech91 0:122d7bf3bbf0 64 cm = adc / 9;
bintech91 0:122d7bf3bbf0 65 else if (adc > 30)
bintech91 0:122d7bf3bbf0 66 cm = adc / 8;
bintech91 0:122d7bf3bbf0 67 else if (adc > 0)
bintech91 0:122d7bf3bbf0 68 cm = adc / 7;
bintech91 0:122d7bf3bbf0 69
bintech91 0:122d7bf3bbf0 70 wait(0.001);
bintech91 0:122d7bf3bbf0 71 sensor = sensor + cm;
bintech91 0:122d7bf3bbf0 72 if (cm == 0)
bintech91 0:122d7bf3bbf0 73 break;
bintech91 0:122d7bf3bbf0 74 cm = sensor / SENSOR_NOISE;
bintech91 0:122d7bf3bbf0 75 }
bintech91 0:122d7bf3bbf0 76 return cm;
bintech91 0:122d7bf3bbf0 77 }
bintech91 0:122d7bf3bbf0 78
bintech91 0:122d7bf3bbf0 79 uint16_t SensorController::distanceSensorLeft() {
bintech91 0:122d7bf3bbf0 80 uint16_t adc;
bintech91 0:122d7bf3bbf0 81 uint16_t cm;
bintech91 0:122d7bf3bbf0 82 uint16_t sensor = 0, i;
bintech91 0:122d7bf3bbf0 83
bintech91 0:122d7bf3bbf0 84 for (i = 0; i < SENSOR_NOISE; i++) {
bintech91 0:122d7bf3bbf0 85 adc = sensorDistanceLeft_.read_u16();
bintech91 0:122d7bf3bbf0 86 adc = 750 - adc;
bintech91 0:122d7bf3bbf0 87 if (adc > 60000)
bintech91 0:122d7bf3bbf0 88 cm = 1;
bintech91 0:122d7bf3bbf0 89 else if (adc > 600)
bintech91 0:122d7bf3bbf0 90 cm = 0;
bintech91 0:122d7bf3bbf0 91 else if (adc > 550)
bintech91 0:122d7bf3bbf0 92 cm = adc / 8;
bintech91 0:122d7bf3bbf0 93 else if (adc > 500)
bintech91 0:122d7bf3bbf0 94 cm = adc / 10;
bintech91 0:122d7bf3bbf0 95 else if (adc > 450)
bintech91 0:122d7bf3bbf0 96 cm = adc / 12;
bintech91 0:122d7bf3bbf0 97 else if (adc > 400)
bintech91 0:122d7bf3bbf0 98 cm = adc / 14;
bintech91 0:122d7bf3bbf0 99 else if (adc > 350)
bintech91 0:122d7bf3bbf0 100 cm = adc / 16;
bintech91 0:122d7bf3bbf0 101 else if (adc > 300)
bintech91 0:122d7bf3bbf0 102 cm = adc / 18;
bintech91 0:122d7bf3bbf0 103 else if (adc > 200)
bintech91 0:122d7bf3bbf0 104 cm = adc / 16;
bintech91 0:122d7bf3bbf0 105 else if (adc > 200)
bintech91 0:122d7bf3bbf0 106 cm = adc / 14;
bintech91 0:122d7bf3bbf0 107 else if (adc > 150)
bintech91 0:122d7bf3bbf0 108 cm = adc / 12;
bintech91 0:122d7bf3bbf0 109 else if (adc > 100)
bintech91 0:122d7bf3bbf0 110 cm = adc / 10;
bintech91 0:122d7bf3bbf0 111 else if (adc > 60)
bintech91 0:122d7bf3bbf0 112 cm = adc / 9;
bintech91 0:122d7bf3bbf0 113 else if (adc > 30)
bintech91 0:122d7bf3bbf0 114 cm = adc / 8;
bintech91 0:122d7bf3bbf0 115 else if (adc > 0)
bintech91 0:122d7bf3bbf0 116 cm = adc / 7;
bintech91 0:122d7bf3bbf0 117
bintech91 0:122d7bf3bbf0 118 wait(0.001);
bintech91 0:122d7bf3bbf0 119 sensor = sensor + cm;
bintech91 0:122d7bf3bbf0 120 if (cm == 0)
bintech91 0:122d7bf3bbf0 121 break;
bintech91 0:122d7bf3bbf0 122 cm = sensor / SENSOR_NOISE;
bintech91 0:122d7bf3bbf0 123 }
bintech91 0:122d7bf3bbf0 124 return cm;
bintech91 0:122d7bf3bbf0 125 }
bintech91 0:122d7bf3bbf0 126 uint16_t SensorController::distanceSensorFront() {
bintech91 0:122d7bf3bbf0 127 uint16_t adc;
bintech91 0:122d7bf3bbf0 128 uint16_t cm;
bintech91 0:122d7bf3bbf0 129 uint16_t sensor = 0, i;
bintech91 0:122d7bf3bbf0 130
bintech91 0:122d7bf3bbf0 131 for (i = 0; i < SENSOR_NOISE; i++) {
bintech91 0:122d7bf3bbf0 132 adc = sensorDistanceFront_.read_u16();
bintech91 0:122d7bf3bbf0 133 adc = 750 - adc;
bintech91 0:122d7bf3bbf0 134 if (adc > 60000)
bintech91 0:122d7bf3bbf0 135 cm = 1;
bintech91 0:122d7bf3bbf0 136 else if (adc > 600)
bintech91 0:122d7bf3bbf0 137 cm = 0;
bintech91 0:122d7bf3bbf0 138 else if (adc > 550)
bintech91 0:122d7bf3bbf0 139 cm = adc / 8;
bintech91 0:122d7bf3bbf0 140 else if (adc > 500)
bintech91 0:122d7bf3bbf0 141 cm = adc / 10;
bintech91 0:122d7bf3bbf0 142 else if (adc > 450)
bintech91 0:122d7bf3bbf0 143 cm = adc / 12;
bintech91 0:122d7bf3bbf0 144 else if (adc > 400)
bintech91 0:122d7bf3bbf0 145 cm = adc / 14;
bintech91 0:122d7bf3bbf0 146 else if (adc > 350)
bintech91 0:122d7bf3bbf0 147 cm = adc / 16;
bintech91 0:122d7bf3bbf0 148 else if (adc > 300)
bintech91 0:122d7bf3bbf0 149 cm = adc / 18;
bintech91 0:122d7bf3bbf0 150 else if (adc > 200)
bintech91 0:122d7bf3bbf0 151 cm = adc / 16;
bintech91 0:122d7bf3bbf0 152 else if (adc > 200)
bintech91 0:122d7bf3bbf0 153 cm = adc / 14;
bintech91 0:122d7bf3bbf0 154 else if (adc > 150)
bintech91 0:122d7bf3bbf0 155 cm = adc / 12;
bintech91 0:122d7bf3bbf0 156 else if (adc > 100)
bintech91 0:122d7bf3bbf0 157 cm = adc / 10;
bintech91 0:122d7bf3bbf0 158 else if (adc > 60)
bintech91 0:122d7bf3bbf0 159 cm = adc / 9;
bintech91 0:122d7bf3bbf0 160 else if (adc > 30)
bintech91 0:122d7bf3bbf0 161 cm = adc / 8;
bintech91 0:122d7bf3bbf0 162 else if (adc > 0)
bintech91 0:122d7bf3bbf0 163 cm = adc / 7;
bintech91 0:122d7bf3bbf0 164
bintech91 0:122d7bf3bbf0 165 wait(0.001);
bintech91 0:122d7bf3bbf0 166 sensor = sensor + cm;
bintech91 0:122d7bf3bbf0 167 if (cm == 0)
bintech91 0:122d7bf3bbf0 168 break;
bintech91 0:122d7bf3bbf0 169 cm = sensor / SENSOR_NOISE;
bintech91 0:122d7bf3bbf0 170 }
bintech91 0:122d7bf3bbf0 171 return cm;
bintech91 0:122d7bf3bbf0 172 }
bintech91 0:122d7bf3bbf0 173
bintech91 0:122d7bf3bbf0 174 uint8_t SensorController::frontRightSensor() {
bintech91 0:122d7bf3bbf0 175 uint16_t i, sensor = 0;
bintech91 0:122d7bf3bbf0 176
bintech91 0:122d7bf3bbf0 177 for (i = 0; i < SENSOR_NOISE; i++) {
bintech91 0:122d7bf3bbf0 178 wait(0.001);
bintech91 0:122d7bf3bbf0 179 sensor = sensor + sensorFrontRight_;
bintech91 0:122d7bf3bbf0 180 }
bintech91 0:122d7bf3bbf0 181 if (sensor > SENSOR_NOISE / 2)
bintech91 0:122d7bf3bbf0 182 return SENSOR_OFF;
bintech91 0:122d7bf3bbf0 183 else
bintech91 0:122d7bf3bbf0 184 return SENSOR_ON;
bintech91 0:122d7bf3bbf0 185 }
bintech91 0:122d7bf3bbf0 186 uint8_t SensorController::frontLeftSensor() {
bintech91 0:122d7bf3bbf0 187 uint16_t i, sensor = 0;
bintech91 0:122d7bf3bbf0 188
bintech91 0:122d7bf3bbf0 189 for (i = 0; i < SENSOR_NOISE; i++) {
bintech91 0:122d7bf3bbf0 190 wait(0.001);
bintech91 0:122d7bf3bbf0 191 sensor = sensor + sensorFrontLeft_;
bintech91 0:122d7bf3bbf0 192 }
bintech91 0:122d7bf3bbf0 193 if (sensor > SENSOR_NOISE / 2)
bintech91 0:122d7bf3bbf0 194 return SENSOR_OFF;
bintech91 0:122d7bf3bbf0 195 else
bintech91 0:122d7bf3bbf0 196 return SENSOR_ON;
bintech91 0:122d7bf3bbf0 197 }
bintech91 0:122d7bf3bbf0 198 uint8_t SensorController::backSensor() {
bintech91 0:122d7bf3bbf0 199 uint16_t i, sensor = 0;
bintech91 0:122d7bf3bbf0 200
bintech91 0:122d7bf3bbf0 201 for (i = 0; i < SENSOR_NOISE; i++) {
bintech91 0:122d7bf3bbf0 202 wait(0.001);
bintech91 0:122d7bf3bbf0 203 sensor = sensor + sensorBack_;
bintech91 0:122d7bf3bbf0 204 }
bintech91 0:122d7bf3bbf0 205 if (sensor > SENSOR_NOISE / 2)
bintech91 0:122d7bf3bbf0 206 return SENSOR_OFF;
bintech91 0:122d7bf3bbf0 207 else
bintech91 0:122d7bf3bbf0 208 return SENSOR_ON;
bintech91 0:122d7bf3bbf0 209 }