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?
User | Revision | Line number | New 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 |