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@96:7465ab270e7a, 2019-06-13 (annotated)

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