Final Project / Mbed 2 deprecated mbed-os-Final

This is a very simple guide, reviewing the steps required to get Blinky working on an Mbed OS platform.

Dependencies:   mbed Adafruit_GFX

main.cpp@95:250afd53b710, 2019-06-08 (annotated)

Committer:
ParkChunMyong
Date:
Sat Jun 08 12:00:38 2019 +0000
Revision:
95:250afd53b710
Parent:
88:bea4f2daa48c
Child:
96:7465ab270e7a
first init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ParkChunMyong 95:250afd53b710 1 #include "TRSensors.h"
ParkChunMyong 95:250afd53b710 2 #include "ReceiverIR.h"
ParkChunMyong 95:250afd53b710 3 #include "Drive.h"
ParkChunMyong 95:250afd53b710 4 #include "Adafruit_SSD1306.h"
ParkChunMyong 95:250afd53b710 5 #include "WS2812.h"
ParkChunMyong 95:250afd53b710 6 #include "PixelArray.h"
ParkChunMyong 95:250afd53b710 7 #include "ultrasonic.h"
ParkChunMyong 95:250afd53b710 8
ParkChunMyong 95:250afd53b710 9 #define NUM_SENSOR 5
ParkChunMyong 95:250afd53b710 10 #define OLED_RESET D9
ParkChunMyong 95:250afd53b710 11 #define WS2812_BUF 77 //number of LEDs in the array
ParkChunMyong 95:250afd53b710 12 #define NUM_COLORS 6 //number of colors to store in the array
ParkChunMyong 95:250afd53b710 13 #define NUM_STEPS 8 //number of steps between colors
ParkChunMyong 95:250afd53b710 14
ParkChunMyong 95:250afd53b710 15 #define PCF8574_ADDR 0x20
ParkChunMyong 95:250afd53b710 16 #define beep_on PCF8574Write(0xDF & PCF8574Read())
ParkChunMyong 95:250afd53b710 17 #define beep_off PCF8574Write(0x20 | PCF8574Read())
ParkChunMyong 95:250afd53b710 18
ParkChunMyong 95:250afd53b710 19 // IR REMOTE
ParkChunMyong 95:250afd53b710 20 ReceiverIR ir_rx(PB_5);
ParkChunMyong 95:250afd53b710 21 RemoteIR::Format format;
ParkChunMyong 95:250afd53b710 22 uint8_t buf[8];
ParkChunMyong 95:250afd53b710 23 int bitlength;
ParkChunMyong 95:250afd53b710 24
ParkChunMyong 95:250afd53b710 25 // Control motor drive
ParkChunMyong 95:250afd53b710 26 Drive drive(D6, D5, A1, A0, A2, A3);
ParkChunMyong 95:250afd53b710 27 int control_speed = 2500;
ParkChunMyong 95:250afd53b710 28 int calibrate_speed = 4000;
ParkChunMyong 95:250afd53b710 29 int running_speed = 4000;
ParkChunMyong 95:250afd53b710 30
ParkChunMyong 95:250afd53b710 31 // TR Sensor
ParkChunMyong 95:250afd53b710 32 TRSensors trs = TRSensors(D10, D11, D12, D13);
ParkChunMyong 95:250afd53b710 33 uint16_t sensorValues[NUM_SENSOR];
ParkChunMyong 95:250afd53b710 34 unsigned int last_proportional;
ParkChunMyong 95:250afd53b710 35 long integral;
ParkChunMyong 95:250afd53b710 36 int last_power_difference = 0;
ParkChunMyong 95:250afd53b710 37
ParkChunMyong 95:250afd53b710 38 // IR sensor
ParkChunMyong 95:250afd53b710 39 I2C i2c_for_irsensor(PB_9, PB_8);
ParkChunMyong 95:250afd53b710 40
ParkChunMyong 95:250afd53b710 41 // Ultra sonic sensor
ParkChunMyong 95:250afd53b710 42 Ultrasonic us(D3, D2, 0.1, false, 0.1);
ParkChunMyong 95:250afd53b710 43
ParkChunMyong 95:250afd53b710 44 // OLED time print
ParkChunMyong 95:250afd53b710 45 I2C i2c_for_oled(I2C_SDA,I2C_SCL);
ParkChunMyong 95:250afd53b710 46 Adafruit_SSD1306_I2c myOled(i2c_for_oled,OLED_RESET,0x78,64,128);
ParkChunMyong 95:250afd53b710 47 Timer t;
ParkChunMyong 95:250afd53b710 48
ParkChunMyong 95:250afd53b710 49 // LED when in goal
ParkChunMyong 95:250afd53b710 50 PixelArray px(WS2812_BUF);
ParkChunMyong 95:250afd53b710 51 WS2812 ws(D7, WS2812_BUF, 6,17,9,14); //nucleo-f411re // See the program page for information on the timing numbers
ParkChunMyong 95:250afd53b710 52
ParkChunMyong 95:250afd53b710 53 // Debug
ParkChunMyong 95:250afd53b710 54 RawSerial pc(PA_2, PA_3, 115200);
ParkChunMyong 95:250afd53b710 55
ParkChunMyong 95:250afd53b710 56 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100) {
ParkChunMyong 95:250afd53b710 57 int cnt = 0;
ParkChunMyong 95:250afd53b710 58 while (ir_rx.getState() != ReceiverIR::Received) {
ParkChunMyong 95:250afd53b710 59 cnt++;
ParkChunMyong 95:250afd53b710 60 if (timeout < cnt) {
ParkChunMyong 95:250afd53b710 61 return -1;
ParkChunMyong 95:250afd53b710 62 }
ParkChunMyong 95:250afd53b710 63 }
ParkChunMyong 95:250afd53b710 64 return ir_rx.getData(format, buf, bufsiz * 8);
ParkChunMyong 95:250afd53b710 65 }
ParkChunMyong 95:250afd53b710 66
ParkChunMyong 95:250afd53b710 67 void calibration(){
ParkChunMyong 95:250afd53b710 68 memset(buf, 0x00, sizeof(buf));
ParkChunMyong 95:250afd53b710 69 drive.setSpeed(calibrate_speed);
ParkChunMyong 95:250afd53b710 70 while(true)
ParkChunMyong 95:250afd53b710 71 {
ParkChunMyong 95:250afd53b710 72 bitlength = receive(&format, buf, sizeof(buf));
ParkChunMyong 95:250afd53b710 73 if (bitlength < 0) {
ParkChunMyong 95:250afd53b710 74 continue;
ParkChunMyong 95:250afd53b710 75 }
ParkChunMyong 95:250afd53b710 76
ParkChunMyong 95:250afd53b710 77 if(buf[2] == 0x18)
ParkChunMyong 95:250afd53b710 78 drive.Forward();
ParkChunMyong 95:250afd53b710 79
ParkChunMyong 95:250afd53b710 80 else if(buf[2] == 0x52)
ParkChunMyong 95:250afd53b710 81 drive.Backward();
ParkChunMyong 95:250afd53b710 82
ParkChunMyong 95:250afd53b710 83 else if(buf[2] == 0x8)
ParkChunMyong 95:250afd53b710 84 drive.Turn_left();
ParkChunMyong 95:250afd53b710 85
ParkChunMyong 95:250afd53b710 86 else if(buf[2] == 0x5A)
ParkChunMyong 95:250afd53b710 87 drive.Turn_right();
ParkChunMyong 95:250afd53b710 88
ParkChunMyong 95:250afd53b710 89 else if(buf[2] == 0x1C)
ParkChunMyong 95:250afd53b710 90 drive.Break();
ParkChunMyong 95:250afd53b710 91
ParkChunMyong 95:250afd53b710 92 else if(buf[2] == 0x0C)
ParkChunMyong 95:250afd53b710 93 {
ParkChunMyong 95:250afd53b710 94 drive.Break();
ParkChunMyong 95:250afd53b710 95 break;
ParkChunMyong 95:250afd53b710 96 }
ParkChunMyong 95:250afd53b710 97
ParkChunMyong 95:250afd53b710 98 wait(0.1);
ParkChunMyong 95:250afd53b710 99 drive.Break();
ParkChunMyong 95:250afd53b710 100 pc.printf("calibration!\r\n");
ParkChunMyong 95:250afd53b710 101
ParkChunMyong 95:250afd53b710 102 trs.calibrate();
ParkChunMyong 95:250afd53b710 103 }
ParkChunMyong 95:250afd53b710 104 }
ParkChunMyong 95:250afd53b710 105
ParkChunMyong 95:250afd53b710 106 void ready_to_drive(){
ParkChunMyong 95:250afd53b710 107 drive.setSpeed(control_speed);
ParkChunMyong 95:250afd53b710 108 memset(buf, 0x00, sizeof(buf));
ParkChunMyong 95:250afd53b710 109 while(true){
ParkChunMyong 95:250afd53b710 110 bitlength = receive(&format, buf, sizeof(buf));
ParkChunMyong 95:250afd53b710 111 if (bitlength < 0) {
ParkChunMyong 95:250afd53b710 112 continue;
ParkChunMyong 95:250afd53b710 113 }
ParkChunMyong 95:250afd53b710 114
ParkChunMyong 95:250afd53b710 115 if(buf[2] == 0x18)
ParkChunMyong 95:250afd53b710 116 drive.Forward();
ParkChunMyong 95:250afd53b710 117
ParkChunMyong 95:250afd53b710 118 else if(buf[2] == 0x52)
ParkChunMyong 95:250afd53b710 119 drive.Backward();
ParkChunMyong 95:250afd53b710 120
ParkChunMyong 95:250afd53b710 121 else if(buf[2] == 0x8)
ParkChunMyong 95:250afd53b710 122 drive.Turn_left();
ParkChunMyong 95:250afd53b710 123
ParkChunMyong 95:250afd53b710 124 else if(buf[2] == 0x5A)
ParkChunMyong 95:250afd53b710 125 drive.Turn_right();
ParkChunMyong 95:250afd53b710 126
ParkChunMyong 95:250afd53b710 127 else if(buf[2] == 0x1C)
ParkChunMyong 95:250afd53b710 128 drive.Break();
ParkChunMyong 95:250afd53b710 129
ParkChunMyong 95:250afd53b710 130 else if(buf[2] == 0x5E)
ParkChunMyong 95:250afd53b710 131 {
ParkChunMyong 95:250afd53b710 132 drive.Break();
ParkChunMyong 95:250afd53b710 133 break;
ParkChunMyong 95:250afd53b710 134 }
ParkChunMyong 95:250afd53b710 135
ParkChunMyong 95:250afd53b710 136 wait(0.1);
ParkChunMyong 95:250afd53b710 137 drive.Break();
ParkChunMyong 95:250afd53b710 138 }
ParkChunMyong 95:250afd53b710 139 }
ParkChunMyong 95:250afd53b710 140
ParkChunMyong 95:250afd53b710 141 void PCF8574Write(char data){
ParkChunMyong 95:250afd53b710 142 i2c_for_irsensor.write((PCF8574_ADDR<<1), &data, 1);
ParkChunMyong 95:250afd53b710 143 }
ParkChunMyong 95:250afd53b710 144
ParkChunMyong 95:250afd53b710 145 void PCF8574Initialize(){
ParkChunMyong 95:250afd53b710 146 PCF8574Write(0x01);
ParkChunMyong 95:250afd53b710 147 }
ParkChunMyong 95:250afd53b710 148
ParkChunMyong 95:250afd53b710 149 char PCF8574Read(){
ParkChunMyong 95:250afd53b710 150 char data = 0xFF;
ParkChunMyong 95:250afd53b710 151 i2c_for_irsensor.read(((PCF8574_ADDR<<1)|0x01), &data, 1);
ParkChunMyong 95:250afd53b710 152 return data;
ParkChunMyong 95:250afd53b710 153 }
ParkChunMyong 95:250afd53b710 154
ParkChunMyong 95:250afd53b710 155 void check_irsensor(void){
ParkChunMyong 95:250afd53b710 156 char data;
ParkChunMyong 95:250afd53b710 157
ParkChunMyong 95:250afd53b710 158 PCF8574Write(0xC0 | PCF8574Read());
ParkChunMyong 95:250afd53b710 159 data = PCF8574Read() | 0x3F;
ParkChunMyong 95:250afd53b710 160 if(data == 0x7F){
ParkChunMyong 95:250afd53b710 161 pc.printf("left\r\n");
ParkChunMyong 95:250afd53b710 162 // avoid_right_obstacle();
ParkChunMyong 95:250afd53b710 163 } else if(data == 0xBF){
ParkChunMyong 95:250afd53b710 164 pc.printf("right\r\n");
ParkChunMyong 95:250afd53b710 165 // avoid_left_obstacle();
ParkChunMyong 95:250afd53b710 166 }
ParkChunMyong 95:250afd53b710 167 }
ParkChunMyong 95:250afd53b710 168
ParkChunMyong 95:250afd53b710 169 void check_obstacle_with_ultrasonic_sensor(void){
ParkChunMyong 95:250afd53b710 170 if(us.getDistance()< 10){ // Reduce magic number
ParkChunMyong 95:250afd53b710 171 beep_on;
ParkChunMyong 95:250afd53b710 172 } else {
ParkChunMyong 95:250afd53b710 173 beep_off;
ParkChunMyong 95:250afd53b710 174 }
ParkChunMyong 95:250afd53b710 175 }
ParkChunMyong 95:250afd53b710 176
ParkChunMyong 95:250afd53b710 177 void start_auto_drive(){
ParkChunMyong 95:250afd53b710 178 drive.setSpeed(running_speed);
ParkChunMyong 95:250afd53b710 179 // drive.Forward();
ParkChunMyong 95:250afd53b710 180 while(true){
ParkChunMyong 95:250afd53b710 181 check_irsensor();
ParkChunMyong 95:250afd53b710 182 check_obstacle_with_ultrasonic_sensor();
ParkChunMyong 95:250afd53b710 183
ParkChunMyong 95:250afd53b710 184 unsigned int position = trs.readLine(sensorValues);
ParkChunMyong 95:250afd53b710 185
ParkChunMyong 95:250afd53b710 186 // // for pc debug
ParkChunMyong 95:250afd53b710 187 // for(int i=0; i<5; i++){
ParkChunMyong 95:250afd53b710 188 // pc.printf("%d\t", sensorValues[i]);
ParkChunMyong 95:250afd53b710 189 // }
ParkChunMyong 95:250afd53b710 190 // pc.printf("\r\n");
ParkChunMyong 95:250afd53b710 191 //
ParkChunMyong 95:250afd53b710 192 // // for oled debug
ParkChunMyong 95:250afd53b710 193 // for(int i=0; i<5; i++){
ParkChunMyong 95:250afd53b710 194 // myOled.printf("%d\t", sensorValues[i]);
ParkChunMyong 95:250afd53b710 195 // myOled.display();
ParkChunMyong 95:250afd53b710 196 // }
ParkChunMyong 95:250afd53b710 197 // myOled.printf("\r");
ParkChunMyong 95:250afd53b710 198 // myOled.display();
ParkChunMyong 95:250afd53b710 199
ParkChunMyong 95:250afd53b710 200 // It need to have a specific rule
ParkChunMyong 95:250afd53b710 201 if( (sensorValues[0] >= 800) &&
ParkChunMyong 95:250afd53b710 202 (sensorValues[1] >= 800) &&
ParkChunMyong 95:250afd53b710 203 (sensorValues[2] >= 800) &&
ParkChunMyong 95:250afd53b710 204 (sensorValues[3] >= 800) &&
ParkChunMyong 95:250afd53b710 205 (sensorValues[4] >= 800) )
ParkChunMyong 95:250afd53b710 206 {
ParkChunMyong 95:250afd53b710 207 drive.Break();
ParkChunMyong 95:250afd53b710 208 break;
ParkChunMyong 95:250afd53b710 209 }
ParkChunMyong 95:250afd53b710 210
ParkChunMyong 95:250afd53b710 211 if(sensorValues[0] >= trs.calibratedMax[0]-15 && sensorValues[0] <= trs.calibratedMax[0]+15)
ParkChunMyong 95:250afd53b710 212 {
ParkChunMyong 95:250afd53b710 213 drive.setSpeed(running_speed-2800, running_speed);
ParkChunMyong 95:250afd53b710 214 // drive.Forward();
ParkChunMyong 95:250afd53b710 215 }
ParkChunMyong 95:250afd53b710 216
ParkChunMyong 95:250afd53b710 217 else if(sensorValues[1] >= trs.calibratedMax[1]-15 && sensorValues[1] <= trs.calibratedMax[1]+15)
ParkChunMyong 95:250afd53b710 218 {
ParkChunMyong 95:250afd53b710 219 drive.setSpeed(running_speed-700, running_speed);
ParkChunMyong 95:250afd53b710 220 // drive.Forward();
ParkChunMyong 95:250afd53b710 221 }
ParkChunMyong 95:250afd53b710 222
ParkChunMyong 95:250afd53b710 223 else if(sensorValues[2] >= trs.calibratedMax[2]-15 && sensorValues[2] <= trs.calibratedMax[2]+15)
ParkChunMyong 95:250afd53b710 224 {
ParkChunMyong 95:250afd53b710 225 drive.setSpeed(running_speed, running_speed);
ParkChunMyong 95:250afd53b710 226 // drive.Forward();
ParkChunMyong 95:250afd53b710 227 }
ParkChunMyong 95:250afd53b710 228
ParkChunMyong 95:250afd53b710 229 else if(sensorValues[3] >= trs.calibratedMax[3]-15 && sensorValues[3] <= trs.calibratedMax[3]+15)
ParkChunMyong 95:250afd53b710 230 {
ParkChunMyong 95:250afd53b710 231 drive.setSpeed(running_speed, running_speed-700);
ParkChunMyong 95:250afd53b710 232 // drive.Forward();
ParkChunMyong 95:250afd53b710 233 }
ParkChunMyong 95:250afd53b710 234
ParkChunMyong 95:250afd53b710 235 else if(sensorValues[4] >= trs.calibratedMax[4]-15 && sensorValues[4] <= trs.calibratedMax[4]+15)
ParkChunMyong 95:250afd53b710 236 {
ParkChunMyong 95:250afd53b710 237 drive.setSpeed(running_speed, running_speed-2800);
ParkChunMyong 95:250afd53b710 238 // drive.Forward();
ParkChunMyong 95:250afd53b710 239 }
ParkChunMyong 95:250afd53b710 240 else {
ParkChunMyong 95:250afd53b710 241 // drive.Forward();
ParkChunMyong 95:250afd53b710 242 }
ParkChunMyong 95:250afd53b710 243
ParkChunMyong 95:250afd53b710 244 wait(0.1);
ParkChunMyong 95:250afd53b710 245 drive.Break();
ParkChunMyong 95:250afd53b710 246 }
ParkChunMyong 95:250afd53b710 247 }
ParkChunMyong 95:250afd53b710 248
ParkChunMyong 95:250afd53b710 249
ParkChunMyong 95:250afd53b710 250 int color_set(uint8_t red,uint8_t green, uint8_t blue)
ParkChunMyong 95:250afd53b710 251 {
ParkChunMyong 95:250afd53b710 252 return ((red<<16) + (green<<8) + blue);
ParkChunMyong 95:250afd53b710 253 }
ParkChunMyong 95:250afd53b710 254
ParkChunMyong 95:250afd53b710 255 // 0 <= stepNumber <= lastStepNumber
ParkChunMyong 95:250afd53b710 256 int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)
ParkChunMyong 95:250afd53b710 257 {
ParkChunMyong 95:250afd53b710 258 return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
ParkChunMyong 95:250afd53b710 259 }
ParkChunMyong 95:250afd53b710 260
ParkChunMyong 95:250afd53b710 261
ParkChunMyong 95:250afd53b710 262 void bottom_led(void){ //led 제어
ParkChunMyong 95:250afd53b710 263 int colorIdx = 0;
ParkChunMyong 95:250afd53b710 264 int colorTo = 0;
ParkChunMyong 95:250afd53b710 265 int colorFrom = 0;
ParkChunMyong 95:250afd53b710 266
ParkChunMyong 95:250afd53b710 267 uint8_t ir = 0;
ParkChunMyong 95:250afd53b710 268 uint8_t ig = 0;
ParkChunMyong 95:250afd53b710 269 uint8_t ib = 0;
ParkChunMyong 95:250afd53b710 270
ParkChunMyong 95:250afd53b710 271
ParkChunMyong 95:250afd53b710 272 ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
ParkChunMyong 95:250afd53b710 273
ParkChunMyong 95:250afd53b710 274 // set up the colours we want to draw with
ParkChunMyong 95:250afd53b710 275 int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
ParkChunMyong 95:250afd53b710 276
ParkChunMyong 95:250afd53b710 277 // Now the buffer is written, write it to the led array.
ParkChunMyong 95:250afd53b710 278 while (1)
ParkChunMyong 95:250afd53b710 279 {
ParkChunMyong 95:250afd53b710 280 //get starting RGB components for interpolation
ParkChunMyong 95:250afd53b710 281 std::size_t c1 = colorbuf[colorFrom];
ParkChunMyong 95:250afd53b710 282 std::size_t r1 = (c1 & 0xff0000) >> 16;
ParkChunMyong 95:250afd53b710 283 std::size_t g1 = (c1 & 0x00ff00) >> 8;
ParkChunMyong 95:250afd53b710 284 std::size_t b1 = (c1 & 0x0000ff);
ParkChunMyong 95:250afd53b710 285
ParkChunMyong 95:250afd53b710 286 //get ending RGB components for interpolation
ParkChunMyong 95:250afd53b710 287 std::size_t c2 = colorbuf[colorTo];
ParkChunMyong 95:250afd53b710 288 std::size_t r2 = (c2 & 0xff0000) >> 16;
ParkChunMyong 95:250afd53b710 289 std::size_t g2 = (c2 & 0x00ff00) >> 8;
ParkChunMyong 95:250afd53b710 290 std::size_t b2 = (c2 & 0x0000ff);
ParkChunMyong 95:250afd53b710 291
ParkChunMyong 95:250afd53b710 292 for (int i = 0; i <= NUM_STEPS; i++)
ParkChunMyong 95:250afd53b710 293 {
ParkChunMyong 95:250afd53b710 294 ir = interpolate(r1, r2, i, NUM_STEPS);
ParkChunMyong 95:250afd53b710 295 ig = interpolate(g1, g2, i, NUM_STEPS);
ParkChunMyong 95:250afd53b710 296 ib = interpolate(b1, b2, i, NUM_STEPS);
ParkChunMyong 95:250afd53b710 297
ParkChunMyong 95:250afd53b710 298 //write the color value for each pixel
ParkChunMyong 95:250afd53b710 299 px.SetAll(color_set(ir,ig,ib));
ParkChunMyong 95:250afd53b710 300
ParkChunMyong 95:250afd53b710 301 //write the II value for each pixel
ParkChunMyong 95:250afd53b710 302 px.SetAllI(32);
ParkChunMyong 95:250afd53b710 303
ParkChunMyong 95:250afd53b710 304 for (int i = WS2812_BUF; i >= 0; i--)
ParkChunMyong 95:250afd53b710 305 {
ParkChunMyong 95:250afd53b710 306 ws.write(px.getBuf());
ParkChunMyong 95:250afd53b710 307 }
ParkChunMyong 95:250afd53b710 308 }
ParkChunMyong 95:250afd53b710 309
ParkChunMyong 95:250afd53b710 310 colorFrom = colorIdx;
ParkChunMyong 95:250afd53b710 311 colorIdx++;
ParkChunMyong 95:250afd53b710 312
ParkChunMyong 95:250afd53b710 313 if (colorIdx >= NUM_COLORS)
ParkChunMyong 95:250afd53b710 314 {
ParkChunMyong 95:250afd53b710 315 colorIdx = 0;
ParkChunMyong 95:250afd53b710 316 }
ParkChunMyong 95:250afd53b710 317
ParkChunMyong 95:250afd53b710 318 colorTo = colorIdx;
ParkChunMyong 95:250afd53b710 319
ParkChunMyong 95:250afd53b710 320 }
ParkChunMyong 95:250afd53b710 321 }
ParkChunMyong 95:250afd53b710 322
ParkChunMyong 95:250afd53b710 323 int main(){
ParkChunMyong 95:250afd53b710 324 //program setup
ParkChunMyong 95:250afd53b710 325 myOled.begin();
ParkChunMyong 95:250afd53b710 326 i2c_for_irsensor.frequency(100000); // main에 써야함
ParkChunMyong 95:250afd53b710 327
ParkChunMyong 95:250afd53b710 328 // Test OLED
ParkChunMyong 95:250afd53b710 329 myOled.printf("oled size: %ux%u \r\nStart Alphabot2\r\n", myOled.width(), myOled.height());
ParkChunMyong 95:250afd53b710 330 myOled.display();
ParkChunMyong 95:250afd53b710 331
ParkChunMyong 95:250afd53b710 332 //program start
ParkChunMyong 95:250afd53b710 333 memset(buf, 0x00, sizeof(buf));
ParkChunMyong 95:250afd53b710 334
ParkChunMyong 95:250afd53b710 335 while(true){
ParkChunMyong 95:250afd53b710 336 bitlength = receive(&format, buf, sizeof(buf));
ParkChunMyong 95:250afd53b710 337 if (bitlength < 0) {
ParkChunMyong 95:250afd53b710 338 continue;
ParkChunMyong 95:250afd53b710 339 }
ParkChunMyong 95:250afd53b710 340
ParkChunMyong 95:250afd53b710 341 if(buf[2] == 0x42)
ParkChunMyong 95:250afd53b710 342 break;
ParkChunMyong 95:250afd53b710 343 }
ParkChunMyong 95:250afd53b710 344
ParkChunMyong 95:250afd53b710 345 //calibration
ParkChunMyong 95:250afd53b710 346 // pc.printf("calibration!\r\n");
ParkChunMyong 95:250afd53b710 347 myOled.printf("calibration!\r");
ParkChunMyong 95:250afd53b710 348 myOled.display();
ParkChunMyong 95:250afd53b710 349 calibration();
ParkChunMyong 95:250afd53b710 350
ParkChunMyong 95:250afd53b710 351 //ready autodriving
ParkChunMyong 95:250afd53b710 352 // pc.printf("ready_to_drive!\r\n");
ParkChunMyong 95:250afd53b710 353 myOled.printf("ready_to_drive!\r");
ParkChunMyong 95:250afd53b710 354 myOled.display();
ParkChunMyong 95:250afd53b710 355 ready_to_drive();
ParkChunMyong 95:250afd53b710 356
ParkChunMyong 95:250afd53b710 357 //go autodrive
ParkChunMyong 95:250afd53b710 358 // pc.printf("start_auto_drive!\r\n");
ParkChunMyong 95:250afd53b710 359 myOled.printf("start_auto_drive!\r\n");
ParkChunMyong 95:250afd53b710 360 myOled.display();
ParkChunMyong 95:250afd53b710 361 t.start();
ParkChunMyong 95:250afd53b710 362 start_auto_drive();
ParkChunMyong 95:250afd53b710 363 t.stop();
ParkChunMyong 95:250afd53b710 364 beep_off;
ParkChunMyong 95:250afd53b710 365 pc.printf("The time taken was %f seconds\r\n", t.read());
ParkChunMyong 95:250afd53b710 366
ParkChunMyong 95:250afd53b710 367 /* OLED time print */
ParkChunMyong 95:250afd53b710 368 myOled.printf("\nGoal! \r\nThe time taken was\r\n%f seconds\r\n", t.read());
ParkChunMyong 95:250afd53b710 369 myOled.display();
ParkChunMyong 95:250afd53b710 370 /* LED run*/
ParkChunMyong 95:250afd53b710 371 bottom_led();
ParkChunMyong 95:250afd53b710 372
ParkChunMyong 95:250afd53b710 373 while(true){}
ParkChunMyong 95:250afd53b710 374 }
ParkChunMyong 95:250afd53b710 375