final_8조 / Mbed OS Line-Tracking

ss

Dependencies:   WS2812 PixelArray Adafruit_GFX

main.cpp@3:700a0cf6beea, 2019-06-15 (annotated)

Committer:
eunsong
Date:
Sat Jun 15 13:48:45 2019 +0000
Revision:
3:700a0cf6beea
Parent:
2:a3f7435f9475 
a

Who changed what in which revision?

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