VNG bot battle
Dependencies: BLE_API mbed nRF51822
Diff: SensorController.cpp
- Revision:
- 0:122d7bf3bbf0
diff -r 000000000000 -r 122d7bf3bbf0 SensorController.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SensorController.cpp Mon Sep 05 19:55:23 2016 +0000 @@ -0,0 +1,209 @@ +/* + * SensorController.cpp + * + * Created on: Aug 26, 2016 + * Author: tanpt + */ + +#include "SensorController.h" + +AnalogIn SensorController::sensorDistanceFront_(p1); +DigitalIn SensorController::sensorFrontRight_(p2); +DigitalIn SensorController::sensorFrontLeft_(p3); +DigitalIn SensorController::sensorBack_(p4); +AnalogIn SensorController::sensorDistanceRight_(p5); +AnalogIn SensorController::sensorDistanceLeft_(p6); + +SensorController::SensorController() { + +} + +SensorController::~SensorController() { + +} + +void SensorController::init() { + sensorFrontLeft_.mode(PullUp); + sensorFrontRight_.mode(PullUp); + sensorBack_.mode(PullUp); + sensorIR_.mode(PullUp); +} +uint16_t SensorController::distanceSensorRight() { + uint16_t adc; + uint16_t cm; + uint16_t sensor = 0, i; + + for (i = 0; i < SENSOR_NOISE; i++) { + adc = sensorFrontRight_.read_u16(); + adc = 750 - adc; + if (adc > 60000) + cm = 1; + else if (adc > 600) + cm = 0; + else if (adc > 550) + cm = adc / 8; + else if (adc > 500) + cm = adc / 10; + else if (adc > 450) + cm = adc / 12; + else if (adc > 400) + cm = adc / 14; + else if (adc > 350) + cm = adc / 16; + else if (adc > 300) + cm = adc / 18; + else if (adc > 200) + cm = adc / 16; + else if (adc > 200) + cm = adc / 14; + else if (adc > 150) + cm = adc / 12; + else if (adc > 100) + cm = adc / 10; + else if (adc > 60) + cm = adc / 9; + else if (adc > 30) + cm = adc / 8; + else if (adc > 0) + cm = adc / 7; + + wait(0.001); + sensor = sensor + cm; + if (cm == 0) + break; + cm = sensor / SENSOR_NOISE; + } + return cm; +} + +uint16_t SensorController::distanceSensorLeft() { + uint16_t adc; + uint16_t cm; + uint16_t sensor = 0, i; + + for (i = 0; i < SENSOR_NOISE; i++) { + adc = sensorDistanceLeft_.read_u16(); + adc = 750 - adc; + if (adc > 60000) + cm = 1; + else if (adc > 600) + cm = 0; + else if (adc > 550) + cm = adc / 8; + else if (adc > 500) + cm = adc / 10; + else if (adc > 450) + cm = adc / 12; + else if (adc > 400) + cm = adc / 14; + else if (adc > 350) + cm = adc / 16; + else if (adc > 300) + cm = adc / 18; + else if (adc > 200) + cm = adc / 16; + else if (adc > 200) + cm = adc / 14; + else if (adc > 150) + cm = adc / 12; + else if (adc > 100) + cm = adc / 10; + else if (adc > 60) + cm = adc / 9; + else if (adc > 30) + cm = adc / 8; + else if (adc > 0) + cm = adc / 7; + + wait(0.001); + sensor = sensor + cm; + if (cm == 0) + break; + cm = sensor / SENSOR_NOISE; + } + return cm; +} +uint16_t SensorController::distanceSensorFront() { + uint16_t adc; + uint16_t cm; + uint16_t sensor = 0, i; + + for (i = 0; i < SENSOR_NOISE; i++) { + adc = sensorDistanceFront_.read_u16(); + adc = 750 - adc; + if (adc > 60000) + cm = 1; + else if (adc > 600) + cm = 0; + else if (adc > 550) + cm = adc / 8; + else if (adc > 500) + cm = adc / 10; + else if (adc > 450) + cm = adc / 12; + else if (adc > 400) + cm = adc / 14; + else if (adc > 350) + cm = adc / 16; + else if (adc > 300) + cm = adc / 18; + else if (adc > 200) + cm = adc / 16; + else if (adc > 200) + cm = adc / 14; + else if (adc > 150) + cm = adc / 12; + else if (adc > 100) + cm = adc / 10; + else if (adc > 60) + cm = adc / 9; + else if (adc > 30) + cm = adc / 8; + else if (adc > 0) + cm = adc / 7; + + wait(0.001); + sensor = sensor + cm; + if (cm == 0) + break; + cm = sensor / SENSOR_NOISE; + } + return cm; +} + +uint8_t SensorController::frontRightSensor() { + uint16_t i, sensor = 0; + + for (i = 0; i < SENSOR_NOISE; i++) { + wait(0.001); + sensor = sensor + sensorFrontRight_; + } + if (sensor > SENSOR_NOISE / 2) + return SENSOR_OFF; + else + return SENSOR_ON; +} +uint8_t SensorController::frontLeftSensor() { + uint16_t i, sensor = 0; + + for (i = 0; i < SENSOR_NOISE; i++) { + wait(0.001); + sensor = sensor + sensorFrontLeft_; + } + if (sensor > SENSOR_NOISE / 2) + return SENSOR_OFF; + else + return SENSOR_ON; +} +uint8_t SensorController::backSensor() { + uint16_t i, sensor = 0; + + for (i = 0; i < SENSOR_NOISE; i++) { + wait(0.001); + sensor = sensor + sensorBack_; + } + if (sensor > SENSOR_NOISE / 2) + return SENSOR_OFF; + else + return SENSOR_ON; +}