VNG bot battle
Dependencies: BLE_API mbed nRF51822
main.log
- Committer:
- bintech91
- Date:
- 2016-09-05
- Revision:
- 0:122d7bf3bbf0
File content as of revision 0:122d7bf3bbf0:
/* * main.cpp * * Created on: Aug 23, 2016 * Author: tanpt */ #include "common.h" #include "MotorController.h" //#include "ble/BLE.h" //#include "ble/services/iBeacon.h" //#include "ble/services/UARTService.h" MotorController motorController; //// Sensor #define NOISE 5 //#define DEBUG #ifdef DEBUG Serial pc(p10, p11); #endif Ticker ticker; AnalogIn analog_ir(p1); DigitalIn SEN_UP_R(p2); DigitalIn SEN_UP_L(p3); DigitalIn SEN_DOWN(p4); DigitalIn SEN_IR(p6); #define ON 0 #define OFF 1 #define VREF 3300.0 //mV uint16_t g_time_led = 0; ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////// ////////////////////////////////////////////////// /* * @param none. * @return Distance (cm), 1-80 cm. */ uint16_t sensor_ir(void) { uint16_t adc; uint16_t cm; uint16_t sensor = 0, i; for(i=0; i<NOISE; i++) { adc = analog_ir.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/NOISE; } #ifdef DEBUG pc.printf("\r\n %d adc, %d cm", adc, cm); #endif return cm; } ////////////////////////////////////////////////// /* * @param none. * @return ON or OFF. */ uint8_t sensor_ir2(void) { uint16_t i, sensor = 0; for(i=0; i<NOISE; i++) { wait(0.001); sensor = sensor + SEN_IR; } if(sensor > NOISE/2) return OFF; else return ON; } ////////////////////////////////////////////////// /* * @param none. * @return ON or OFF. */ uint8_t sensor_up_left(void) { uint16_t i, sensor = 0; for(i=0; i<NOISE; i++) { wait(0.001); sensor = sensor + SEN_UP_L; } if(sensor > NOISE/2) return OFF; else return ON; } ////////////////////////////////////////////////// /* * @param none. * @return ON or OFF. */ uint8_t sensor_up_right(void) { uint16_t i, sensor = 0; for(i=0; i<NOISE; i++) { wait(0.001); sensor = sensor + SEN_UP_R; } if(sensor > NOISE/2) return OFF; else return ON; } ////////////////////////////////////////////////// /* * @param none. * @return ON or OFF. */ uint8_t sensor_down(void) { uint16_t i, sensor = 0; for(i=0; i<NOISE; i++) { wait(0.001); sensor = sensor + SEN_DOWN; } if(sensor > NOISE/2) return OFF; else return ON; } ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// void periodicCallback(void) { } ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// int main(void) { wait(0.1); //Init Hardware SEN_UP_L.mode(PullUp); SEN_UP_R.mode(PullUp); SEN_DOWN.mode(PullUp); SEN_IR.mode(PullUp); //Init interupt Timer ticker.attach(periodicCallback, 0.00015); wait(0.5); while (true) { //rotator_right(20); uint8_t sen_in = sensor_ir(); if( (sen_in > 0) && (sen_in < 50) ) { motorController.moveForward(1000); while( (sensor_up_left() == OFF) && (sensor_up_right() == OFF) ){ motorController.moveBackward(1000); } } else { motorController.turnRight(1000); } } } /* int main(void) { wait(0.1); //Init Hardware SEN_UP_L.mode(PullUp); SEN_UP_R.mode(PullUp); SEN_DOWN.mode(PullUp); SEN_IR.mode(PullUp); //Init interupt Timer ticker.attach(periodicCallback, 0.00015); wait(0.5); while (true) { //rotator_right(20); uint8_t sen_in = sensor_ir(); if( (sen_in > 0) && (sen_in < 50) ) { move_up(100, 100); while( (sensor_up_left() == OFF) && (sensor_up_right() == OFF) ){ move_down(40,40); } } else { rotator_right(100); } } } */