SensorController.cpp

00001 /*
00002  * SensorController.cpp
00003  *
00004  *  Created on: Aug 26, 2016
00005  *      Author: tanpt
00006  */
00007 
00008 #include  "SensorController.h"
00009 
00010 AnalogIn SensorController::sensorDistanceFront_(p1);
00011 DigitalIn SensorController::sensorFrontRight_(p2);
00012 DigitalIn SensorController::sensorFrontLeft_(p3);
00013 DigitalIn SensorController::sensorBack_(p4);
00014 AnalogIn SensorController::sensorDistanceRight_(p5);
00015 AnalogIn SensorController::sensorDistanceLeft_(p6);
00016 
00017 SensorController::SensorController() {
00018 
00019 }
00020 
00021 SensorController::~SensorController() {
00022 
00023 }
00024 
00025 void SensorController::init() {
00026     sensorFrontLeft_.mode(PullUp);
00027     sensorFrontRight_.mode(PullUp);
00028     sensorBack_.mode(PullUp);
00029     sensorIR_.mode(PullUp);
00030 }
00031 uint16_t SensorController::distanceSensorRight() {
00032     uint16_t adc;
00033     uint16_t cm;
00034     uint16_t sensor = 0, i;
00035 
00036     for (i = 0; i < SENSOR_NOISE; i++) {
00037         adc = sensorFrontRight_.read_u16();
00038         adc = 750 - adc;
00039         if (adc > 60000)
00040             cm = 1;
00041         else if (adc > 600)
00042             cm = 0;
00043         else if (adc > 550)
00044             cm = adc / 8;
00045         else if (adc > 500)
00046             cm = adc / 10;
00047         else if (adc > 450)
00048             cm = adc / 12;
00049         else if (adc > 400)
00050             cm = adc / 14;
00051         else if (adc > 350)
00052             cm = adc / 16;
00053         else if (adc > 300)
00054             cm = adc / 18;
00055         else if (adc > 200)
00056             cm = adc / 16;
00057         else if (adc > 200)
00058             cm = adc / 14;
00059         else if (adc > 150)
00060             cm = adc / 12;
00061         else if (adc > 100)
00062             cm = adc / 10;
00063         else if (adc > 60)
00064             cm = adc / 9;
00065         else if (adc > 30)
00066             cm = adc / 8;
00067         else if (adc > 0)
00068             cm = adc / 7;
00069 
00070         wait(0.001);
00071         sensor = sensor + cm;
00072         if (cm == 0)
00073             break;
00074         cm = sensor / SENSOR_NOISE;
00075     }
00076     return cm;
00077 }
00078 
00079 uint16_t SensorController::distanceSensorLeft() {
00080     uint16_t adc;
00081     uint16_t cm;
00082     uint16_t sensor = 0, i;
00083 
00084     for (i = 0; i < SENSOR_NOISE; i++) {
00085         adc = sensorDistanceLeft_.read_u16();
00086         adc = 750 - adc;
00087         if (adc > 60000)
00088             cm = 1;
00089         else if (adc > 600)
00090             cm = 0;
00091         else if (adc > 550)
00092             cm = adc / 8;
00093         else if (adc > 500)
00094             cm = adc / 10;
00095         else if (adc > 450)
00096             cm = adc / 12;
00097         else if (adc > 400)
00098             cm = adc / 14;
00099         else if (adc > 350)
00100             cm = adc / 16;
00101         else if (adc > 300)
00102             cm = adc / 18;
00103         else if (adc > 200)
00104             cm = adc / 16;
00105         else if (adc > 200)
00106             cm = adc / 14;
00107         else if (adc > 150)
00108             cm = adc / 12;
00109         else if (adc > 100)
00110             cm = adc / 10;
00111         else if (adc > 60)
00112             cm = adc / 9;
00113         else if (adc > 30)
00114             cm = adc / 8;
00115         else if (adc > 0)
00116             cm = adc / 7;
00117 
00118         wait(0.001);
00119         sensor = sensor + cm;
00120         if (cm == 0)
00121             break;
00122         cm = sensor / SENSOR_NOISE;
00123     }
00124     return cm;
00125 }
00126 uint16_t SensorController::distanceSensorFront() {
00127     uint16_t adc;
00128     uint16_t cm;
00129     uint16_t sensor = 0, i;
00130 
00131     for (i = 0; i < SENSOR_NOISE; i++) {
00132         adc = sensorDistanceFront_.read_u16();
00133         adc = 750 - adc;
00134         if (adc > 60000)
00135             cm = 1;
00136         else if (adc > 600)
00137             cm = 0;
00138         else if (adc > 550)
00139             cm = adc / 8;
00140         else if (adc > 500)
00141             cm = adc / 10;
00142         else if (adc > 450)
00143             cm = adc / 12;
00144         else if (adc > 400)
00145             cm = adc / 14;
00146         else if (adc > 350)
00147             cm = adc / 16;
00148         else if (adc > 300)
00149             cm = adc / 18;
00150         else if (adc > 200)
00151             cm = adc / 16;
00152         else if (adc > 200)
00153             cm = adc / 14;
00154         else if (adc > 150)
00155             cm = adc / 12;
00156         else if (adc > 100)
00157             cm = adc / 10;
00158         else if (adc > 60)
00159             cm = adc / 9;
00160         else if (adc > 30)
00161             cm = adc / 8;
00162         else if (adc > 0)
00163             cm = adc / 7;
00164 
00165         wait(0.001);
00166         sensor = sensor + cm;
00167         if (cm == 0)
00168             break;
00169         cm = sensor / SENSOR_NOISE;
00170     }
00171     return cm;
00172 }
00173 
00174 uint8_t SensorController::frontRightSensor() {
00175     uint16_t i, sensor = 0;
00176 
00177     for (i = 0; i < SENSOR_NOISE; i++) {
00178         wait(0.001);
00179         sensor = sensor + sensorFrontRight_;
00180     }
00181     if (sensor > SENSOR_NOISE / 2)
00182         return SENSOR_OFF;
00183     else
00184         return SENSOR_ON;
00185 }
00186 uint8_t SensorController::frontLeftSensor() {
00187     uint16_t i, sensor = 0;
00188 
00189     for (i = 0; i < SENSOR_NOISE; i++) {
00190         wait(0.001);
00191         sensor = sensor + sensorFrontLeft_;
00192     }
00193     if (sensor > SENSOR_NOISE / 2)
00194         return SENSOR_OFF;
00195     else
00196         return SENSOR_ON;
00197 }
00198 uint8_t SensorController::backSensor() {
00199     uint16_t i, sensor = 0;
00200 
00201     for (i = 0; i < SENSOR_NOISE; i++) {
00202         wait(0.001);
00203         sensor = sensor + sensorBack_;
00204     }
00205     if (sensor > SENSOR_NOISE / 2)
00206         return SENSOR_OFF;
00207     else
00208         return SENSOR_ON;
00209 }