Tan Pham / Mbed 2 deprecated binbot

VNG bot battle

Dependencies:   BLE_API mbed nRF51822

main.log@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 * main.cpp
bintech91 0:122d7bf3bbf0 3 *
bintech91 0:122d7bf3bbf0 4 * Created on: Aug 23, 2016
bintech91 0:122d7bf3bbf0 5 * Author: tanpt
bintech91 0:122d7bf3bbf0 6 */
bintech91 0:122d7bf3bbf0 7
bintech91 0:122d7bf3bbf0 8 #include "common.h"
bintech91 0:122d7bf3bbf0 9 #include "MotorController.h"
bintech91 0:122d7bf3bbf0 10
bintech91 0:122d7bf3bbf0 11 //#include "ble/BLE.h"
bintech91 0:122d7bf3bbf0 12 //#include "ble/services/iBeacon.h"
bintech91 0:122d7bf3bbf0 13 //#include "ble/services/UARTService.h"
bintech91 0:122d7bf3bbf0 14
bintech91 0:122d7bf3bbf0 15 MotorController motorController;
bintech91 0:122d7bf3bbf0 16
bintech91 0:122d7bf3bbf0 17 //// Sensor
bintech91 0:122d7bf3bbf0 18 #define NOISE 5
bintech91 0:122d7bf3bbf0 19 //#define DEBUG
bintech91 0:122d7bf3bbf0 20
bintech91 0:122d7bf3bbf0 21 #ifdef DEBUG
bintech91 0:122d7bf3bbf0 22 Serial pc(p10, p11);
bintech91 0:122d7bf3bbf0 23 #endif
bintech91 0:122d7bf3bbf0 24 Ticker ticker;
bintech91 0:122d7bf3bbf0 25 AnalogIn analog_ir(p1);
bintech91 0:122d7bf3bbf0 26 DigitalIn SEN_UP_R(p2);
bintech91 0:122d7bf3bbf0 27 DigitalIn SEN_UP_L(p3);
bintech91 0:122d7bf3bbf0 28 DigitalIn SEN_DOWN(p4);
bintech91 0:122d7bf3bbf0 29 DigitalIn SEN_IR(p6);
bintech91 0:122d7bf3bbf0 30
bintech91 0:122d7bf3bbf0 31 #define ON 0
bintech91 0:122d7bf3bbf0 32 #define OFF 1
bintech91 0:122d7bf3bbf0 33 #define VREF 3300.0 //mV
bintech91 0:122d7bf3bbf0 34
bintech91 0:122d7bf3bbf0 35 uint16_t g_time_led = 0;
bintech91 0:122d7bf3bbf0 36
bintech91 0:122d7bf3bbf0 37 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 38 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 39 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 40
bintech91 0:122d7bf3bbf0 41 //////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 42 //////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 43 /*
bintech91 0:122d7bf3bbf0 44 * @param none.
bintech91 0:122d7bf3bbf0 45 * @return Distance (cm), 1-80 cm.
bintech91 0:122d7bf3bbf0 46 */
bintech91 0:122d7bf3bbf0 47 uint16_t sensor_ir(void)
bintech91 0:122d7bf3bbf0 48 {
bintech91 0:122d7bf3bbf0 49 uint16_t adc;
bintech91 0:122d7bf3bbf0 50 uint16_t cm;
bintech91 0:122d7bf3bbf0 51 uint16_t sensor = 0, i;
bintech91 0:122d7bf3bbf0 52
bintech91 0:122d7bf3bbf0 53 for(i=0; i<NOISE; i++) {
bintech91 0:122d7bf3bbf0 54 adc = analog_ir.read_u16();
bintech91 0:122d7bf3bbf0 55 adc = 750-adc;
bintech91 0:122d7bf3bbf0 56 if (adc > 60000) cm = 1;
bintech91 0:122d7bf3bbf0 57 else if (adc > 600) cm = 0;
bintech91 0:122d7bf3bbf0 58 else if (adc > 550) cm = adc/8;
bintech91 0:122d7bf3bbf0 59 else if (adc > 500) cm = adc/10;
bintech91 0:122d7bf3bbf0 60 else if (adc > 450) cm = adc/12;
bintech91 0:122d7bf3bbf0 61 else if (adc > 400) cm = adc/14;
bintech91 0:122d7bf3bbf0 62 else if (adc > 350) cm = adc/16;
bintech91 0:122d7bf3bbf0 63 else if (adc > 300) cm = adc/18;
bintech91 0:122d7bf3bbf0 64 else if (adc > 200) cm = adc/16;
bintech91 0:122d7bf3bbf0 65 else if (adc > 200) cm = adc/14;
bintech91 0:122d7bf3bbf0 66 else if (adc > 150) cm = adc/12;
bintech91 0:122d7bf3bbf0 67 else if (adc > 100) cm = adc/10;
bintech91 0:122d7bf3bbf0 68 else if (adc > 60) cm = adc/9;
bintech91 0:122d7bf3bbf0 69 else if (adc > 30) cm = adc/8;
bintech91 0:122d7bf3bbf0 70 else if (adc > 0) cm = adc/7;
bintech91 0:122d7bf3bbf0 71
bintech91 0:122d7bf3bbf0 72 wait(0.001);
bintech91 0:122d7bf3bbf0 73 sensor = sensor + cm;
bintech91 0:122d7bf3bbf0 74 if(cm == 0) break;
bintech91 0:122d7bf3bbf0 75 cm = sensor/NOISE;
bintech91 0:122d7bf3bbf0 76 }
bintech91 0:122d7bf3bbf0 77
bintech91 0:122d7bf3bbf0 78 #ifdef DEBUG
bintech91 0:122d7bf3bbf0 79 pc.printf("\r\n %d adc, %d cm", adc, cm);
bintech91 0:122d7bf3bbf0 80 #endif
bintech91 0:122d7bf3bbf0 81 return cm;
bintech91 0:122d7bf3bbf0 82 }
bintech91 0:122d7bf3bbf0 83
bintech91 0:122d7bf3bbf0 84 //////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 85 /*
bintech91 0:122d7bf3bbf0 86 * @param none.
bintech91 0:122d7bf3bbf0 87 * @return ON or OFF.
bintech91 0:122d7bf3bbf0 88 */
bintech91 0:122d7bf3bbf0 89 uint8_t sensor_ir2(void)
bintech91 0:122d7bf3bbf0 90 {
bintech91 0:122d7bf3bbf0 91 uint16_t i, sensor = 0;
bintech91 0:122d7bf3bbf0 92
bintech91 0:122d7bf3bbf0 93 for(i=0; i<NOISE; i++) {
bintech91 0:122d7bf3bbf0 94 wait(0.001);
bintech91 0:122d7bf3bbf0 95 sensor = sensor + SEN_IR;
bintech91 0:122d7bf3bbf0 96 }
bintech91 0:122d7bf3bbf0 97 if(sensor > NOISE/2) return OFF;
bintech91 0:122d7bf3bbf0 98 else return ON;
bintech91 0:122d7bf3bbf0 99 }
bintech91 0:122d7bf3bbf0 100
bintech91 0:122d7bf3bbf0 101 //////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 102 /*
bintech91 0:122d7bf3bbf0 103 * @param none.
bintech91 0:122d7bf3bbf0 104 * @return ON or OFF.
bintech91 0:122d7bf3bbf0 105 */
bintech91 0:122d7bf3bbf0 106 uint8_t sensor_up_left(void)
bintech91 0:122d7bf3bbf0 107 {
bintech91 0:122d7bf3bbf0 108 uint16_t i, sensor = 0;
bintech91 0:122d7bf3bbf0 109
bintech91 0:122d7bf3bbf0 110 for(i=0; i<NOISE; i++) {
bintech91 0:122d7bf3bbf0 111 wait(0.001);
bintech91 0:122d7bf3bbf0 112 sensor = sensor + SEN_UP_L;
bintech91 0:122d7bf3bbf0 113 }
bintech91 0:122d7bf3bbf0 114 if(sensor > NOISE/2) return OFF;
bintech91 0:122d7bf3bbf0 115 else return ON;
bintech91 0:122d7bf3bbf0 116 }
bintech91 0:122d7bf3bbf0 117 //////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 118 /*
bintech91 0:122d7bf3bbf0 119 * @param none.
bintech91 0:122d7bf3bbf0 120 * @return ON or OFF.
bintech91 0:122d7bf3bbf0 121 */
bintech91 0:122d7bf3bbf0 122 uint8_t sensor_up_right(void)
bintech91 0:122d7bf3bbf0 123 {
bintech91 0:122d7bf3bbf0 124 uint16_t i, sensor = 0;
bintech91 0:122d7bf3bbf0 125
bintech91 0:122d7bf3bbf0 126 for(i=0; i<NOISE; i++) {
bintech91 0:122d7bf3bbf0 127 wait(0.001);
bintech91 0:122d7bf3bbf0 128 sensor = sensor + SEN_UP_R;
bintech91 0:122d7bf3bbf0 129 }
bintech91 0:122d7bf3bbf0 130 if(sensor > NOISE/2) return OFF;
bintech91 0:122d7bf3bbf0 131 else return ON;
bintech91 0:122d7bf3bbf0 132 }
bintech91 0:122d7bf3bbf0 133 //////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 134 /*
bintech91 0:122d7bf3bbf0 135 * @param none.
bintech91 0:122d7bf3bbf0 136 * @return ON or OFF.
bintech91 0:122d7bf3bbf0 137 */
bintech91 0:122d7bf3bbf0 138 uint8_t sensor_down(void)
bintech91 0:122d7bf3bbf0 139 {
bintech91 0:122d7bf3bbf0 140 uint16_t i, sensor = 0;
bintech91 0:122d7bf3bbf0 141
bintech91 0:122d7bf3bbf0 142 for(i=0; i<NOISE; i++) {
bintech91 0:122d7bf3bbf0 143 wait(0.001);
bintech91 0:122d7bf3bbf0 144 sensor = sensor + SEN_DOWN;
bintech91 0:122d7bf3bbf0 145 }
bintech91 0:122d7bf3bbf0 146 if(sensor > NOISE/2) return OFF;
bintech91 0:122d7bf3bbf0 147 else return ON;
bintech91 0:122d7bf3bbf0 148 }
bintech91 0:122d7bf3bbf0 149 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 150 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 151 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 152 void periodicCallback(void)
bintech91 0:122d7bf3bbf0 153 {
bintech91 0:122d7bf3bbf0 154
bintech91 0:122d7bf3bbf0 155 }
bintech91 0:122d7bf3bbf0 156
bintech91 0:122d7bf3bbf0 157 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 158 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 159 //////////////////////////////////////////////////////////////////////////////////////////////////
bintech91 0:122d7bf3bbf0 160
bintech91 0:122d7bf3bbf0 161 int main(void)
bintech91 0:122d7bf3bbf0 162 {
bintech91 0:122d7bf3bbf0 163 wait(0.1);
bintech91 0:122d7bf3bbf0 164 //Init Hardware
bintech91 0:122d7bf3bbf0 165 SEN_UP_L.mode(PullUp);
bintech91 0:122d7bf3bbf0 166 SEN_UP_R.mode(PullUp);
bintech91 0:122d7bf3bbf0 167 SEN_DOWN.mode(PullUp);
bintech91 0:122d7bf3bbf0 168 SEN_IR.mode(PullUp);
bintech91 0:122d7bf3bbf0 169 //Init interupt Timer
bintech91 0:122d7bf3bbf0 170 ticker.attach(periodicCallback, 0.00015);
bintech91 0:122d7bf3bbf0 171 wait(0.5);
bintech91 0:122d7bf3bbf0 172 while (true) {
bintech91 0:122d7bf3bbf0 173 //rotator_right(20);
bintech91 0:122d7bf3bbf0 174 uint8_t sen_in = sensor_ir();
bintech91 0:122d7bf3bbf0 175 if( (sen_in > 0) && (sen_in < 50) )
bintech91 0:122d7bf3bbf0 176 {
bintech91 0:122d7bf3bbf0 177 motorController.moveForward(1000);
bintech91 0:122d7bf3bbf0 178 while( (sensor_up_left() == OFF) && (sensor_up_right() == OFF) ){
bintech91 0:122d7bf3bbf0 179 motorController.moveBackward(1000);
bintech91 0:122d7bf3bbf0 180 }
bintech91 0:122d7bf3bbf0 181 } else {
bintech91 0:122d7bf3bbf0 182 motorController.turnRight(1000);
bintech91 0:122d7bf3bbf0 183 }
bintech91 0:122d7bf3bbf0 184 }
bintech91 0:122d7bf3bbf0 185 }
bintech91 0:122d7bf3bbf0 186
bintech91 0:122d7bf3bbf0 187 /*
bintech91 0:122d7bf3bbf0 188 int main(void)
bintech91 0:122d7bf3bbf0 189 {
bintech91 0:122d7bf3bbf0 190 wait(0.1);
bintech91 0:122d7bf3bbf0 191 //Init Hardware
bintech91 0:122d7bf3bbf0 192 SEN_UP_L.mode(PullUp);
bintech91 0:122d7bf3bbf0 193 SEN_UP_R.mode(PullUp);
bintech91 0:122d7bf3bbf0 194 SEN_DOWN.mode(PullUp);
bintech91 0:122d7bf3bbf0 195 SEN_IR.mode(PullUp);
bintech91 0:122d7bf3bbf0 196
bintech91 0:122d7bf3bbf0 197 //Init interupt Timer
bintech91 0:122d7bf3bbf0 198 ticker.attach(periodicCallback, 0.00015);
bintech91 0:122d7bf3bbf0 199 wait(0.5);
bintech91 0:122d7bf3bbf0 200 while (true) {
bintech91 0:122d7bf3bbf0 201 //rotator_right(20);
bintech91 0:122d7bf3bbf0 202 uint8_t sen_in = sensor_ir();
bintech91 0:122d7bf3bbf0 203 if( (sen_in > 0) && (sen_in < 50) )
bintech91 0:122d7bf3bbf0 204 {
bintech91 0:122d7bf3bbf0 205 move_up(100, 100);
bintech91 0:122d7bf3bbf0 206 while( (sensor_up_left() == OFF) && (sensor_up_right() == OFF) ){
bintech91 0:122d7bf3bbf0 207 move_down(40,40);
bintech91 0:122d7bf3bbf0 208 }
bintech91 0:122d7bf3bbf0 209 } else {
bintech91 0:122d7bf3bbf0 210 rotator_right(100);
bintech91 0:122d7bf3bbf0 211 }
bintech91 0:122d7bf3bbf0 212 }
bintech91 0:122d7bf3bbf0 213 }
bintech91 0:122d7bf3bbf0 214 */
bintech91 0:122d7bf3bbf0 215
bintech91 0:122d7bf3bbf0 216
bintech91 0:122d7bf3bbf0 217
bintech91 0:122d7bf3bbf0 218