final_8조 / Mbed OS Line-Tracking

ss

Dependencies:   WS2812 PixelArray Adafruit_GFX

main.cpp@0:27e31cadeb36, 2019-06-15 (annotated)

Committer:
eunsong
Date:
Sat Jun 15 13:09:31 2019 +0000
Revision:
0:27e31cadeb36
Child:
2:a3f7435f9475
yes

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 0:27e31cadeb36 76 flag flag_cal_Min;
eunsong 0:27e31cadeb36 77 flag flag_cal_Max;
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 neothread.start(&NeopixelOn);
eunsong 0:27e31cadeb36 118 oled.clearDisplay();
eunsong 0:27e31cadeb36 119 oled.printf("Welcome to Alphabot\r\n\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 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 0:27e31cadeb36 132 flag_neo = YOK;
eunsong 0:27e31cadeb36 133 }
eunsong 0:27e31cadeb36 134
eunsong 0:27e31cadeb36 135
eunsong 0:27e31cadeb36 136 }
eunsong 0:27e31cadeb36 137 //====================================================
eunsong 0:27e31cadeb36 138 //==================initialization====================
eunsong 0:27e31cadeb36 139 //====================================================
eunsong 0:27e31cadeb36 140 void Initialization(){
eunsong 0:27e31cadeb36 141
eunsong 0:27e31cadeb36 142 //추후 다른 변수 초기화 전부 이곳에!!!
eunsong 0:27e31cadeb36 143
eunsong 0:27e31cadeb36 144 flag_over = NOK;
eunsong 0:27e31cadeb36 145 flag_IR = NOK;
eunsong 0:27e31cadeb36 146 flag_distance = NOK;
eunsong 0:27e31cadeb36 147
eunsong 0:27e31cadeb36 148 TR.calibrate_init(calMin,calMax);
eunsong 0:27e31cadeb36 149
eunsong 0:27e31cadeb36 150 Motor_init();
eunsong 0:27e31cadeb36 151
eunsong 0:27e31cadeb36 152 Calibration();
eunsong 0:27e31cadeb36 153
eunsong 0:27e31cadeb36 154 }
eunsong 0:27e31cadeb36 155
eunsong 0:27e31cadeb36 156 void Motor_Stop(){
eunsong 0:27e31cadeb36 157 pc.printf("===============================Motor Stop!\r\n");
eunsong 0:27e31cadeb36 158 AIN1 = 0;
eunsong 0:27e31cadeb36 159 AIN2 = 0;
eunsong 0:27e31cadeb36 160 BIN1 = 0;
eunsong 0:27e31cadeb36 161 BIN2 = 0;
eunsong 0:27e31cadeb36 162 PWMB.pulsewidth_us(0);
eunsong 0:27e31cadeb36 163 PWMA.pulsewidth_us(0);
eunsong 0:27e31cadeb36 164 }
eunsong 0:27e31cadeb36 165
eunsong 0:27e31cadeb36 166 void Motor_init(){
eunsong 0:27e31cadeb36 167 AIN1 = 0;
eunsong 0:27e31cadeb36 168 AIN2 = 1;
eunsong 0:27e31cadeb36 169 BIN1 = 0;
eunsong 0:27e31cadeb36 170 BIN2 = 1;
eunsong 0:27e31cadeb36 171 PWMB.period_us(500);
eunsong 0:27e31cadeb36 172 PWMA.period_us(500);
eunsong 0:27e31cadeb36 173
eunsong 0:27e31cadeb36 174 den = 0;
eunsong 0:27e31cadeb36 175 R_PWM = 0;
eunsong 0:27e31cadeb36 176 L_PWM = 0;
eunsong 0:27e31cadeb36 177 weight= 0;
eunsong 0:27e31cadeb36 178 print_c = 0;
eunsong 0:27e31cadeb36 179
eunsong 0:27e31cadeb36 180
eunsong 0:27e31cadeb36 181 }
eunsong 0:27e31cadeb36 182 void Calibration(){
eunsong 0:27e31cadeb36 183
eunsong 0:27e31cadeb36 184 while(flag_cal_Max == NOK){
eunsong 0:27e31cadeb36 185
eunsong 0:27e31cadeb36 186 if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
eunsong 0:27e31cadeb36 187 for(int i =0 ; i<4; i ++){
eunsong 0:27e31cadeb36 188 IR_buf_sum |=(IR_buf[i]<<8*(3-i));
eunsong 0:27e31cadeb36 189 }
eunsong 0:27e31cadeb36 190 pc.printf("%X\r\n",IR_buf_sum);
eunsong 0:27e31cadeb36 191 if(IR_buf_sum == 0x00FF0CF3){
eunsong 0:27e31cadeb36 192 io.write(0x7F);
eunsong 0:27e31cadeb36 193 TR.calibrate(sensor_val, calMin, calMax);
eunsong 0:27e31cadeb36 194 wait(0.5);
eunsong 0:27e31cadeb36 195 io.write(0xFF);
eunsong 0:27e31cadeb36 196 for(int i = 0; i<5; i++){
eunsong 0:27e31cadeb36 197 pc.printf("Min_sensor_value[%d] = %d\r\n",i+1,calMin[i]);
eunsong 0:27e31cadeb36 198 pc.printf("Max_sensor_value[%d] = %d\r\n",i+1,calMax[i]);
eunsong 0:27e31cadeb36 199 }
eunsong 0:27e31cadeb36 200 pc.printf("================================\r\n");
eunsong 0:27e31cadeb36 201
eunsong 0:27e31cadeb36 202 flag_cal_Max = YOK;
eunsong 0:27e31cadeb36 203 }
eunsong 0:27e31cadeb36 204 IR_buf_sum = 0;
eunsong 0:27e31cadeb36 205 }
eunsong 0:27e31cadeb36 206 }
eunsong 0:27e31cadeb36 207
eunsong 0:27e31cadeb36 208 while(flag_cal_Min == NOK){
eunsong 0:27e31cadeb36 209
eunsong 0:27e31cadeb36 210 if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
eunsong 0:27e31cadeb36 211 for(int i =0 ; i<4; i ++){
eunsong 0:27e31cadeb36 212 IR_buf_sum |=(IR_buf[i]<<8*(3-i));
eunsong 0:27e31cadeb36 213 }
eunsong 0:27e31cadeb36 214 pc.printf("%X\r\n",IR_buf_sum);
eunsong 0:27e31cadeb36 215 if(IR_buf_sum == 0x00FF18E7){
eunsong 0:27e31cadeb36 216 io.write(0xBF);
eunsong 0:27e31cadeb36 217 TR.calibrate(sensor_val, calMin, calMax);
eunsong 0:27e31cadeb36 218 wait(0.5);
eunsong 0:27e31cadeb36 219 io.write(0xFF);
eunsong 0:27e31cadeb36 220 for(int i = 0; i<5; i++){
eunsong 0:27e31cadeb36 221 pc.printf("Min_sensor_value[%d] = %d\r\n",i+1,calMin[i]);
eunsong 0:27e31cadeb36 222 pc.printf("Max_sensor_value[%d] = %d\r\n",i+1,calMax[i]);
eunsong 0:27e31cadeb36 223 }
eunsong 0:27e31cadeb36 224 pc.printf("================================\r\n");
eunsong 0:27e31cadeb36 225 flag_cal_Min = YOK;
eunsong 0:27e31cadeb36 226 }
eunsong 0:27e31cadeb36 227 IR_buf_sum = 0;
eunsong 0:27e31cadeb36 228 }
eunsong 0:27e31cadeb36 229 }
eunsong 0:27e31cadeb36 230
eunsong 0:27e31cadeb36 231
eunsong 0:27e31cadeb36 232
eunsong 0:27e31cadeb36 233 //=================start===============================
eunsong 0:27e31cadeb36 234 while(flag_start == NOK){
eunsong 0:27e31cadeb36 235
eunsong 0:27e31cadeb36 236 if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
eunsong 0:27e31cadeb36 237 for(int i =0 ; i<4; i ++){
eunsong 0:27e31cadeb36 238 IR_buf_sum |=(IR_buf[i]<<8*(3-i));
eunsong 0:27e31cadeb36 239 }
eunsong 0:27e31cadeb36 240
eunsong 0:27e31cadeb36 241 pc.printf("%X\r\n",IR_buf_sum);
eunsong 0:27e31cadeb36 242 if(IR_buf_sum == 0x00FF5EA1){
eunsong 0:27e31cadeb36 243 pc.printf("===============start!===============\r\n");
eunsong 0:27e31cadeb36 244 flag_start = YOK;
eunsong 0:27e31cadeb36 245 }
eunsong 0:27e31cadeb36 246 IR_buf_sum = 0;
eunsong 0:27e31cadeb36 247 }
eunsong 0:27e31cadeb36 248 }
eunsong 0:27e31cadeb36 249 //================================================================
eunsong 0:27e31cadeb36 250 }
eunsong 0:27e31cadeb36 251
eunsong 0:27e31cadeb36 252
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 //Using logic control=================================================================
eunsong 0:27e31cadeb36 260 void Driving(){
eunsong 0:27e31cadeb36 261
eunsong 0:27e31cadeb36 262
eunsong 0:27e31cadeb36 263 total.start();
eunsong 0:27e31cadeb36 264
eunsong 0:27e31cadeb36 265 Etime.start();
eunsong 0:27e31cadeb36 266 while(!flag_end){
eunsong 0:27e31cadeb36 267 Etime.reset();
eunsong 0:27e31cadeb36 268
eunsong 0:27e31cadeb36 269
eunsong 0:27e31cadeb36 270 // Obstacle_check();
eunsong 0:27e31cadeb36 271
eunsong 0:27e31cadeb36 272 if(flag_distance == NOK){
eunsong 0:27e31cadeb36 273 Actuator();
eunsong 0:27e31cadeb36 274 }
eunsong 0:27e31cadeb36 275
eunsong 0:27e31cadeb36 276
eunsong 0:27e31cadeb36 277 Distance_check();
eunsong 0:27e31cadeb36 278
eunsong 0:27e31cadeb36 279
eunsong 0:27e31cadeb36 280 do{
eunsong 0:27e31cadeb36 281 }while(Etime.read()<0.00300001);
eunsong 0:27e31cadeb36 282 }
eunsong 0:27e31cadeb36 283
eunsong 0:27e31cadeb36 284 }
eunsong 0:27e31cadeb36 285
eunsong 0:27e31cadeb36 286
eunsong 0:27e31cadeb36 287 int End_check(int *sensor_values){
eunsong 0:27e31cadeb36 288 int avg = 0;
eunsong 0:27e31cadeb36 289 int sum = 0;
eunsong 0:27e31cadeb36 290
eunsong 0:27e31cadeb36 291 for(int i = 0; i < NUMSENSORS; i++){
eunsong 0:27e31cadeb36 292 sum += sensor_values[i];
eunsong 0:27e31cadeb36 293 }
eunsong 0:27e31cadeb36 294 avg = sum / NUMSENSORS;
eunsong 0:27e31cadeb36 295 //pc.printf("\tavg function: %d\r\n",avg);
eunsong 0:27e31cadeb36 296 return avg;
eunsong 0:27e31cadeb36 297 }
eunsong 0:27e31cadeb36 298
eunsong 0:27e31cadeb36 299
eunsong 0:27e31cadeb36 300
eunsong 0:27e31cadeb36 301 void Actuator(){
eunsong 0:27e31cadeb36 302
eunsong 0:27e31cadeb36 303 den = 30;
eunsong 0:27e31cadeb36 304
eunsong 0:27e31cadeb36 305 //int temp = TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE;
eunsong 0:27e31cadeb36 306 int temp = TR.readLine(sensor_val, calMin, calMax,on_line,1);
eunsong 0:27e31cadeb36 307 //pc.printf("new temp: %d\r\n",temp);
eunsong 0:27e31cadeb36 308
eunsong 0:27e31cadeb36 309 TR.readCalibrated(sensor_for_end, calMin, calMax);
eunsong 0:27e31cadeb36 310 int avg = End_check(sensor_for_end);
eunsong 0:27e31cadeb36 311
eunsong 0:27e31cadeb36 312 if(avg <= 100){
eunsong 0:27e31cadeb36 313 pc.printf("avg:%d\r\n",avg);
eunsong 0:27e31cadeb36 314 Motor_Stop();
eunsong 0:27e31cadeb36 315 flag_end = YOK;
eunsong 0:27e31cadeb36 316 total.stop();
eunsong 0:27e31cadeb36 317 }
eunsong 0:27e31cadeb36 318
eunsong 0:27e31cadeb36 319 temp = temp - MIDDLE;
eunsong 0:27e31cadeb36 320
eunsong 0:27e31cadeb36 321 if(temp>=0) direction = LEFT;
eunsong 0:27e31cadeb36 322 else direction = RIGHT;
eunsong 0:27e31cadeb36 323
eunsong 0:27e31cadeb36 324
eunsong 0:27e31cadeb36 325 weight = abs(temp)/den;
eunsong 0:27e31cadeb36 326
eunsong 0:27e31cadeb36 327 if((print_c%500) == 0){
eunsong 0:27e31cadeb36 328 pc.printf("flag_out = %d\r\n", flag_out);
eunsong 0:27e31cadeb36 329 pc.printf("temp = %d\r\n", temp);
eunsong 0:27e31cadeb36 330 pc.printf("online = %d\r\n", on_line[0]);
eunsong 0:27e31cadeb36 331 }
eunsong 0:27e31cadeb36 332
eunsong 0:27e31cadeb36 333 R_PWM = 140;
eunsong 0:27e31cadeb36 334 L_PWM = 140;
eunsong 0:27e31cadeb36 335
eunsong 0:27e31cadeb36 336
eunsong 0:27e31cadeb36 337
eunsong 0:27e31cadeb36 338 //if(weight > (2000/den)*2/3) weight = (2000/den)*3/4;
eunsong 0:27e31cadeb36 339
eunsong 0:27e31cadeb36 340 if(weight >= (2000/den)*3/4){
eunsong 0:27e31cadeb36 341 if(direction == LEFT){
eunsong 0:27e31cadeb36 342 PWMA.pulsewidth_us(L_PWM+0.0*weight);
eunsong 0:27e31cadeb36 343 PWMB.pulsewidth_us(R_PWM-2.1*weight);
eunsong 0:27e31cadeb36 344 }
eunsong 0:27e31cadeb36 345 else{
eunsong 0:27e31cadeb36 346 PWMA.pulsewidth_us(L_PWM-2.1*weight);
eunsong 0:27e31cadeb36 347 PWMB.pulsewidth_us(R_PWM+0.0*weight);
eunsong 0:27e31cadeb36 348 }
eunsong 0:27e31cadeb36 349 }else{
eunsong 0:27e31cadeb36 350
eunsong 0:27e31cadeb36 351 if(direction == LEFT){
eunsong 0:27e31cadeb36 352 PWMA.pulsewidth_us(L_PWM+0.0*weight);
eunsong 0:27e31cadeb36 353 PWMB.pulsewidth_us(R_PWM-1.4*weight);
eunsong 0:27e31cadeb36 354 }
eunsong 0:27e31cadeb36 355 else{
eunsong 0:27e31cadeb36 356 PWMA.pulsewidth_us(L_PWM-1.4*weight);
eunsong 0:27e31cadeb36 357 PWMB.pulsewidth_us(R_PWM+0.0*weight);
eunsong 0:27e31cadeb36 358 }
eunsong 0:27e31cadeb36 359 }
eunsong 0:27e31cadeb36 360
eunsong 0:27e31cadeb36 361
eunsong 0:27e31cadeb36 362 if(weight >= (2000/den)*2/3 && flag_over == NOK)
eunsong 0:27e31cadeb36 363 {
eunsong 0:27e31cadeb36 364 flag_over = YOK;
eunsong 0:27e31cadeb36 365 P_direction = direction;
eunsong 0:27e31cadeb36 366 P_weight = weight;
eunsong 0:27e31cadeb36 367 }
eunsong 0:27e31cadeb36 368
eunsong 0:27e31cadeb36 369 if((flag_over == YOK) && (abs(weight)<= (2000/den)*1/5)&&on_line[0]==1)
eunsong 0:27e31cadeb36 370 {
eunsong 0:27e31cadeb36 371 if(P_weight >=(2000/den)*2/3 && P_weight<=(2000/den)*4/5){
eunsong 0:27e31cadeb36 372 if(P_direction == LEFT){
eunsong 0:27e31cadeb36 373 PWMA.pulsewidth_us(L_PWM-P_weight);
eunsong 0:27e31cadeb36 374 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 375 }
eunsong 0:27e31cadeb36 376 else{
eunsong 0:27e31cadeb36 377 PWMA.pulsewidth_us(L_PWM);
eunsong 0:27e31cadeb36 378 PWMB.pulsewidth_us(R_PWM-P_weight);
eunsong 0:27e31cadeb36 379 }
eunsong 0:27e31cadeb36 380 }
eunsong 0:27e31cadeb36 381 if(P_weight>=(2000/den)*4/5){
eunsong 0:27e31cadeb36 382 if(P_direction == LEFT){
eunsong 0:27e31cadeb36 383 PWMA.pulsewidth_us(L_PWM-1.0*P_weight);
eunsong 0:27e31cadeb36 384 PWMB.pulsewidth_us(R_PWM+1.0*P_weight);
eunsong 0:27e31cadeb36 385 }
eunsong 0:27e31cadeb36 386 else{
eunsong 0:27e31cadeb36 387 PWMA.pulsewidth_us(L_PWM+1.0*P_weight);
eunsong 0:27e31cadeb36 388 PWMB.pulsewidth_us(R_PWM-1.0*P_weight);
eunsong 0:27e31cadeb36 389 }
eunsong 0:27e31cadeb36 390 }
eunsong 0:27e31cadeb36 391 // pc.printf("I'm Here\r\n");
eunsong 0:27e31cadeb36 392
eunsong 0:27e31cadeb36 393 wait(0.12);
eunsong 0:27e31cadeb36 394 flag_over = NOK;
eunsong 0:27e31cadeb36 395 }
eunsong 0:27e31cadeb36 396
eunsong 0:27e31cadeb36 397 P_temp = temp;
eunsong 0:27e31cadeb36 398 print_c++;
eunsong 0:27e31cadeb36 399 }
eunsong 0:27e31cadeb36 400
eunsong 0:27e31cadeb36 401
eunsong 0:27e31cadeb36 402 void Distance_check(){
eunsong 0:27e31cadeb36 403
eunsong 0:27e31cadeb36 404 if(flag_distance == NOK){
eunsong 0:27e31cadeb36 405 sensor.distance();
eunsong 0:27e31cadeb36 406 Ultra_distance = sensor.returndistance();
eunsong 0:27e31cadeb36 407 //pc.printf("distance = %d\r\n",flag_distance);
eunsong 0:27e31cadeb36 408 if((Ultra_distance >= 16 && Ultra_distance <= 19)) flag_distance = YOK;
eunsong 0:27e31cadeb36 409 }
eunsong 0:27e31cadeb36 410
eunsong 0:27e31cadeb36 411 if(flag_distance == YOK){
eunsong 0:27e31cadeb36 412 while(1){
eunsong 0:27e31cadeb36 413 // pc.printf("distance check. turn left!!\r\n");
eunsong 0:27e31cadeb36 414 PWMB.pulsewidth_us(100);
eunsong 0:27e31cadeb36 415 PWMA.pulsewidth_us(100);
eunsong 0:27e31cadeb36 416 AIN1.write(1);
eunsong 0:27e31cadeb36 417 AIN2.write(0);
eunsong 0:27e31cadeb36 418 BIN1.write(0);
eunsong 0:27e31cadeb36 419 BIN2.write(1);
eunsong 0:27e31cadeb36 420 //pc.printf("ABS == ")
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 //io.write 체크
eunsong 0:27e31cadeb36 435 //flag_IR 변경 YOK
eunsong 0:27e31cadeb36 436 // pc.printf("flag_out = %x\r\n", io.read());
eunsong 0:27e31cadeb36 437 if(io.read() == 0x7F){ //왼쪽읽음
eunsong 0:27e31cadeb36 438 flag_IR = YOK;
eunsong 0:27e31cadeb36 439 flag_obstacle = YOK;
eunsong 0:27e31cadeb36 440 Obs_direction = LEFT;
eunsong 0:27e31cadeb36 441 }
eunsong 0:27e31cadeb36 442 else if(io.read() == 0xbF) //오른쪽 읽음
eunsong 0:27e31cadeb36 443 {
eunsong 0:27e31cadeb36 444 flag_IR = YOK;
eunsong 0:27e31cadeb36 445 flag_obstacle = YOK;
eunsong 0:27e31cadeb36 446 Obs_direction = RIGHT;
eunsong 0:27e31cadeb36 447 }
eunsong 0:27e31cadeb36 448
eunsong 0:27e31cadeb36 449 if(flag_IR){
eunsong 0:27e31cadeb36 450 Otime.start();
eunsong 0:27e31cadeb36 451 //flag_IR = NOK;
eunsong 0:27e31cadeb36 452 // pc.printf("\tActuator Obstacel start\r\n");
eunsong 0:27e31cadeb36 453
eunsong 0:27e31cadeb36 454 while(flag_obstacle == YOK){
eunsong 0:27e31cadeb36 455 //while(io.read() != 0xFF){
eunsong 0:27e31cadeb36 456
eunsong 0:27e31cadeb36 457 Actuator_Obstacle(Obs_direction);
eunsong 0:27e31cadeb36 458 pc.printf("obstacle!\r\n");
eunsong 0:27e31cadeb36 459 //pc.printf("\t\tActuator Obstacel start\r\n");
eunsong 0:27e31cadeb36 460
eunsong 0:27e31cadeb36 461 }
eunsong 0:27e31cadeb36 462 Otime.stop();
eunsong 0:27e31cadeb36 463 Otime.reset();
eunsong 0:27e31cadeb36 464 flag_IR = NOK;
eunsong 0:27e31cadeb36 465 }
eunsong 0:27e31cadeb36 466
eunsong 0:27e31cadeb36 467
eunsong 0:27e31cadeb36 468
eunsong 0:27e31cadeb36 469
eunsong 0:27e31cadeb36 470 }
eunsong 0:27e31cadeb36 471
eunsong 0:27e31cadeb36 472 void Actuator_Obstacle(int dir){
eunsong 0:27e31cadeb36 473
eunsong 0:27e31cadeb36 474 R_PWM = 100;
eunsong 0:27e31cadeb36 475 L_PWM = 100;
eunsong 0:27e31cadeb36 476
eunsong 0:27e31cadeb36 477 if(Otime.read() <= 0.5){
eunsong 0:27e31cadeb36 478
eunsong 0:27e31cadeb36 479 if(dir == 0){
eunsong 0:27e31cadeb36 480 PWMA.pulsewidth_us(L_PWM-20);
eunsong 0:27e31cadeb36 481 PWMB.pulsewidth_us(R_PWM-100);
eunsong 0:27e31cadeb36 482 }
eunsong 0:27e31cadeb36 483 else if(dir == 1 ){
eunsong 0:27e31cadeb36 484 PWMA.pulsewidth_us(L_PWM-100);
eunsong 0:27e31cadeb36 485 PWMB.pulsewidth_us(R_PWM-40);
eunsong 0:27e31cadeb36 486 }
eunsong 0:27e31cadeb36 487
eunsong 0:27e31cadeb36 488 }else if(Otime.read() <= 1.0){
eunsong 0:27e31cadeb36 489
eunsong 0:27e31cadeb36 490 if(dir == 0){
eunsong 0:27e31cadeb36 491 PWMA.pulsewidth_us(L_PWM+5);
eunsong 0:27e31cadeb36 492 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 493 }
eunsong 0:27e31cadeb36 494 else if(dir == 1 ){
eunsong 0:27e31cadeb36 495 PWMA.pulsewidth_us(L_PWM+5);
eunsong 0:27e31cadeb36 496 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 497 }
eunsong 0:27e31cadeb36 498
eunsong 0:27e31cadeb36 499 }else if(Otime.read() <= 2.0){
eunsong 0:27e31cadeb36 500
eunsong 0:27e31cadeb36 501 if(dir == 0){
eunsong 0:27e31cadeb36 502 PWMA.pulsewidth_us(L_PWM-100);
eunsong 0:27e31cadeb36 503 PWMB.pulsewidth_us(R_PWM-40);
eunsong 0:27e31cadeb36 504 }
eunsong 0:27e31cadeb36 505 else if(dir == 1 ){
eunsong 0:27e31cadeb36 506 PWMA.pulsewidth_us(L_PWM-20);
eunsong 0:27e31cadeb36 507 PWMB.pulsewidth_us(R_PWM-100);
eunsong 0:27e31cadeb36 508 }
eunsong 0:27e31cadeb36 509
eunsong 0:27e31cadeb36 510 }else{
eunsong 0:27e31cadeb36 511
eunsong 0:27e31cadeb36 512 if((abs(TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE)<=400 && on_line[0] == 1)) flag_obstacle = NOK;
eunsong 0:27e31cadeb36 513
eunsong 0:27e31cadeb36 514 PWMA.pulsewidth_us(L_PWM);
eunsong 0:27e31cadeb36 515 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 516
eunsong 0:27e31cadeb36 517 }
eunsong 0:27e31cadeb36 518
eunsong 0:27e31cadeb36 519 }
eunsong 0:27e31cadeb36 520
eunsong 0:27e31cadeb36 521
eunsong 0:27e31cadeb36 522 int color_set(uint8_t red,uint8_t green, uint8_t blue)
eunsong 0:27e31cadeb36 523 {
eunsong 0:27e31cadeb36 524 return ((red<<16) + (green<<8) + blue);
eunsong 0:27e31cadeb36 525 }
eunsong 0:27e31cadeb36 526
eunsong 0:27e31cadeb36 527 // 0 <= stepNumber <= lastStepNumber
eunsong 0:27e31cadeb36 528 int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)
eunsong 0:27e31cadeb36 529 {
eunsong 0:27e31cadeb36 530 return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
eunsong 0:27e31cadeb36 531 }
eunsong 0:27e31cadeb36 532
eunsong 0:27e31cadeb36 533 void NeopixelOn(){
eunsong 0:27e31cadeb36 534 int colorIdx = 0;
eunsong 0:27e31cadeb36 535 int colorTo = 0;
eunsong 0:27e31cadeb36 536 int colorFrom = 0;
eunsong 0:27e31cadeb36 537
eunsong 0:27e31cadeb36 538 uint8_t ir = 0;
eunsong 0:27e31cadeb36 539 uint8_t ig = 0;
eunsong 0:27e31cadeb36 540 uint8_t ib = 0;
eunsong 0:27e31cadeb36 541
eunsong 0:27e31cadeb36 542 ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
eunsong 0:27e31cadeb36 543
eunsong 0:27e31cadeb36 544 // set up the colours we want to draw with
eunsong 0:27e31cadeb36 545 int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
eunsong 0:27e31cadeb36 546
eunsong 0:27e31cadeb36 547 // Now the buffer is written, write it to the led array.
eunsong 0:27e31cadeb36 548 while (flag_neo)
eunsong 0:27e31cadeb36 549 {
eunsong 0:27e31cadeb36 550 //get starting RGB components for interpolation
eunsong 0:27e31cadeb36 551 std::size_t c1 = colorbuf[colorFrom];
eunsong 0:27e31cadeb36 552 std::size_t r1 = (c1 & 0xff0000) >> 16;
eunsong 0:27e31cadeb36 553 std::size_t g1 = (c1 & 0x00ff00) >> 8;
eunsong 0:27e31cadeb36 554 std::size_t b1 = (c1 & 0x0000ff);
eunsong 0:27e31cadeb36 555
eunsong 0:27e31cadeb36 556 //get ending RGB components for interpolation
eunsong 0:27e31cadeb36 557 std::size_t c2 = colorbuf[colorTo];
eunsong 0:27e31cadeb36 558 std::size_t r2 = (c2 & 0xff0000) >> 16;
eunsong 0:27e31cadeb36 559 std::size_t g2 = (c2 & 0x00ff00) >> 8;
eunsong 0:27e31cadeb36 560 std::size_t b2 = (c2 & 0x0000ff);
eunsong 0:27e31cadeb36 561
eunsong 0:27e31cadeb36 562 for (int i = 0; i <= NUM_STEPS; i++)
eunsong 0:27e31cadeb36 563 {
eunsong 0:27e31cadeb36 564 ir = interpolate(r1, r2, i, NUM_STEPS);
eunsong 0:27e31cadeb36 565 ig = interpolate(g1, g2, i, NUM_STEPS);
eunsong 0:27e31cadeb36 566 ib = interpolate(b1, b2, i, NUM_STEPS);
eunsong 0:27e31cadeb36 567
eunsong 0:27e31cadeb36 568 //write the color value for each pixel
eunsong 0:27e31cadeb36 569 px.SetAll(color_set(ir,ig,ib));
eunsong 0:27e31cadeb36 570
eunsong 0:27e31cadeb36 571 //write the II value for each pixel
eunsong 0:27e31cadeb36 572 px.SetAllI(32);
eunsong 0:27e31cadeb36 573
eunsong 0:27e31cadeb36 574 for (int i = WS2812_BUF; i >= 0; i--)
eunsong 0:27e31cadeb36 575 {
eunsong 0:27e31cadeb36 576 ws.write(px.getBuf());
eunsong 0:27e31cadeb36 577 }
eunsong 0:27e31cadeb36 578 }
eunsong 0:27e31cadeb36 579
eunsong 0:27e31cadeb36 580 colorFrom = colorIdx;
eunsong 0:27e31cadeb36 581 colorIdx++;
eunsong 0:27e31cadeb36 582
eunsong 0:27e31cadeb36 583 if (colorIdx >= NUM_COLORS)
eunsong 0:27e31cadeb36 584 {
eunsong 0:27e31cadeb36 585 colorIdx = 0;
eunsong 0:27e31cadeb36 586 }
eunsong 0:27e31cadeb36 587
eunsong 0:27e31cadeb36 588 colorTo = colorIdx;
eunsong 0:27e31cadeb36 589 }
eunsong 0:27e31cadeb36 590 }
eunsong 0:27e31cadeb36 591
eunsong 0:27e31cadeb36 592
eunsong 0:27e31cadeb36 593 /*
eunsong 0:27e31cadeb36 594 else if(Otime.read() <= 2.0){
eunsong 0:27e31cadeb36 595
eunsong 0:27e31cadeb36 596 if(dir == 0){
eunsong 0:27e31cadeb36 597 PWMA.pulsewidth_us(L_PWM+5);
eunsong 0:27e31cadeb36 598 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 599 }
eunsong 0:27e31cadeb36 600 else if(dir == 1 ){
eunsong 0:27e31cadeb36 601 PWMA.pulsewidth_us(L_PWM+5);
eunsong 0:27e31cadeb36 602 PWMB.pulsewidth_us(R_PWM);
eunsong 0:27e31cadeb36 603 }
eunsong 0:27e31cadeb36 604
eunsong 0:27e31cadeb36 605 }
eunsong 0:27e31cadeb36 606 */