![](/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
main.cpp
- Committer:
- ParkChunMyong
- Date:
- 2019-06-13
- Revision:
- 96:7465ab270e7a
- Parent:
- 95:250afd53b710
File content as of revision 96:7465ab270e7a:
#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 KP 0.45 #define KI 0.0001 #define KD 0.1 #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 = 3000; int calibrate_speed = 6000; int running_speed = 4000; //auto const int maximum = 1000; float multiples, multiples2; int cnt = 0; // 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); bool isClose = false; // 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(); 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){ while(true){ PCF8574Write(0xC0 | PCF8574Read()); int dist = us.getDistance(); if(dist < 30 && dist != -1){ // Reduce magic number // beep_on; isClose = true; } else { // beep_off; isClose = false; } // myOled.clearDisplay(); // myOled.setTextCursor(0, 0); // myOled.printf("dist: %d\r\n", dist); // myOled.display(); wait(0.05); } } void start_auto_drive(){ drive.setSpeed(running_speed); // drive.Forward(); Thread thread; thread.start(check_obstacle_with_ultrasonic_sensor); while(true){ unsigned int position = trs.readLine(sensorValues); int proportional = (int)position - 2000; int derivative = proportional - last_proportional; integral += proportional; // last_proportional = proportional; int power_difference = proportional*KP + integral*KI + derivative*KD; // It need to have a specific rule if( (sensorValues[0] >= 700) && (sensorValues[1] >= 700) && (sensorValues[2] >= 700) && (sensorValues[4] >= 700) ) { drive.Break(); break; } if(power_difference > maximum) power_difference = maximum; if(power_difference < -maximum) power_difference = -maximum; int power_difference_n = power_difference * 2.3; float left, right; // error 보정 if(power_difference_n < 0) { left = control_speed; right = control_speed-power_difference_n; } else { left = control_speed+power_difference_n; right = control_speed; } if(position < 900) { multiples2 = 0.5f; left = 0; cnt = 0; } else if(position < 1300) { multiples2 = 0.5f; left = 0; cnt = 0; } else if(position < 1550) { multiples2 = 0.5f; left = 0; cnt = 0; } else if(position < 2000) { if(cnt < 5) multiples2 = 0.5f; else multiples2 = 1.0f; cnt++; } else if(position < 2350) { if(cnt < 5) multiples2 = 0.5f; else multiples2 = 1.0f; cnt++; } else if(position < 2500) { multiples2 = 0.5f; right = 0; cnt = 0; } else if(position < 3100) { multiples2 = 0.5f; right = 0; cnt = 0; } else { multiples2 = 0.5f; right = 0; cnt = 0; } if(isClose) { multiples = 1.2f * multiples2; if((sensorValues[0] >= 500) || (sensorValues[1] >= 500)){ do{ drive.setSpeed(0, 5000); drive.Forward(); wait(0.01); drive.Break(); position = trs.readLine(sensorValues); } while(sensorValues[2] >= 700); continue; } } else { multiples = 2.4f * multiples2; } last_proportional = proportional; left *= multiples; right *= multiples; drive.setSpeed((int)left, (int)right); drive.Forward(); wait(0.005); drive.Break(); // myOled.clearDisplay(); // myOled.setTextCursor(0, 0); // // for oled debug // for(int i=0; i<5; i++){ // myOled.printf("%d\t", sensorValues[i]); // myOled.display(); // } // // myOled.printf("\r\np:%d\r\npo:%d\r\n", position, power_difference); // myOled.display(); // myOled.printf("\r\nleft:%d\r\nright:%d\r\n", (int)left, (int)right); // myOled.display(); } } 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 (true) { //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 setupa myOled.begin(); i2c_for_irsensor.frequency(100000); // main에 써야함 PCF8574Initialize(); beep_off; PCF8574Write(0xC0 | PCF8574Read()); // 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*/ Thread thread; thread.start(bottom_led); // bottom_led(); while(true){ beep_off; myOled.display(); } }