final_8조 / Mbed OS Line-Tracking

ss

Dependencies:   WS2812 PixelArray Adafruit_GFX

main.cpp@2:a3f7435f9475, 2019-06-15 (annotated)

Committer:
eunsong
Date:
Sat Jun 15 13:42:33 2019 +0000
Revision:
2:a3f7435f9475
Parent:
0:27e31cadeb36
Child:
3:700a0cf6beea
real final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eunsong 0:27e31cadeb36 1 #include "mbed.h"
eunsong 0:27e31cadeb36 2 #include "IRreflection.h"
eunsong 0:27e31cadeb36 3 #include "ReceiverIR.h"
eunsong 0:27e31cadeb36 4 #include "PCF8574.h"
eunsong 0:27e31cadeb36 5 #include "hcsr04.h"
eunsong 0:27e31cadeb36 6 #include "Adafruit_SSD1306.h"
eunsong 0:27e31cadeb36 7 #include "WS2812.h"
eunsong 0:27e31cadeb36 8 #include "PixelArray.h"
eunsong 0:27e31cadeb36 9
eunsong 0:27e31cadeb36 10 #define WS2812_BUF 77 //number of LEDs in the array
eunsong 0:27e31cadeb36 11 #define NUM_COLORS 6 //number of colors to store in the array
eunsong 0:27e31cadeb36 12 #define NUM_STEPS 8 //number of steps between colors
eunsong 0:27e31cadeb36 13
eunsong 0:27e31cadeb36 14 Timer total;
eunsong 0:27e31cadeb36 15 Timer Etime;
eunsong 0:27e31cadeb36 16 Timer Otime;
eunsong 0:27e31cadeb36 17 Timer AngleTimer;
eunsong 0:27e31cadeb36 18
eunsong 0:27e31cadeb36 19 //WS2812 neopixel================
eunsong 0:27e31cadeb36 20 PixelArray px(WS2812_BUF);
eunsong 0:27e31cadeb36 21 WS2812 ws(D7, WS2812_BUF, 6,17,9,14); //nucleo-f411re
eunsong 0:27e31cadeb36 22 Thread neothread;
eunsong 0:27e31cadeb36 23
eunsong 0:27e31cadeb36 24 // temperary=========================
eunsong 0:27e31cadeb36 25 RawSerial pc(USBTX, USBRX, 115200);
eunsong 0:27e31cadeb36 26 //ultrasonic sensor=====================
eunsong 0:27e31cadeb36 27 int limit = 5;
eunsong 0:27e31cadeb36 28 HCSR04 sensor(D3, D2,pc,0,limit);
eunsong 0:27e31cadeb36 29 int turn = 0;
eunsong 0:27e31cadeb36 30 int Ultra_distance = 0;
eunsong 0:27e31cadeb36 31
eunsong 0:27e31cadeb36 32 //PCF8574============================
eunsong 0:27e31cadeb36 33 PCF8574 io(I2C_SDA,I2C_SCL,0x40);
eunsong 0:27e31cadeb36 34 //IR_Reflection : declaration========
eunsong 0:27e31cadeb36 35 TRSensors TR(D11,D12,D13,D10);
eunsong 0:27e31cadeb36 36
eunsong 0:27e31cadeb36 37 static int sensor_for_end[NUMSENSORS];
eunsong 0:27e31cadeb36 38 static int sensor_val[NUMSENSORS];
eunsong 0:27e31cadeb36 39 static int calMin[NUMSENSORS];
eunsong 0:27e31cadeb36 40 static int calMax[NUMSENSORS];
eunsong 0:27e31cadeb36 41 //==================================
eunsong 0:27e31cadeb36 42 //==============Motor===============
eunsong 0:27e31cadeb36 43 PwmOut PWMA(D6); //PB_10,D6
eunsong 0:27e31cadeb36 44 PwmOut PWMB(D5);
eunsong 0:27e31cadeb36 45
eunsong 0:27e31cadeb36 46 DigitalOut AIN1(A1);
eunsong 0:27e31cadeb36 47 DigitalOut AIN2(A0);
eunsong 0:27e31cadeb36 48 DigitalOut BIN1(A2);
eunsong 0:27e31cadeb36 49 DigitalOut BIN2(A3);
eunsong 0:27e31cadeb36 50
eunsong 0:27e31cadeb36 51 #define MIDDLE 2000
eunsong 0:27e31cadeb36 52 int R_PWM;
eunsong 0:27e31cadeb36 53 int L_PWM;
eunsong 0:27e31cadeb36 54 int weight; int P_weight;
eunsong 0:27e31cadeb36 55 int P_temp; int Fix_temp;
eunsong 0:27e31cadeb36 56 int print_c;
eunsong 0:27e31cadeb36 57 int den;
eunsong 0:27e31cadeb36 58 static int on_line[1];
eunsong 0:27e31cadeb36 59 typedef enum{LEFT = 0, RIGHT} DIRE;
eunsong 0:27e31cadeb36 60 DIRE direction;
eunsong 0:27e31cadeb36 61 DIRE Over_direction;
eunsong 0:27e31cadeb36 62 DIRE P_direction;
eunsong 0:27e31cadeb36 63 DIRE Obs_direction;
eunsong 0:27e31cadeb36 64 //==================================
eunsong 0:27e31cadeb36 65 //IR_Remote : declaration===========
eunsong 0:27e31cadeb36 66 ReceiverIR ir_rx(D4);
eunsong 0:27e31cadeb36 67
eunsong 0:27e31cadeb36 68 RemoteIR::Format format;
eunsong 0:27e31cadeb36 69 uint8_t IR_buf[32];
eunsong 0:27e31cadeb36 70 uint32_t IR_buf_sum;
eunsong 0:27e31cadeb36 71 int bitcount;
eunsong 0:27e31cadeb36 72 //==========flag===================
eunsong 0:27e31cadeb36 73
eunsong 0:27e31cadeb36 74 typedef enum{NOK = 0, YOK}flag;
eunsong 0:27e31cadeb36 75 // Nessary change to enum!!!
eunsong 2:a3f7435f9475 76 flag flag_cal_Max;
eunsong 0:27e31cadeb36 77 flag flag_cal_Min;
eunsong 0:27e31cadeb36 78 flag flag_start;
eunsong 0:27e31cadeb36 79 flag flag_over;
eunsong 0:27e31cadeb36 80 flag flag_out;
eunsong 0:27e31cadeb36 81 flag flag_IR;
eunsong 0:27e31cadeb36 82 flag flag_end;
eunsong 0:27e31cadeb36 83 flag flag_angle;
eunsong 0:27e31cadeb36 84 flag flag_obstacle;
eunsong 0:27e31cadeb36 85 flag flag_distance;
eunsong 0:27e31cadeb36 86 flag flag_neo;
eunsong 0:27e31cadeb36 87 //==============================
eunsong 0:27e31cadeb36 88 void Initialization(void);
eunsong 0:27e31cadeb36 89 void Motor_init(void);
eunsong 0:27e31cadeb36 90 void Motor_Stop(void);
eunsong 0:27e31cadeb36 91 void Calibration(void);
eunsong 0:27e31cadeb36 92 void Driving(void);
eunsong 0:27e31cadeb36 93 void Actuator(void);
eunsong 0:27e31cadeb36 94 void Distance_check(void);
eunsong 0:27e31cadeb36 95 void Obstacle_check(void);
eunsong 0:27e31cadeb36 96 void Actuator_Obstacle(int dir);
eunsong 0:27e31cadeb36 97 int End_check(int *sensor_values);
eunsong 0:27e31cadeb36 98 int color_set(uint8_t red,uint8_t green, uint8_t blue);
eunsong 0:27e31cadeb36 99 int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber);
eunsong 0:27e31cadeb36 100 void NeopixelOn(void);
eunsong 0:27e31cadeb36 101
eunsong 0:27e31cadeb36 102 //===========================OLED
eunsong 0:27e31cadeb36 103 class I2CPreInit : public I2C
eunsong 0:27e31cadeb36 104 {
eunsong 0:27e31cadeb36 105 public:
eunsong 0:27e31cadeb36 106 I2CPreInit(PinName sda, PinName scl) : I2C(sda, scl)
eunsong 0:27e31cadeb36 107 {
eunsong 0:27e31cadeb36 108 frequency(400000);
eunsong 0:27e31cadeb36 109 start();
eunsong 0:27e31cadeb36 110 };
eunsong 0:27e31cadeb36 111 };
eunsong 0:27e31cadeb36 112
eunsong 0:27e31cadeb36 113 I2C i2c(D14, D15);
eunsong 0:27e31cadeb36 114 Adafruit_SSD1306_I2c oled(i2c, D9, 0x78, 64, 128);
eunsong 0:27e31cadeb36 115
eunsong 0:27e31cadeb36 116 int main(){
eunsong 0:27e31cadeb36 117 oled.clearDisplay();
eunsong 2:a3f7435f9475 118 oled.printf("Welcome to Alphabot\r\n");
eunsong 0:27e31cadeb36 119 oled.display();
eunsong 0:27e31cadeb36 120
eunsong 0:27e31cadeb36 121 Initialization();
eunsong 0:27e31cadeb36 122
eunsong 0:27e31cadeb36 123 Driving();
eunsong 0:27e31cadeb36 124
eunsong 0:27e31cadeb36 125 if(flag_end){
eunsong 2:a3f7435f9475 126 // neothread.start(&NeopixelOn);
eunsong 0:27e31cadeb36 127 oled.clearDisplay();
eunsong 0:27e31cadeb36 128 int duration = total.read();
eunsong 0:27e31cadeb36 129 oled.printf("Congratulation!! \r\n");
eunsong 0:27e31cadeb36 130 oled.printf("Time is %.3f", total.read());
eunsong 0:27e31cadeb36 131 oled.display();
eunsong 2:a3f7435f9475 132 NeopixelOn();
eunsong 2:a3f7435f9475 133 // wait(10);
eunsong 0:27e31cadeb36 134 }
eunsong 2:a3f7435f9475 135 //NeopixelOn();
eunsong 0:27e31cadeb36 136
eunsong 0:27e31cadeb36 137 }
eunsong 0:27e31cadeb36 138 //====================================================
eunsong 0:27e31cadeb36 139 //==================initialization====================
eunsong 0:27e31cadeb36 140 //====================================================
eunsong 0:27e31cadeb36 141 void Initialization(){
eunsong 0:27e31cadeb36 142
eunsong 0:27e31cadeb36 143 //추후 다른 변수 초기화 전부 이곳에!!!
eunsong 0:27e31cadeb36 144
eunsong 0:27e31cadeb36 145 flag_over = NOK;
eunsong 0:27e31cadeb36 146 flag_IR = NOK;
eunsong 0:27e31cadeb36 147 flag_distance = NOK;
eunsong 2:a3f7435f9475 148 flag_cal_Min = NOK;
eunsong 2:a3f7435f9475 149 flag_cal_Max = NOK;
eunsong 0:27e31cadeb36 150
eunsong 0:27e31cadeb36 151 TR.calibrate_init(calMin,calMax);
eunsong 0:27e31cadeb36 152
eunsong 0:27e31cadeb36 153 Motor_init();
eunsong 0:27e31cadeb36 154
eunsong 0:27e31cadeb36 155 Calibration();
eunsong 0:27e31cadeb36 156
eunsong 0:27e31cadeb36 157 }
eunsong 0:27e31cadeb36 158
eunsong 0:27e31cadeb36 159 void Motor_Stop(){
eunsong 0:27e31cadeb36 160 pc.printf("===============================Motor Stop!\r\n");
eunsong 0:27e31cadeb36 161 AIN1 = 0;
eunsong 0:27e31cadeb36 162 AIN2 = 0;
eunsong 0:27e31cadeb36 163 BIN1 = 0;
eunsong 0:27e31cadeb36 164 BIN2 = 0;
eunsong 0:27e31cadeb36 165 PWMB.pulsewidth_us(0);
eunsong 0:27e31cadeb36 166 PWMA.pulsewidth_us(0);
eunsong 0:27e31cadeb36 167 }
eunsong 0:27e31cadeb36 168
eunsong 0:27e31cadeb36 169 void Motor_init(){
eunsong 0:27e31cadeb36 170 AIN1 = 0;
eunsong 0:27e31cadeb36 171 AIN2 = 1;
eunsong 0:27e31cadeb36 172 BIN1 = 0;
eunsong 0:27e31cadeb36 173 BIN2 = 1;
eunsong 0:27e31cadeb36 174 PWMB.period_us(500);
eunsong 0:27e31cadeb36 175 PWMA.period_us(500);
eunsong 0:27e31cadeb36 176
eunsong 0:27e31cadeb36 177 den = 0;
eunsong 0:27e31cadeb36 178 R_PWM = 0;
eunsong 0:27e31cadeb36 179 L_PWM = 0;
eunsong 0:27e31cadeb36 180 weight= 0;
eunsong 0:27e31cadeb36 181 print_c = 0;
eunsong 0:27e31cadeb36 182
eunsong 0:27e31cadeb36 183
eunsong 0:27e31cadeb36 184 }
eunsong 0:27e31cadeb36 185 void Calibration(){
eunsong 0:27e31cadeb36 186
eunsong 0:27e31cadeb36 187 while(flag_cal_Max == NOK){
eunsong 0:27e31cadeb36 188
eunsong 0:27e31cadeb36 189 if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
eunsong 0:27e31cadeb36 190 for(int i =0 ; i<4; i ++){
eunsong 0:27e31cadeb36 191 IR_buf_sum |=(IR_buf[i]<<8*(3-i));
eunsong 2:a3f7435f9475 192 }
eunsong 0:27e31cadeb36 193 pc.printf("%X\r\n",IR_buf_sum);
eunsong 0:27e31cadeb36 194 if(IR_buf_sum == 0x00FF0CF3){
eunsong 0:27e31cadeb36 195 io.write(0x7F);
eunsong 0:27e31cadeb36 196 TR.calibrate(sensor_val, calMin, calMax);
eunsong 0:27e31cadeb36 197 wait(0.5);
eunsong 2:a3f7435f9475 198 TR.calibrate(sensor_val, calMin, calMax);
eunsong 0:27e31cadeb36 199 io.write(0xFF);
eunsong 0:27e31cadeb36 200 for(int i = 0; i<5; i++){
eunsong 0:27e31cadeb36 201 pc.printf("Min_sensor_value[%d] = %d\r\n",i+1,calMin[i]);
eunsong 0:27e31cadeb36 202 pc.printf("Max_sensor_value[%d] = %d\r\n",i+1,calMax[i]);
eunsong 0:27e31cadeb36 203 }
eunsong 0:27e31cadeb36 204 pc.printf("================================\r\n");
eunsong 0:27e31cadeb36 205
eunsong 0:27e31cadeb36 206 flag_cal_Max = YOK;
eunsong 0:27e31cadeb36 207 }
eunsong 0:27e31cadeb36 208 IR_buf_sum = 0;
eunsong 0:27e31cadeb36 209 }
eunsong 0:27e31cadeb36 210 }
eunsong 0:27e31cadeb36 211
eunsong 0:27e31cadeb36 212 while(flag_cal_Min == NOK){
eunsong 0:27e31cadeb36 213
eunsong 0:27e31cadeb36 214 if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
eunsong 0:27e31cadeb36 215 for(int i =0 ; i<4; i ++){
eunsong 0:27e31cadeb36 216 IR_buf_sum |=(IR_buf[i]<<8*(3-i));
eunsong 0:27e31cadeb36 217 }
eunsong 0:27e31cadeb36 218 pc.printf("%X\r\n",IR_buf_sum);
eunsong 0:27e31cadeb36 219 if(IR_buf_sum == 0x00FF18E7){
eunsong 0:27e31cadeb36 220 io.write(0xBF);
eunsong 0:27e31cadeb36 221 TR.calibrate(sensor_val, calMin, calMax);
eunsong 0:27e31cadeb36 222 wait(0.5);
eunsong 2:a3f7435f9475 223 TR.calibrate(sensor_val, calMin, calMax);
eunsong 0:27e31cadeb36 224 io.write(0xFF);
eunsong 2:a3f7435f9475 225
eunsong 0:27e31cadeb36 226 for(int i = 0; i<5; i++){
eunsong 0:27e31cadeb36 227 pc.printf("Min_sensor_value[%d] = %d\r\n",i+1,calMin[i]);
eunsong 0:27e31cadeb36 228 pc.printf("Max_sensor_value[%d] = %d\r\n",i+1,calMax[i]);
eunsong 0:27e31cadeb36 229 }
eunsong 0:27e31cadeb36 230 pc.printf("================================\r\n");
eunsong 0:27e31cadeb36 231 flag_cal_Min = YOK;
eunsong 0:27e31cadeb36 232 }
eunsong 0:27e31cadeb36 233 IR_buf_sum = 0;
eunsong 0:27e31cadeb36 234 }
eunsong 0:27e31cadeb36 235 }
eunsong 0:27e31cadeb36 236
eunsong 2:a3f7435f9475 237
eunsong 0:27e31cadeb36 238
eunsong 0:27e31cadeb36 239 //=================start===============================
eunsong 0:27e31cadeb36 240 while(flag_start == NOK){
eunsong 0:27e31cadeb36 241
eunsong 0:27e31cadeb36 242 if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
eunsong 0:27e31cadeb36 243 for(int i =0 ; i<4; i ++){
eunsong 0:27e31cadeb36 244 IR_buf_sum |=(IR_buf[i]<<8*(3-i));
eunsong 0:27e31cadeb36 245 }
eunsong 0:27e31cadeb36 246
eunsong 0:27e31cadeb36 247 pc.printf("%X\r\n",IR_buf_sum);
eunsong 0:27e31cadeb36 248 if(IR_buf_sum == 0x00FF5EA1){
eunsong 0:27e31cadeb36 249 pc.printf("===============start!===============\r\n");
eunsong 0:27e31cadeb36 250 flag_start = YOK;
eunsong 0:27e31cadeb36 251 }
eunsong 0:27e31cadeb36 252 IR_buf_sum = 0;
eunsong 0:27e31cadeb36 253 }
eunsong 0:27e31cadeb36 254 }
eunsong 0:27e31cadeb36 255 //================================================================
eunsong 0:27e31cadeb36 256 }
eunsong 0:27e31cadeb36 257
eunsong 0:27e31cadeb36 258
eunsong 0:27e31cadeb36 259
eunsong 0:27e31cadeb36 260
eunsong 0:27e31cadeb36 261
eunsong 0:27e31cadeb36 262
eunsong 0:27e31cadeb36 263
eunsong 0:27e31cadeb36 264
eunsong 0:27e31cadeb36 265 //Using logic control=================================================================
eunsong 0:27e31cadeb36 266 void Driving(){
eunsong 0:27e31cadeb36 267
eunsong 0:27e31cadeb36 268
eunsong 0:27e31cadeb36 269 total.start();
eunsong 0:27e31cadeb36 270 Etime.start();
eunsong 0:27e31cadeb36 271 while(!flag_end){
eunsong 0:27e31cadeb36 272 Etime.reset();
eunsong 0:27e31cadeb36 273
eunsong 0:27e31cadeb36 274
eunsong 2:a3f7435f9475 275 Obstacle_check();
eunsong 0:27e31cadeb36 276
eunsong 0:27e31cadeb36 277 if(flag_distance == NOK){
eunsong 0:27e31cadeb36 278 Actuator();
eunsong 0:27e31cadeb36 279 }
eunsong 0:27e31cadeb36 280
eunsong 0:27e31cadeb36 281
eunsong 0:27e31cadeb36 282 Distance_check();
eunsong 0:27e31cadeb36 283
eunsong 0:27e31cadeb36 284
eunsong 0:27e31cadeb36 285 do{
eunsong 2:a3f7435f9475 286 }while(Etime.read()<=0.0075);
eunsong 2:a3f7435f9475 287
eunsong 0:27e31cadeb36 288 }
eunsong 0:27e31cadeb36 289
eunsong 0:27e31cadeb36 290 }
eunsong 0:27e31cadeb36 291
eunsong 0:27e31cadeb36 292
eunsong 2:a3f7435f9475 293 int End_check(int *sensor_values){
eunsong 0:27e31cadeb36 294 int avg = 0;
eunsong 0:27e31cadeb36 295 int sum = 0;
eunsong 0:27e31cadeb36 296
eunsong 0:27e31cadeb36 297 for(int i = 0; i < NUMSENSORS; i++){
eunsong 0:27e31cadeb36 298 sum += sensor_values[i];
eunsong 0:27e31cadeb36 299 }
eunsong 0:27e31cadeb36 300 avg = sum / NUMSENSORS;
eunsong 0:27e31cadeb36 301 return avg;
eunsong 0:27e31cadeb36 302 }
eunsong 0:27e31cadeb36 303
eunsong 0:27e31cadeb36 304
eunsong 0:27e31cadeb36 305
eunsong 0:27e31cadeb36 306 void Actuator(){
eunsong 0:27e31cadeb36 307
eunsong 0:27e31cadeb36 308 den = 30;
eunsong 0:27e31cadeb36 309
eunsong 2:a3f7435f9475 310
eunsong 0:27e31cadeb36 311 int temp = TR.readLine(sensor_val, calMin, calMax,on_line,1);
eunsong 2:a3f7435f9475 312
eunsong 0:27e31cadeb36 313
eunsong 0:27e31cadeb36 314 TR.readCalibrated(sensor_for_end, calMin, calMax);
eunsong 0:27e31cadeb36 315 int avg = End_check(sensor_for_end);
eunsong 0:27e31cadeb36 316
eunsong 0:27e31cadeb36 317 if(avg <= 100){
eunsong 0:27e31cadeb36 318 pc.printf("avg:%d\r\n",avg);
eunsong 0:27e31cadeb36 319 Motor_Stop();
eunsong 0:27e31cadeb36 320 flag_end = YOK;
eunsong 0:27e31cadeb36 321 total.stop();
eunsong 0:27e31cadeb36 322 }
eunsong 0:27e31cadeb36 323
eunsong 0:27e31cadeb36 324 temp = temp - MIDDLE;
eunsong 0:27e31cadeb36 325
eunsong 0:27e31cadeb36 326 if(temp>=0) direction = LEFT;
eunsong 0:27e31cadeb36 327 else direction = RIGHT;
eunsong 0:27e31cadeb36 328
eunsong 0:27e31cadeb36 329
eunsong 0:27e31cadeb36 330 weight = abs(temp)/den;
eunsong 0:27e31cadeb36 331
eunsong 0:27e31cadeb36 332 if((print_c%500) == 0){
eunsong 0:27e31cadeb36 333 pc.printf("flag_out = %d\r\n", flag_out);
eunsong 0:27e31cadeb36 334 pc.printf("temp = %d\r\n", temp);
eunsong 0:27e31cadeb36 335 pc.printf("online = %d\r\n", on_line[0]);
eunsong 0:27e31cadeb36 336 }
eunsong 0:27e31cadeb36 337
eunsong 2:a3f7435f9475 338 R_PWM = 135;
eunsong 2:a3f7435f9475 339 L_PWM = 133;
eunsong 0:27e31cadeb36 340
eunsong 0:27e31cadeb36 341
eunsong 0:27e31cadeb36 342 if(weight >= (2000/den)*3/4){
eunsong 0:27e31cadeb36 343 if(direction == LEFT){
eunsong 2:a3f7435f9475 344 PWMA.pulsewidth_us(L_PWM+3+0.0*weight);
eunsong 0:27e31cadeb36 345 PWMB.pulsewidth_us(R_PWM-2.1*weight);
eunsong 0:27e31cadeb36 346 }
eunsong 0:27e31cadeb36 347 else{
eunsong 2:a3f7435f9475 348 PWMA.pulsewidth_us(L_PWM+3-2.1*weight);
eunsong 0:27e31cadeb36 349 PWMB.pulsewidth_us(R_PWM+0.0*weight);
eunsong 0:27e31cadeb36 350 }
eunsong 0:27e31cadeb36 351 }else{
eunsong 0:27e31cadeb36 352
eunsong 0:27e31cadeb36 353 if(direction == LEFT){
eunsong 2:a3f7435f9475 354 PWMA.pulsewidth_us(L_PWM+3+0.0*weight);
eunsong 2:a3f7435f9475 355 PWMB.pulsewidth_us(R_PWM-1.5*weight);
eunsong 0:27e31cadeb36 356 }
eunsong 0:27e31cadeb36 357 else{
eunsong 2:a3f7435f9475 358 PWMA.pulsewidth_us(L_PWM+3-1.5*weight);
eunsong 0:27e31cadeb36 359 PWMB.pulsewidth_us(R_PWM+0.0*weight);
eunsong 0:27e31cadeb36 360 }
eunsong 0:27e31cadeb36 361 }
eunsong 0:27e31cadeb36 362
eunsong 0:27e31cadeb36 363
eunsong 0:27e31cadeb36 364 if(weight >= (2000/den)*2/3 && flag_over == NOK)
eunsong 0:27e31cadeb36 365 {
eunsong 0:27e31cadeb36 366 flag_over = YOK;
eunsong 0:27e31cadeb36 367 P_direction = direction;
eunsong 0:27e31cadeb36 368 P_weight = weight;
eunsong 0:27e31cadeb36 369 }
eunsong 0:27e31cadeb36 370
eunsong 0:27e31cadeb36 371 if((flag_over == YOK) && (abs(weight)<= (2000/den)*1/5)&&on_line[0]==1)
eunsong 0:27e31cadeb36 372 {
eunsong 0:27e31cadeb36 373 if(P_weight >=(2000/den)*2/3 && P_weight<=(2000/den)*4/5){
eunsong 0:27e31cadeb36 374 if(P_direction == LEFT){
eunsong 2:a3f7435f9475 375 PWMA.pulsewidth_us(L_PWM-P_weight+3);
eunsong 0:27e31cadeb36 376 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 377 }
eunsong 0:27e31cadeb36 378 else{
eunsong 2:a3f7435f9475 379 PWMA.pulsewidth_us(L_PWM+3);
eunsong 0:27e31cadeb36 380 PWMB.pulsewidth_us(R_PWM-P_weight);
eunsong 0:27e31cadeb36 381 }
eunsong 0:27e31cadeb36 382 }
eunsong 0:27e31cadeb36 383 if(P_weight>=(2000/den)*4/5){
eunsong 0:27e31cadeb36 384 if(P_direction == LEFT){
eunsong 2:a3f7435f9475 385 PWMA.pulsewidth_us(L_PWM+3-1.0*P_weight);
eunsong 0:27e31cadeb36 386 PWMB.pulsewidth_us(R_PWM+1.0*P_weight);
eunsong 0:27e31cadeb36 387 }
eunsong 0:27e31cadeb36 388 else{
eunsong 2:a3f7435f9475 389 PWMA.pulsewidth_us(L_PWM+3+1.0*P_weight);
eunsong 0:27e31cadeb36 390 PWMB.pulsewidth_us(R_PWM-1.0*P_weight);
eunsong 0:27e31cadeb36 391 }
eunsong 0:27e31cadeb36 392 }
eunsong 0:27e31cadeb36 393 // pc.printf("I'm Here\r\n");
eunsong 0:27e31cadeb36 394
eunsong 0:27e31cadeb36 395 wait(0.12);
eunsong 0:27e31cadeb36 396 flag_over = NOK;
eunsong 0:27e31cadeb36 397 }
eunsong 0:27e31cadeb36 398
eunsong 0:27e31cadeb36 399 P_temp = temp;
eunsong 0:27e31cadeb36 400 print_c++;
eunsong 0:27e31cadeb36 401 }
eunsong 0:27e31cadeb36 402
eunsong 0:27e31cadeb36 403
eunsong 0:27e31cadeb36 404 void Distance_check(){
eunsong 0:27e31cadeb36 405
eunsong 0:27e31cadeb36 406 if(flag_distance == NOK){
eunsong 0:27e31cadeb36 407 sensor.distance();
eunsong 0:27e31cadeb36 408 Ultra_distance = sensor.returndistance();
eunsong 0:27e31cadeb36 409 //pc.printf("distance = %d\r\n",flag_distance);
eunsong 0:27e31cadeb36 410 if((Ultra_distance >= 16 && Ultra_distance <= 19)) flag_distance = YOK;
eunsong 0:27e31cadeb36 411 }
eunsong 0:27e31cadeb36 412
eunsong 0:27e31cadeb36 413 if(flag_distance == YOK){
eunsong 0:27e31cadeb36 414 while(1){
eunsong 0:27e31cadeb36 415 PWMB.pulsewidth_us(100);
eunsong 0:27e31cadeb36 416 PWMA.pulsewidth_us(100);
eunsong 0:27e31cadeb36 417 AIN1.write(1);
eunsong 0:27e31cadeb36 418 AIN2.write(0);
eunsong 0:27e31cadeb36 419 BIN1.write(0);
eunsong 0:27e31cadeb36 420 BIN2.write(1);
eunsong 0:27e31cadeb36 421 if((abs(TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE)<=400 && on_line[0] == 1)){
eunsong 0:27e31cadeb36 422 Motor_init();
eunsong 0:27e31cadeb36 423 flag_distance = NOK;
eunsong 0:27e31cadeb36 424 PWMB.pulsewidth_us(0);
eunsong 0:27e31cadeb36 425 PWMA.pulsewidth_us(0);
eunsong 0:27e31cadeb36 426 break;
eunsong 0:27e31cadeb36 427 }
eunsong 0:27e31cadeb36 428 }
eunsong 0:27e31cadeb36 429 }
eunsong 0:27e31cadeb36 430 }
eunsong 0:27e31cadeb36 431
eunsong 0:27e31cadeb36 432 void Obstacle_check(){
eunsong 0:27e31cadeb36 433
eunsong 0:27e31cadeb36 434 if(io.read() == 0x7F){ //왼쪽읽음
eunsong 0:27e31cadeb36 435 flag_IR = YOK;
eunsong 0:27e31cadeb36 436 flag_obstacle = YOK;
eunsong 0:27e31cadeb36 437 Obs_direction = LEFT;
eunsong 0:27e31cadeb36 438 }
eunsong 0:27e31cadeb36 439 else if(io.read() == 0xbF) //오른쪽 읽음
eunsong 0:27e31cadeb36 440 {
eunsong 0:27e31cadeb36 441 flag_IR = YOK;
eunsong 0:27e31cadeb36 442 flag_obstacle = YOK;
eunsong 0:27e31cadeb36 443 Obs_direction = RIGHT;
eunsong 0:27e31cadeb36 444 }
eunsong 0:27e31cadeb36 445
eunsong 0:27e31cadeb36 446 if(flag_IR){
eunsong 0:27e31cadeb36 447 Otime.start();
eunsong 0:27e31cadeb36 448
eunsong 0:27e31cadeb36 449 while(flag_obstacle == YOK){
eunsong 0:27e31cadeb36 450 Actuator_Obstacle(Obs_direction);
eunsong 0:27e31cadeb36 451 pc.printf("obstacle!\r\n");
eunsong 0:27e31cadeb36 452 }
eunsong 0:27e31cadeb36 453 Otime.stop();
eunsong 0:27e31cadeb36 454 Otime.reset();
eunsong 0:27e31cadeb36 455 flag_IR = NOK;
eunsong 0:27e31cadeb36 456 }
eunsong 0:27e31cadeb36 457
eunsong 0:27e31cadeb36 458 }
eunsong 0:27e31cadeb36 459
eunsong 0:27e31cadeb36 460 void Actuator_Obstacle(int dir){
eunsong 0:27e31cadeb36 461
eunsong 0:27e31cadeb36 462 R_PWM = 100;
eunsong 0:27e31cadeb36 463 L_PWM = 100;
eunsong 0:27e31cadeb36 464
eunsong 0:27e31cadeb36 465 if(Otime.read() <= 0.5){
eunsong 0:27e31cadeb36 466
eunsong 0:27e31cadeb36 467 if(dir == 0){
eunsong 0:27e31cadeb36 468 PWMA.pulsewidth_us(L_PWM-20);
eunsong 0:27e31cadeb36 469 PWMB.pulsewidth_us(R_PWM-100);
eunsong 0:27e31cadeb36 470 }
eunsong 0:27e31cadeb36 471 else if(dir == 1 ){
eunsong 0:27e31cadeb36 472 PWMA.pulsewidth_us(L_PWM-100);
eunsong 2:a3f7435f9475 473 PWMB.pulsewidth_us(R_PWM-30);
eunsong 0:27e31cadeb36 474 }
eunsong 0:27e31cadeb36 475
eunsong 0:27e31cadeb36 476 }else if(Otime.read() <= 1.0){
eunsong 0:27e31cadeb36 477
eunsong 0:27e31cadeb36 478 if(dir == 0){
eunsong 0:27e31cadeb36 479 PWMA.pulsewidth_us(L_PWM+5);
eunsong 0:27e31cadeb36 480 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 481 }
eunsong 0:27e31cadeb36 482 else if(dir == 1 ){
eunsong 0:27e31cadeb36 483 PWMA.pulsewidth_us(L_PWM+5);
eunsong 0:27e31cadeb36 484 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 485 }
eunsong 0:27e31cadeb36 486
eunsong 0:27e31cadeb36 487 }else if(Otime.read() <= 2.0){
eunsong 0:27e31cadeb36 488
eunsong 0:27e31cadeb36 489 if(dir == 0){
eunsong 0:27e31cadeb36 490 PWMA.pulsewidth_us(L_PWM-100);
eunsong 2:a3f7435f9475 491 PWMB.pulsewidth_us(R_PWM-30);
eunsong 0:27e31cadeb36 492 }
eunsong 0:27e31cadeb36 493 else if(dir == 1 ){
eunsong 2:a3f7435f9475 494 PWMA.pulsewidth_us(L_PWM-10);
eunsong 0:27e31cadeb36 495 PWMB.pulsewidth_us(R_PWM-100);
eunsong 0:27e31cadeb36 496 }
eunsong 0:27e31cadeb36 497
eunsong 0:27e31cadeb36 498 }else{
eunsong 0:27e31cadeb36 499
eunsong 0:27e31cadeb36 500 if((abs(TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE)<=400 && on_line[0] == 1)) flag_obstacle = NOK;
eunsong 0:27e31cadeb36 501
eunsong 0:27e31cadeb36 502 PWMA.pulsewidth_us(L_PWM);
eunsong 0:27e31cadeb36 503 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 504
eunsong 0:27e31cadeb36 505 }
eunsong 0:27e31cadeb36 506
eunsong 0:27e31cadeb36 507 }
eunsong 0:27e31cadeb36 508
eunsong 0:27e31cadeb36 509
eunsong 0:27e31cadeb36 510 int color_set(uint8_t red,uint8_t green, uint8_t blue)
eunsong 0:27e31cadeb36 511 {
eunsong 0:27e31cadeb36 512 return ((red<<16) + (green<<8) + blue);
eunsong 0:27e31cadeb36 513 }
eunsong 0:27e31cadeb36 514
eunsong 0:27e31cadeb36 515 // 0 <= stepNumber <= lastStepNumber
eunsong 0:27e31cadeb36 516 int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)
eunsong 0:27e31cadeb36 517 {
eunsong 0:27e31cadeb36 518 return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
eunsong 0:27e31cadeb36 519 }
eunsong 0:27e31cadeb36 520
eunsong 0:27e31cadeb36 521 void NeopixelOn(){
eunsong 0:27e31cadeb36 522 int colorIdx = 0;
eunsong 0:27e31cadeb36 523 int colorTo = 0;
eunsong 0:27e31cadeb36 524 int colorFrom = 0;
eunsong 0:27e31cadeb36 525
eunsong 0:27e31cadeb36 526 uint8_t ir = 0;
eunsong 0:27e31cadeb36 527 uint8_t ig = 0;
eunsong 0:27e31cadeb36 528 uint8_t ib = 0;
eunsong 0:27e31cadeb36 529
eunsong 0:27e31cadeb36 530 ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
eunsong 0:27e31cadeb36 531
eunsong 0:27e31cadeb36 532 // set up the colours we want to draw with
eunsong 0:27e31cadeb36 533 int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
eunsong 0:27e31cadeb36 534
eunsong 0:27e31cadeb36 535 // Now the buffer is written, write it to the led array.
eunsong 2:a3f7435f9475 536 while (1)
eunsong 0:27e31cadeb36 537 {
eunsong 2:a3f7435f9475 538 //pc.printf("thread\r\n");
eunsong 0:27e31cadeb36 539 //get starting RGB components for interpolation
eunsong 0:27e31cadeb36 540 std::size_t c1 = colorbuf[colorFrom];
eunsong 0:27e31cadeb36 541 std::size_t r1 = (c1 & 0xff0000) >> 16;
eunsong 0:27e31cadeb36 542 std::size_t g1 = (c1 & 0x00ff00) >> 8;
eunsong 0:27e31cadeb36 543 std::size_t b1 = (c1 & 0x0000ff);
eunsong 0:27e31cadeb36 544
eunsong 0:27e31cadeb36 545 //get ending RGB components for interpolation
eunsong 0:27e31cadeb36 546 std::size_t c2 = colorbuf[colorTo];
eunsong 0:27e31cadeb36 547 std::size_t r2 = (c2 & 0xff0000) >> 16;
eunsong 0:27e31cadeb36 548 std::size_t g2 = (c2 & 0x00ff00) >> 8;
eunsong 0:27e31cadeb36 549 std::size_t b2 = (c2 & 0x0000ff);
eunsong 0:27e31cadeb36 550
eunsong 0:27e31cadeb36 551 for (int i = 0; i <= NUM_STEPS; i++)
eunsong 0:27e31cadeb36 552 {
eunsong 0:27e31cadeb36 553 ir = interpolate(r1, r2, i, NUM_STEPS);
eunsong 0:27e31cadeb36 554 ig = interpolate(g1, g2, i, NUM_STEPS);
eunsong 0:27e31cadeb36 555 ib = interpolate(b1, b2, i, NUM_STEPS);
eunsong 0:27e31cadeb36 556
eunsong 0:27e31cadeb36 557 //write the color value for each pixel
eunsong 0:27e31cadeb36 558 px.SetAll(color_set(ir,ig,ib));
eunsong 0:27e31cadeb36 559
eunsong 0:27e31cadeb36 560 //write the II value for each pixel
eunsong 0:27e31cadeb36 561 px.SetAllI(32);
eunsong 0:27e31cadeb36 562
eunsong 0:27e31cadeb36 563 for (int i = WS2812_BUF; i >= 0; i--)
eunsong 0:27e31cadeb36 564 {
eunsong 0:27e31cadeb36 565 ws.write(px.getBuf());
eunsong 0:27e31cadeb36 566 }
eunsong 0:27e31cadeb36 567 }
eunsong 0:27e31cadeb36 568
eunsong 0:27e31cadeb36 569 colorFrom = colorIdx;
eunsong 0:27e31cadeb36 570 colorIdx++;
eunsong 0:27e31cadeb36 571
eunsong 0:27e31cadeb36 572 if (colorIdx >= NUM_COLORS)
eunsong 0:27e31cadeb36 573 {
eunsong 0:27e31cadeb36 574 colorIdx = 0;
eunsong 0:27e31cadeb36 575 }
eunsong 0:27e31cadeb36 576
eunsong 0:27e31cadeb36 577 colorTo = colorIdx;
eunsong 0:27e31cadeb36 578 }
eunsong 2:a3f7435f9475 579 }