main.cpp

00001 #include "mbed.h"
00002 #include "RemoteIR.h"
00003 #include "ReceiverIR.h"
00004 #include "TB6612FNG.h"
00005 #include "TRSensors.h"
00006 #include "ultrasonic.h"
00007 #include "Adafruit_SSD1306.h"
00008 #include "WS2812.h"
00009 #include "PixelArray.h"
00010 
00011 #define button_SENSORS 5
00012 #define ADT7420_TEMP_REG (0x00)
00013 #define ADT7420_CONF_REG (0x03)
00014 #define EVAL_ADT7420_ADDR (0x48)
00015 #define PCF8574_ADDR (0x20)
00016 #define WS2812_BUF 100
00017 #define WS2812_BUF2 4
00018 #define NUM_COLORS 3
00019 #define NUM_LEDS_PER_COLOR 4
00020 
00021 // create object
00022 DigitalOut dc(D8,1);
00023 DigitalOut rst(D9,1);
00024 I2C i2c(I2C_SDA, I2C_SCL);
00025 TB6612FNG motorDriver(D6, A1, A0, D5, A2, A3);
00026 Ultrasonic ultra(D3, D2, .1, false);
00027 ReceiverIR IR(D4);
00028 TRSensors trs;
00029 RawSerial pc(USBTX, USBRX, 115200);
00030 Adafruit_SSD1306_I2c gOled2(i2c,D9,0x7A,64,128);
00031 Timer timer;
00032 PixelArray px(WS2812_BUF);
00033 WS2812 ws(D7, WS2812_BUF, 7, 15, 10, 15);
00034 // Thread thread1;
00035 // Thread thread2;
00036 
00037 
00038 unsigned int sensorValues[button_SENSORS]; 
00039 
00040 // Error variables
00041 static int prev_err = 0;
00042 static int int_err = 0;
00043 int err = 0;
00044 
00045 // PID & velocity value
00046 static float pval = 0.15;
00047 static float ival = 0.00003;
00048 static float dval = 2.3;
00049 static float vel = 120.0;
00050 
00051 volatile int button = 0;
00052 int position = 0;
00053 int dist = 0;
00054 int cnt = 0;
00055 float t = 0;
00056 
00057 // Reflective sensor
00058 bool rightObs= false;
00059 bool leftObs = false;
00060 
00061 // motor Driver
00062 float fPwmAPeriod;
00063 float fPwmAPulsewidth;
00064 float fPwmBPeriod;
00065 float fPwmBPulsewidth;
00066 
00067 // opearte ultrasonic
00068 //void ultrasonic_thread()
00069 //{
00070 //    while (1)
00071 //    {
00072 //        ultra.setMode(false);           // have updates every .1 seconds and try only once.
00073 //        ultra.trig();
00074 //        dist = ultra.getDistance();
00075 //        pc.printf("Detected value: %d\r\n", dist);
00076 //    }
00077 //}
00078 
00079 
00080 // opearte reflective sensors
00081 /*
00082 void reflective_thread()
00083 {
00084     while(1){
00085         char data_write[2];
00086         char data_read[2];
00087         i2c.frequency(100000);
00088         i2c.read((PCF8574_ADDR<<1|0x01), data_read, 2, 0);
00089         pc.printf("hexa: %x, %x\r\n", data_read[0], data_read[1]);
00090         if(data_read[0]>>6 == 0b10)
00091         {
00092             pc.printf("Obstacle on left!\r\n");
00093             data_write[0] = 0xDF;
00094             leftObs = true;
00095         }
00096         else if(data_read[0]>>6 == 0b01)
00097         {
00098             pc.printf("Obstacle on right!\r\n");
00099             data_write[0] = 0xDF;
00100             rightObs = true;
00101         }
00102         else if(data_read[0]>>6 == 0b00)
00103         {
00104             pc.printf("Obstacle on front!\r\n");
00105             data_write[0] = 0xDF;
00106             leftObs = true;
00107             rightObs = true;
00108             wait(3);
00109         }
00110         
00111         else
00112         {
00113             data_write[0]=0xFF;
00114         }
00115         i2c.write((PCF8574_ADDR<<1),data_write,1,0);      
00116     }
00117 }*/
00118 
00119 inline void update_display(){
00120                 gOled2.clearDisplay();
00121                 gOled2.setTextCursor(0,0);
00122                 gOled2.printf("E-RON alphabot\r\n");
00123                 gOled2.printf("Timer: %.2f\r\n",t);
00124                 gOled2.printf("V: %.0f\r\n",vel);
00125                 gOled2.printf("P: %.2f\r\n",pval);
00126                 gOled2.printf("I: %.5f\r\n",ival);
00127                 gOled2.printf("D: %.2f\r\n",dval);
00128                 gOled2.display();
00129 }
00130     int colorbuf[NUM_COLORS] = {0xff0000,0x00ff00,0x0000ff};
00131     int colorbuf3 =0x000000;
00132     
00133 int main()
00134 {      
00135     for (int i = 0; i < WS2812_BUF; i++) {
00136         px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
00137     }
00138     for (int j=0; j<WS2812_BUF; j++) {
00139         // px.SetI(pixel position, II value)
00140         px.SetI(j%WS2812_BUF, 0xf+(0xf*4));
00141     }
00142     
00143  //   thread1.start(ultrasonic_thread);
00144  //   thread2.start(reflective_thread);
00145     RemoteIR::Format format;
00146     uint8_t buf[32];
00147     int bitcount;
00148     update_display();
00149     while(1)
00150     { // read the value of the code
00151         if (IR.getState() == ReceiverIR::Received)
00152         {
00153             bitcount = IR.getData(&format, buf, sizeof(buf) * 8);
00154             pc.printf("%d\r\n", bitcount);
00155             pc.printf("buf[2]: %d\r\n", buf[2]);
00156             button = buf[2];
00157         }
00158         
00159         switch(button)
00160         {
00161             case 0x45:
00162             // ch- button (P value up)
00163                 button = 0x1C;
00164                 pval += 0.01;
00165                 pval = fabs(pval);
00166                 update_display();
00167 //                pc.printf("pval:%f\r\n", pval);
00168                 wait(0.3);
00169                 break;
00170             
00171             case 0x46:
00172             // ch common button (I value up)
00173                 button = 0x1C;
00174                 ival += 0.00001;
00175                 ival = fabs(ival);
00176                 update_display() ;        
00177 //                pc.printf("ival:%f\r\n", ival);
00178                 wait(0.3);
00179                 break;
00180             
00181             case 0x47: 
00182             // ch+ button (D value up)
00183                 button = 0x1C;
00184                 dval += 0.05;
00185                 dval = fabs(dval);
00186                 update_display();
00187                 wait(0.3);
00188                 break;
00189                 
00190             case 0x44:
00191             // prev button (P value down)
00192                 button = 0x1C;
00193                 pval -= 0.01;
00194                 pval = fabs(pval);
00195                 update_display();
00196                 wait(0.3);
00197                 break;
00198             
00199             case 0x40:
00200             // next button (I value down)
00201                 button = 0x1C;
00202                 ival -= 0.00001;
00203                 ival = fabs(ival);
00204                 update_display();
00205                 wait(0.3);
00206                 button = 0x1C;
00207                 break;
00208                 
00209             case 0x43:
00210             // play/pause button (D value down)
00211                 button = 0x1C;
00212                 dval -= 0.05;
00213                 dval = fabs(dval);
00214                 update_display();
00215                 wait(0.3);
00216                 button = 0x1C;
00217                 break;
00218 
00219             case 0x07:
00220             // vol- button (velocity down)
00221                 button = 0x1C;
00222                 vel -= 5;
00223                 update_display();
00224                 wait(0.3);
00225                 button = 0x1C;
00226                 break;
00227                 
00228             case 0x15:
00229             // vol+ button (velocity up)
00230                 button = 0x1C;
00231                 vel += 5;
00232                 update_display();
00233                 wait(0.3);
00234                 button = 0x1C;
00235                 break;
00236                 
00237             case 0x16:
00238             // 0 button (Reset the error variables)
00239                 int_err = 0;
00240                 err = 0;
00241                 prev_err = 0;
00242                 wait(1);
00243                 button = 0x1C;
00244                 break;
00245             
00246             case 0x19:
00247             // 100+ button (Calibrate)
00248                 pc.printf("calibrate!\r\n");
00249                 trs.calibrate();
00250                 pc.printf("calibrate done\r\n");
00251                 wait(0.2);
00252                 button = 0x1C;
00253                 break;
00254                 
00255             case 0x18:
00256             // 2 button (move forward)
00257                 motorDriver.user_forward(0.3,0.3);
00258                 trs.calibrate();
00259                 wait(0.1);
00260                 button = 0x1C;
00261                 break;
00262             
00263             case 0x52:
00264             // 8 button (move backward)
00265                 motorDriver.user_backward(0.3,0.3);
00266                 trs.calibrate();
00267                 wait(0.1);
00268                 button = 0x1C;
00269                 break;
00270             
00271             case 0x5A:
00272             // 6 button (clockwise turn)
00273                 motorDriver.user_right(0.3,0.1);
00274                 wait(0.1);
00275                 button = 0x1C;
00276                 break;
00277             
00278             case 0x08:
00279             // 4 button (counter clockwise turn)
00280                 motorDriver.user_left(0.1,0.3);
00281                 wait(0.1);
00282                 button = 0x1C;
00283                 break;
00284             
00285             case 0x1C:
00286             // 5 button (motor stop)
00287                 motorDriver.stop();
00288                 break;
00289             
00290             case 0x0D:
00291             // 200+ button (line tracer)
00292                 t=0;
00293                 timer.reset();
00294                 timer.start();
00295                 ultra.setMode(false);           // have updates every .1 seconds and try only once.
00296 
00297                 while(1)
00298                 {  
00299                     t=timer.read();
00300                     position=trs.readLine(sensorValues,0);
00301                     ultra.trig();
00302                     dist = ultra.getDistance();
00303                     pc.printf("dist:%d\r\n", dist);
00304                     if(dist<=21){
00305                           motorDriver.user_left(0.2,0.2);
00306                           wait(0.1);
00307                           while(1){
00308                            
00309                            // pc.printf("distance: %d\r\n", dist);
00310                            // pc.printf("position: %d\r\n", position);
00311                             position=trs.readLine(sensorValues,0);
00312                             if(position > 2000)
00313                             {
00314                                 int_err = 0;
00315                                 err = 0;
00316                                 prev_err = 0;
00317                                 break;
00318                             }
00319                         }
00320                     }
00321                     
00322                     err=(int)position-2000; // error>0 --> right, error<0 --> left
00323                     int_err+=err;
00324 
00325                     const float kp = pval;
00326                     const float kd = dval;
00327                     const float ki = ival;
00328             
00329                     float power_difference = kp*err + kd*(err-prev_err) + ki*int_err; //+derivative; //error를 누적하는 것
00330                     prev_err = err;
00331             
00332                     const float maximum=vel;
00333                     if(power_difference > maximum)
00334                         power_difference = maximum;
00335                     if(power_difference < -maximum)
00336                         power_difference = -maximum;      
00337                         
00338                     if(power_difference<0)
00339                         motorDriver.user_forward((maximum)/255,(maximum+power_difference)/255);               
00340                     else 
00341                         motorDriver.user_forward((maximum-power_difference)/255,(maximum)/255); 
00342                     
00343                     pc.printf("position value: %d\r\n", position);
00344                     
00345                     //pc.printf("cnt: %d\r\n", cnt);
00346                     for(int i=0;i<5;i++){
00347                         pc.printf("%d\r\n",sensorValues[i]);
00348                     }
00349                     if((sensorValues[0] > 650) && (sensorValues[1]>650) && (sensorValues[2]>650) && (sensorValues[3]>650) && (sensorValues[4]>650))
00350                     {   // 5 IR sensor are on black
00351                         timer.stop();
00352                         t=timer.read();
00353                         update_display();
00354                         motorDriver.stop();
00355                         for(int z=48;z>=0;z=z-4){
00356                         ws.write_offsets(px.getBuf(),z,z,z);
00357                         wait(0.1);
00358                         }
00359                         
00360                         button = 0x1C;
00361                         break;
00362                     }
00363                 }
00364                 break;
00365                 
00366             case 0x42:
00367             // 7 button (read sensor values)
00368                 position=trs.readLine(sensorValues,0);
00369                 for(int i=0; i<5; i++) {
00370                     pc.printf("%d\r\n",sensorValues[i]);
00371                 }
00372                 pc.printf("done!\r\n");
00373                 button = 0x1C;
00374                 break;
00375 
00376                 
00377             case 0x4A:
00378             // 9 button (read position)
00379                 int j = 0;
00380                 while(j!=100)
00381                 {
00382                     j++;
00383                     position=trs.readLine(sensorValues,0);
00384                     pc.printf("%d\r\n",position);
00385                 } 
00386                 button = 0x1C;
00387                 break;                
00388             default:
00389                 // wrong button
00390                 pc.printf("wrong button!\r\n");
00391                 break;
00392         }
00393     }
00394 }