![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
This is a very simple guide, reviewing the steps required to get Blinky working on an Mbed OS platform.
Dependencies: mbed Adafruit_GFX
Diff: main.cpp
- Revision:
- 95:250afd53b710
- Parent:
- 88:bea4f2daa48c
- Child:
- 96:7465ab270e7a
diff -r ef34de021f40 -r 250afd53b710 main.cpp --- a/main.cpp Thu Apr 11 15:00:04 2019 +0100 +++ b/main.cpp Sat Jun 08 12:00:38 2019 +0000 @@ -1,32 +1,375 @@ -/* mbed Microcontroller Library - * Copyright (c) 2018 ARM Limited - * SPDX-License-Identifier: Apache-2.0 - */ - -#include "mbed.h" -#include "stats_report.h" - -DigitalOut led1(LED1); - -#define SLEEP_TIME 500 // (msec) -#define PRINT_AFTER_N_LOOPS 20 - -// main() runs in its own thread in the OS -int main() -{ - SystemReport sys_state( SLEEP_TIME * PRINT_AFTER_N_LOOPS /* Loop delay time in ms */); - - int count = 0; - while (true) { - // Blink LED and wait 0.5 seconds - led1 = !led1; - wait_ms(SLEEP_TIME); - - if ((0 == count) || (PRINT_AFTER_N_LOOPS == count)) { - // Following the main thread wait, report on the current system status - sys_state.report_state(); - count = 0; - } - ++count; - } -} +#include "TRSensors.h" +#include "ReceiverIR.h" +#include "Drive.h" +#include "Adafruit_SSD1306.h" +#include "WS2812.h" +#include "PixelArray.h" +#include "ultrasonic.h" + +#define NUM_SENSOR 5 +#define OLED_RESET D9 +#define WS2812_BUF 77 //number of LEDs in the array +#define NUM_COLORS 6 //number of colors to store in the array +#define NUM_STEPS 8 //number of steps between colors + +#define PCF8574_ADDR 0x20 +#define beep_on PCF8574Write(0xDF & PCF8574Read()) +#define beep_off PCF8574Write(0x20 | PCF8574Read()) + +// IR REMOTE +ReceiverIR ir_rx(PB_5); +RemoteIR::Format format; +uint8_t buf[8]; +int bitlength; + +// Control motor drive +Drive drive(D6, D5, A1, A0, A2, A3); +int control_speed = 2500; +int calibrate_speed = 4000; +int running_speed = 4000; + +// TR Sensor +TRSensors trs = TRSensors(D10, D11, D12, D13); +uint16_t sensorValues[NUM_SENSOR]; +unsigned int last_proportional; +long integral; +int last_power_difference = 0; + +// IR sensor +I2C i2c_for_irsensor(PB_9, PB_8); + +// Ultra sonic sensor +Ultrasonic us(D3, D2, 0.1, false, 0.1); + +// OLED time print +I2C i2c_for_oled(I2C_SDA,I2C_SCL); +Adafruit_SSD1306_I2c myOled(i2c_for_oled,OLED_RESET,0x78,64,128); +Timer t; + +// LED when in goal +PixelArray px(WS2812_BUF); +WS2812 ws(D7, WS2812_BUF, 6,17,9,14); //nucleo-f411re // See the program page for information on the timing numbers + +// Debug +RawSerial pc(PA_2, PA_3, 115200); + +int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100) { + int cnt = 0; + while (ir_rx.getState() != ReceiverIR::Received) { + cnt++; + if (timeout < cnt) { + return -1; + } + } + return ir_rx.getData(format, buf, bufsiz * 8); +} + +void calibration(){ + memset(buf, 0x00, sizeof(buf)); + drive.setSpeed(calibrate_speed); + while(true) + { + bitlength = receive(&format, buf, sizeof(buf)); + if (bitlength < 0) { + continue; + } + + if(buf[2] == 0x18) + drive.Forward(); + + else if(buf[2] == 0x52) + drive.Backward(); + + else if(buf[2] == 0x8) + drive.Turn_left(); + + else if(buf[2] == 0x5A) + drive.Turn_right(); + + else if(buf[2] == 0x1C) + drive.Break(); + + else if(buf[2] == 0x0C) + { + drive.Break(); + break; + } + + wait(0.1); + drive.Break(); + pc.printf("calibration!\r\n"); + + trs.calibrate(); + } +} + +void ready_to_drive(){ + drive.setSpeed(control_speed); + memset(buf, 0x00, sizeof(buf)); + while(true){ + bitlength = receive(&format, buf, sizeof(buf)); + if (bitlength < 0) { + continue; + } + + if(buf[2] == 0x18) + drive.Forward(); + + else if(buf[2] == 0x52) + drive.Backward(); + + else if(buf[2] == 0x8) + drive.Turn_left(); + + else if(buf[2] == 0x5A) + drive.Turn_right(); + + else if(buf[2] == 0x1C) + drive.Break(); + + else if(buf[2] == 0x5E) + { + drive.Break(); + break; + } + + wait(0.1); + drive.Break(); + } +} + +void PCF8574Write(char data){ + i2c_for_irsensor.write((PCF8574_ADDR<<1), &data, 1); +} + +void PCF8574Initialize(){ + PCF8574Write(0x01); +} + +char PCF8574Read(){ + char data = 0xFF; + i2c_for_irsensor.read(((PCF8574_ADDR<<1)|0x01), &data, 1); + return data; +} + +void check_irsensor(void){ + char data; + + PCF8574Write(0xC0 | PCF8574Read()); + data = PCF8574Read() | 0x3F; + if(data == 0x7F){ + pc.printf("left\r\n"); + // avoid_right_obstacle(); + } else if(data == 0xBF){ + pc.printf("right\r\n"); + // avoid_left_obstacle(); + } +} + +void check_obstacle_with_ultrasonic_sensor(void){ + if(us.getDistance()< 10){ // Reduce magic number + beep_on; + } else { + beep_off; + } +} + +void start_auto_drive(){ + drive.setSpeed(running_speed); +// drive.Forward(); + while(true){ + check_irsensor(); + check_obstacle_with_ultrasonic_sensor(); + + unsigned int position = trs.readLine(sensorValues); + +// // for pc debug +// for(int i=0; i<5; i++){ +// pc.printf("%d\t", sensorValues[i]); +// } +// pc.printf("\r\n"); +// +// // for oled debug +// for(int i=0; i<5; i++){ +// myOled.printf("%d\t", sensorValues[i]); +// myOled.display(); +// } +// myOled.printf("\r"); +// myOled.display(); + + // It need to have a specific rule + if( (sensorValues[0] >= 800) && + (sensorValues[1] >= 800) && + (sensorValues[2] >= 800) && + (sensorValues[3] >= 800) && + (sensorValues[4] >= 800) ) + { + drive.Break(); + break; + } + + if(sensorValues[0] >= trs.calibratedMax[0]-15 && sensorValues[0] <= trs.calibratedMax[0]+15) + { + drive.setSpeed(running_speed-2800, running_speed); +// drive.Forward(); + } + + else if(sensorValues[1] >= trs.calibratedMax[1]-15 && sensorValues[1] <= trs.calibratedMax[1]+15) + { + drive.setSpeed(running_speed-700, running_speed); +// drive.Forward(); + } + + else if(sensorValues[2] >= trs.calibratedMax[2]-15 && sensorValues[2] <= trs.calibratedMax[2]+15) + { + drive.setSpeed(running_speed, running_speed); +// drive.Forward(); + } + + else if(sensorValues[3] >= trs.calibratedMax[3]-15 && sensorValues[3] <= trs.calibratedMax[3]+15) + { + drive.setSpeed(running_speed, running_speed-700); +// drive.Forward(); + } + + else if(sensorValues[4] >= trs.calibratedMax[4]-15 && sensorValues[4] <= trs.calibratedMax[4]+15) + { + drive.setSpeed(running_speed, running_speed-2800); +// drive.Forward(); + } + else { +// drive.Forward(); + } + + wait(0.1); + drive.Break(); + } +} + + +int color_set(uint8_t red,uint8_t green, uint8_t blue) +{ + return ((red<<16) + (green<<8) + blue); +} + +// 0 <= stepNumber <= lastStepNumber +int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber) +{ + return (endValue - startValue) * stepNumber / lastStepNumber + startValue; +} + + +void bottom_led(void){ //led 제어 + int colorIdx = 0; + int colorTo = 0; + int colorFrom = 0; + + uint8_t ir = 0; + uint8_t ig = 0; + uint8_t ib = 0; + + + ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling + + // set up the colours we want to draw with + int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f}; + + // Now the buffer is written, write it to the led array. + while (1) + { + //get starting RGB components for interpolation + std::size_t c1 = colorbuf[colorFrom]; + std::size_t r1 = (c1 & 0xff0000) >> 16; + std::size_t g1 = (c1 & 0x00ff00) >> 8; + std::size_t b1 = (c1 & 0x0000ff); + + //get ending RGB components for interpolation + std::size_t c2 = colorbuf[colorTo]; + std::size_t r2 = (c2 & 0xff0000) >> 16; + std::size_t g2 = (c2 & 0x00ff00) >> 8; + std::size_t b2 = (c2 & 0x0000ff); + + for (int i = 0; i <= NUM_STEPS; i++) + { + ir = interpolate(r1, r2, i, NUM_STEPS); + ig = interpolate(g1, g2, i, NUM_STEPS); + ib = interpolate(b1, b2, i, NUM_STEPS); + + //write the color value for each pixel + px.SetAll(color_set(ir,ig,ib)); + + //write the II value for each pixel + px.SetAllI(32); + + for (int i = WS2812_BUF; i >= 0; i--) + { + ws.write(px.getBuf()); + } + } + + colorFrom = colorIdx; + colorIdx++; + + if (colorIdx >= NUM_COLORS) + { + colorIdx = 0; + } + + colorTo = colorIdx; + + } +} + +int main(){ + //program setup + myOled.begin(); + i2c_for_irsensor.frequency(100000); // main에 써야함 + + // Test OLED + myOled.printf("oled size: %ux%u \r\nStart Alphabot2\r\n", myOled.width(), myOled.height()); + myOled.display(); + + //program start + memset(buf, 0x00, sizeof(buf)); + + while(true){ + bitlength = receive(&format, buf, sizeof(buf)); + if (bitlength < 0) { + continue; + } + + if(buf[2] == 0x42) + break; + } + + //calibration +// pc.printf("calibration!\r\n"); + myOled.printf("calibration!\r"); + myOled.display(); + calibration(); + + //ready autodriving +// pc.printf("ready_to_drive!\r\n"); + myOled.printf("ready_to_drive!\r"); + myOled.display(); + ready_to_drive(); + + //go autodrive +// pc.printf("start_auto_drive!\r\n"); + myOled.printf("start_auto_drive!\r\n"); + myOled.display(); + t.start(); + start_auto_drive(); + t.stop(); + beep_off; + pc.printf("The time taken was %f seconds\r\n", t.read()); + + /* OLED time print */ + myOled.printf("\nGoal! \r\nThe time taken was\r\n%f seconds\r\n", t.read()); + myOled.display(); + /* LED run*/ + bottom_led(); + + while(true){} +} +