main.cpp

00001 //8조 최은송 김한결
00002 #include "mbed.h"
00003 #include "IRreflection.h"
00004 #include "ReceiverIR.h"
00005 #include "PCF8574.h"
00006 #include "hcsr04.h"
00007 #include "Adafruit_SSD1306.h"
00008 #include "WS2812.h"
00009 #include "PixelArray.h"
00010 
00011 #define WS2812_BUF 77   //number of LEDs in the array
00012 #define NUM_COLORS 6    //number of colors to store in the array
00013 #define NUM_STEPS 8    //number of steps between colors
00014 
00015 Timer total;
00016 Timer Etime;
00017 Timer Otime;
00018 Timer AngleTimer;
00019 
00020 //WS2812 neopixel================
00021 PixelArray px(WS2812_BUF);
00022 WS2812 ws(D7, WS2812_BUF, 6,17,9,14);   //nucleo-f411re
00023 Thread neothread;
00024 
00025 // temperary=========================
00026 RawSerial pc(USBTX, USBRX, 115200);
00027 //ultrasonic sensor=====================
00028 int limit = 5;
00029 HCSR04 sensor(D3, D2,pc,0,limit); 
00030 int turn = 0;
00031 int Ultra_distance = 0;
00032 
00033 //PCF8574============================
00034 PCF8574 io(I2C_SDA,I2C_SCL,0x40);
00035 //IR_Reflection : declaration========
00036 TRSensors TR(D11,D12,D13,D10);
00037 
00038 static int sensor_for_end[NUMSENSORS];
00039 static int sensor_val[NUMSENSORS];
00040 static int calMin[NUMSENSORS];
00041 static int calMax[NUMSENSORS];
00042 //==================================
00043 //==============Motor===============
00044 PwmOut PWMA(D6);  //PB_10,D6
00045 PwmOut PWMB(D5);
00046 
00047 DigitalOut AIN1(A1);
00048 DigitalOut AIN2(A0);
00049 DigitalOut BIN1(A2);
00050 DigitalOut BIN2(A3);
00051 
00052 #define MIDDLE 2000
00053 int R_PWM;  
00054 int L_PWM;
00055 int weight; int P_weight;
00056 int P_temp; int Fix_temp;
00057 int print_c;
00058 int den;
00059 static int on_line[1];
00060 typedef enum{LEFT = 0, RIGHT} DIRE;
00061 DIRE direction;
00062 DIRE Over_direction;
00063 DIRE P_direction;
00064 DIRE Obs_direction;
00065 //==================================
00066 //IR_Remote : declaration===========
00067 ReceiverIR ir_rx(D4);
00068 
00069 RemoteIR::Format format;
00070 uint8_t IR_buf[32];
00071 uint32_t IR_buf_sum;
00072 int bitcount;
00073 //==========flag===================
00074 
00075 typedef enum{NOK = 0, YOK}flag;
00076 // Nessary change to enum!!!
00077 flag flag_cal_Max;
00078 flag flag_cal_Min;
00079 flag flag_start;
00080 flag flag_over;
00081 flag flag_out;
00082 flag flag_IR;
00083 flag flag_end;
00084 flag flag_angle;
00085 flag flag_obstacle;
00086 flag flag_distance;
00087 flag flag_neo;
00088 //==============================
00089 void Initialization(void);
00090 void Motor_init(void);
00091 void Motor_Stop(void);
00092 void Calibration(void);
00093 void Driving(void);
00094 void Actuator(void);
00095 void Distance_check(void);
00096 void Obstacle_check(void);
00097 void Actuator_Obstacle(int dir);
00098 int End_check(int *sensor_values);
00099 int color_set(uint8_t red,uint8_t green, uint8_t blue);
00100 int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber);
00101 void NeopixelOn(void);
00102 
00103 //===========================OLED
00104 class I2CPreInit : public I2C
00105 {
00106 public:
00107     I2CPreInit(PinName sda, PinName scl) : I2C(sda, scl)
00108     {
00109         frequency(400000);
00110         start();
00111     };
00112 };
00113  
00114 I2C i2c(D14, D15);
00115 Adafruit_SSD1306_I2c oled(i2c, D9, 0x78, 64, 128); 
00116 
00117 int main(){
00118     oled.clearDisplay();
00119     oled.printf("Welcome to Alphabot\r\n");
00120     oled.display();
00121 
00122     Initialization();
00123     
00124     Driving();
00125    
00126     if(flag_end){
00127         // neothread.start(&NeopixelOn);
00128          oled.clearDisplay();
00129          int duration = total.read();
00130          oled.printf("Congratulation!! \r\n");
00131          oled.printf("Time is %.3f", total.read());
00132          oled.display();
00133          NeopixelOn();
00134         // wait(10);
00135     }
00136     //NeopixelOn();
00137 
00138 }
00139 //====================================================
00140 //==================initialization====================
00141 //====================================================
00142 void Initialization(){
00143     
00144     //추후 다른 변수 초기화 전부 이곳에!!!
00145     
00146     flag_over = NOK;
00147     flag_IR = NOK;
00148     flag_distance = NOK;
00149     flag_cal_Min = NOK;
00150     flag_cal_Max = NOK;
00151     
00152     TR.calibrate_init(calMin,calMax);
00153     
00154     Motor_init();
00155     
00156     Calibration();
00157     
00158 }
00159 
00160 void Motor_Stop(){
00161     pc.printf("===============================Motor Stop!\r\n");
00162     AIN1 = 0;
00163     AIN2 = 0;
00164     BIN1 = 0;
00165     BIN2 = 0;
00166     PWMB.pulsewidth_us(0);
00167     PWMA.pulsewidth_us(0);
00168 }
00169 
00170 void Motor_init(){
00171     AIN1 = 0;
00172     AIN2 = 1;
00173     BIN1 = 0;
00174     BIN2 = 1;
00175     PWMB.period_us(500);
00176     PWMA.period_us(500);
00177     
00178     den = 0;
00179     R_PWM = 0;
00180     L_PWM = 0;
00181     weight= 0;
00182     print_c = 0;
00183 
00184     
00185 }
00186 void Calibration(){
00187     
00188     while(flag_cal_Max == NOK){
00189      
00190     if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
00191         for(int i =0 ; i<4; i ++){
00192             IR_buf_sum |=(IR_buf[i]<<8*(3-i));
00193         }
00194         pc.printf("%X\r\n",IR_buf_sum);
00195             if(IR_buf_sum == 0x00FF0CF3){
00196                  io.write(0x7F);
00197                  TR.calibrate(sensor_val, calMin, calMax);
00198                  wait(0.5);
00199                  TR.calibrate(sensor_val, calMin, calMax);
00200                  io.write(0xFF);
00201     for(int i = 0; i<5; i++){
00202         pc.printf("Min_sensor_value[%d] = %d\r\n",i+1,calMin[i]);
00203         pc.printf("Max_sensor_value[%d] = %d\r\n",i+1,calMax[i]);
00204     }
00205     pc.printf("================================\r\n");
00206 
00207                  flag_cal_Max = YOK;
00208                 }
00209         IR_buf_sum = 0;
00210         }
00211     }
00212     
00213     while(flag_cal_Min == NOK){
00214      
00215     if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
00216         for(int i =0 ; i<4; i ++){
00217             IR_buf_sum |=(IR_buf[i]<<8*(3-i));
00218         } 
00219         pc.printf("%X\r\n",IR_buf_sum);
00220             if(IR_buf_sum == 0x00FF18E7){
00221                  io.write(0xBF);
00222                  TR.calibrate(sensor_val, calMin, calMax);
00223                  wait(0.5);
00224                  TR.calibrate(sensor_val, calMin, calMax);
00225                  io.write(0xFF);
00226                  
00227 for(int i = 0; i<5; i++){
00228 pc.printf("Min_sensor_value[%d] = %d\r\n",i+1,calMin[i]);
00229 pc.printf("Max_sensor_value[%d] = %d\r\n",i+1,calMax[i]);
00230 }
00231 pc.printf("================================\r\n");
00232                  flag_cal_Min = YOK;
00233                 }
00234         IR_buf_sum = 0;
00235         }
00236     }
00237     
00238     
00239     
00240 //=================start===============================
00241     while(flag_start == NOK){
00242      
00243     if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
00244         for(int i =0 ; i<4; i ++){
00245             IR_buf_sum |=(IR_buf[i]<<8*(3-i));
00246         } 
00247         
00248         pc.printf("%X\r\n",IR_buf_sum);
00249             if(IR_buf_sum == 0x00FF5EA1){
00250                 pc.printf("===============start!===============\r\n");
00251                 flag_start = YOK;
00252                 }
00253         IR_buf_sum = 0;
00254         }
00255     } 
00256 //================================================================   
00257 }
00258 
00259 
00260 
00261 
00262 
00263 
00264 
00265 
00266 //Using logic control=================================================================
00267 void Driving(){
00268     
00269     
00270     total.start();
00271     Etime.start();
00272     while(!flag_end){
00273     Etime.reset();
00274 
00275    
00276  Obstacle_check();
00277     
00278     if(flag_distance == NOK){
00279         Actuator();
00280     }
00281     
00282     
00283     Distance_check();
00284  
00285     
00286     do{
00287     }while(Etime.read()<=0.0075);
00288     
00289     }
00290     
00291 }
00292 
00293 
00294 int End_check(int *sensor_values){
00295     int avg = 0;
00296     int sum = 0;
00297     
00298     for(int i = 0; i < NUMSENSORS; i++){
00299         sum += sensor_values[i];
00300     }
00301     avg = sum / NUMSENSORS;
00302     return avg;
00303 }
00304 
00305 
00306 
00307 void Actuator(){
00308     
00309     den = 30;
00310     
00311     
00312     int temp = TR.readLine(sensor_val, calMin, calMax,on_line,1);
00313     
00314     
00315     TR.readCalibrated(sensor_for_end, calMin, calMax);
00316     int avg = End_check(sensor_for_end);
00317     
00318     if(avg <= 100){
00319        pc.printf("avg:%d\r\n",avg);
00320        Motor_Stop(); 
00321        flag_end = YOK;  
00322        total.stop();
00323     }
00324     
00325     temp = temp - MIDDLE;
00326       
00327     if(temp>=0) direction = LEFT;
00328     else        direction = RIGHT;
00329     
00330     
00331     weight = abs(temp)/den;
00332     
00333     if((print_c%500) == 0){
00334         pc.printf("flag_out = %d\r\n", flag_out);
00335         pc.printf("temp = %d\r\n", temp);
00336         pc.printf("online = %d\r\n", on_line[0]);
00337         }
00338     
00339     R_PWM = 135;
00340     L_PWM = 133;
00341     
00342     
00343     if(weight >= (2000/den)*3/4){
00344         if(direction == LEFT){
00345         PWMA.pulsewidth_us(L_PWM+3+0.0*weight);
00346         PWMB.pulsewidth_us(R_PWM-2.1*weight);
00347         }
00348         else{
00349         PWMA.pulsewidth_us(L_PWM+3-2.1*weight);
00350         PWMB.pulsewidth_us(R_PWM+0.0*weight);
00351         }
00352     }else{
00353     
00354         if(direction == LEFT){
00355         PWMA.pulsewidth_us(L_PWM+3+0.0*weight);
00356         PWMB.pulsewidth_us(R_PWM-1.5*weight);
00357         }
00358         else{
00359         PWMA.pulsewidth_us(L_PWM+3-1.5*weight);
00360         PWMB.pulsewidth_us(R_PWM+0.0*weight);
00361         }
00362     }
00363     
00364     
00365     if(weight >= (2000/den)*2/3 && flag_over == NOK)
00366     {
00367         flag_over = YOK;
00368         P_direction = direction;
00369         P_weight = weight;
00370     }
00371     
00372     if((flag_over == YOK) && (abs(weight)<= (2000/den)*1/5)&&on_line[0]==1)
00373     {  
00374         if(P_weight >=(2000/den)*2/3 && P_weight<=(2000/den)*4/5){
00375             if(P_direction == LEFT){
00376             PWMA.pulsewidth_us(L_PWM-P_weight+3);
00377             PWMB.pulsewidth_us(R_PWM); 
00378             }
00379             else{
00380             PWMA.pulsewidth_us(L_PWM+3);
00381             PWMB.pulsewidth_us(R_PWM-P_weight);
00382             }
00383         }
00384     if(P_weight>=(2000/den)*4/5){
00385             if(P_direction == LEFT){
00386             PWMA.pulsewidth_us(L_PWM+3-1.0*P_weight);
00387             PWMB.pulsewidth_us(R_PWM+1.0*P_weight); 
00388             }
00389             else{
00390             PWMA.pulsewidth_us(L_PWM+3+1.0*P_weight);
00391             PWMB.pulsewidth_us(R_PWM-1.0*P_weight);
00392             }
00393         }
00394        // pc.printf("I'm Here\r\n");
00395         
00396         wait(0.12);
00397     flag_over = NOK;
00398     }
00399     
00400     P_temp = temp;
00401     print_c++;
00402 }
00403 
00404 
00405 void Distance_check(){
00406     
00407     if(flag_distance == NOK){
00408     sensor.distance();
00409     Ultra_distance = sensor.returndistance();
00410    //pc.printf("distance = %d\r\n",flag_distance);
00411     if((Ultra_distance >= 16 && Ultra_distance <= 19)) flag_distance = YOK;
00412     }
00413     
00414     if(flag_distance == YOK){
00415      while(1){ 
00416             PWMB.pulsewidth_us(100);
00417             PWMA.pulsewidth_us(100);
00418             AIN1.write(1);
00419             AIN2.write(0);
00420             BIN1.write(0);
00421             BIN2.write(1);
00422             if((abs(TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE)<=400 && on_line[0] == 1)){
00423                 Motor_init();
00424                 flag_distance = NOK;
00425                 PWMB.pulsewidth_us(0);
00426                 PWMA.pulsewidth_us(0);
00427                 break;
00428             }
00429         } 
00430     }
00431 }
00432 
00433 void Obstacle_check(){
00434     
00435     if(io.read() == 0x7F){     //왼쪽읽음 
00436         flag_IR = YOK;
00437         flag_obstacle = YOK;
00438         Obs_direction = LEFT;
00439     }
00440     else if(io.read() == 0xbF)  //오른쪽 읽음
00441     {
00442         flag_IR = YOK;
00443         flag_obstacle = YOK;
00444         Obs_direction = RIGHT;
00445     }
00446     
00447     if(flag_IR){
00448         Otime.start();
00449         
00450         while(flag_obstacle == YOK){
00451              Actuator_Obstacle(Obs_direction); 
00452              pc.printf("obstacle!\r\n");  
00453         }
00454         Otime.stop();
00455         Otime.reset();
00456         flag_IR = NOK;
00457     }
00458 
00459 }
00460 
00461 void Actuator_Obstacle(int dir){
00462  
00463     R_PWM = 100;
00464     L_PWM = 100;
00465  
00466 if(Otime.read() <= 0.5){
00467  
00468          if(dir == 0){
00469              PWMA.pulsewidth_us(L_PWM-20);
00470              PWMB.pulsewidth_us(R_PWM-100); 
00471               }
00472          else if(dir == 1 ){
00473              PWMA.pulsewidth_us(L_PWM-100);
00474              PWMB.pulsewidth_us(R_PWM-30);
00475            }
00476            
00477 }else if(Otime.read() <= 1.0){
00478 
00479          if(dir == 0){
00480              PWMA.pulsewidth_us(L_PWM+5);
00481              PWMB.pulsewidth_us(R_PWM); 
00482               }
00483          else if(dir == 1 ){
00484              PWMA.pulsewidth_us(L_PWM+5);
00485              PWMB.pulsewidth_us(R_PWM);
00486            }
00487              
00488 }else if(Otime.read() <= 2.0){
00489 
00490          if(dir == 0){
00491              PWMA.pulsewidth_us(L_PWM-100);
00492              PWMB.pulsewidth_us(R_PWM-30); 
00493               }
00494          else if(dir == 1 ){
00495              PWMA.pulsewidth_us(L_PWM-10);
00496              PWMB.pulsewidth_us(R_PWM-100);
00497            }
00498            
00499 }else{
00500     
00501         if((abs(TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE)<=400 && on_line[0] == 1)) flag_obstacle = NOK;
00502      
00503         PWMA.pulsewidth_us(L_PWM);
00504         PWMB.pulsewidth_us(R_PWM);
00505 
00506     }
00507 
00508 }
00509 
00510 
00511 int color_set(uint8_t red,uint8_t green, uint8_t blue)
00512 {
00513   return ((red<<16) + (green<<8) + blue);   
00514 }
00515 
00516 // 0 <= stepNumber <= lastStepNumber
00517 int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)
00518 {
00519     return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
00520 }
00521 
00522 void NeopixelOn(){
00523     int colorIdx = 0;
00524     int colorTo = 0;
00525     int colorFrom = 0;
00526     
00527     uint8_t ir = 0;
00528     uint8_t ig = 0;
00529     uint8_t ib = 0;
00530     
00531     ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
00532     
00533     // set up the colours we want to draw with
00534     int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
00535     
00536     // Now the buffer is written, write it to the led array.
00537     while (1) 
00538     {
00539         //pc.printf("thread\r\n");
00540         //get starting RGB components for interpolation
00541         std::size_t c1 = colorbuf[colorFrom];
00542         std::size_t r1 = (c1 & 0xff0000) >> 16;
00543         std::size_t g1 = (c1 & 0x00ff00) >> 8;
00544         std::size_t b1 = (c1 & 0x0000ff);
00545         
00546         //get ending RGB components for interpolation
00547         std::size_t c2 = colorbuf[colorTo];
00548         std::size_t r2 = (c2 & 0xff0000) >> 16;
00549         std::size_t g2 = (c2 & 0x00ff00) >> 8;
00550         std::size_t b2 = (c2 & 0x0000ff);
00551         
00552         for (int i = 0; i <= NUM_STEPS; i++)
00553         {
00554             ir = interpolate(r1, r2, i, NUM_STEPS);
00555             ig = interpolate(g1, g2, i, NUM_STEPS);
00556             ib = interpolate(b1, b2, i, NUM_STEPS);
00557             
00558             //write the color value for each pixel
00559             px.SetAll(color_set(ir,ig,ib));
00560             
00561             //write the II value for each pixel
00562             px.SetAllI(32);
00563             
00564             for (int i = WS2812_BUF; i >= 0; i--) 
00565             {
00566                 ws.write(px.getBuf());
00567             }
00568         }
00569         
00570         colorFrom = colorIdx;
00571         colorIdx++;
00572         
00573         if (colorIdx >= NUM_COLORS)
00574         {
00575             colorIdx = 0;
00576         }
00577         
00578         colorTo = colorIdx;
00579     }    
00580 }