main.cpp

00001 #include "mbed.h"
00002 //#include "motor.h"
00003 #include "ReceiverIR.h"
00004 #include "Adafruit_SSD1306.h"   // OLED
00005 #include "WS2812.h"             // botom led
00006 #include "PixelArray.h"         // bottom led
00007 
00008 
00009 #define NUM_SENSORS 5
00010 #define PCF8574_ADDR 0x20 //0b0010_0000
00011 
00012 #define WS2812_BUF 10
00013 #define NUM_COLORS 6
00014 #define NUM_LEDS_PER_COLOR 4
00015 
00016 ReceiverIR motor(D4, 0.2, D6, D5, PA_0, PA_1, PB_0, PA_4);
00017 SPI spi(D11, D12, D13);
00018 DigitalOut spi_cs(D10, 1);
00019 RawSerial pc(SERIAL_TX, SERIAL_RX, 115200);
00020 I2C i2c(I2C_SDA, I2C_SCL);                                    // D14, D15
00021 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);    // D9, (D8: D/C - data/command change)
00022 DigitalOut DataComm(D8);
00023  
00024 PixelArray px(WS2812_BUF);
00025 WS2812 ws(D7, WS2812_BUF, 6, 17, 9, 14);
00026  
00027 Thread TR_thread;
00028 Thread remote_thread;
00029 Thread oled_thread;
00030 
00031 
00032 // Timer
00033 Timer timer_ms;
00034 Timer timer_s; 
00035 Timer timer_pid;
00036  
00037 unsigned long avg = 0;
00038 unsigned int sum = 0;
00039  
00040 unsigned int sensor_values[NUM_SENSORS];
00041 unsigned int max_sensor_values[NUM_SENSORS];
00042 unsigned int min_sensor_values[NUM_SENSORS];
00043  
00044 unsigned int *calibratedMin;
00045 unsigned int *calibratedMax;
00046  
00047 int value;
00048 int on_line = 0;
00049 static int pos = 0;
00050 unsigned int last_proportional = 0;
00051 long integral = 0;
00052  
00053 volatile int flag = 0;
00054 volatile int start = 0;
00055 volatile int timer_flag = 0;
00056 
00057 int ch;
00058  
00059 int min_t=0;
00060 int sec_t=0;
00061 int msec_t=0;  
00062 
00063 int currTime = 0;
00064 int prevTime = 0;
00065 int dt ;
00066 
00067 char data_write[1] ;
00068 char data_read[1] ;
00069 int status;
00070 
00071 int count_stop;
00072 int err;
00073 int pid;
00074  
00075 int readLine(unsigned int *sensor_values);
00076 void tr_ready(void);
00077 void calibration(void);
00078 void init(void);
00079 void i2c_init(void);
00080  
00081 void tracing_pid(void);
00082 void set_ir_val(void);
00083 void get_pos(void);
00084 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
00085 void remote_ctrl(void);
00086 void oled_disp(void);
00087  
00088 void bottom_led(void);
00089 void control(void);
00090  
00091 int main()
00092 {
00093     
00094     pc.printf("start\r\n");
00095     spi.format(16, 0);
00096     spi.frequency(2000000);
00097     
00098     i2c_init();
00099     init();
00100  
00101     
00102     remote_thread.start(&remote_ctrl);
00103     
00104     while(!motor.cal_start);
00105  
00106     calibration();
00107  
00108     while(!flag);
00109  
00110     TR_thread.start(&tr_ready);
00111  
00112  
00113     while(!start);
00114     
00115     pc.printf("motor forward\r\n");
00116     
00117     motor.forward();
00118     timer_ms.start();
00119     timer_s.start(); 
00120 
00121     while(!timer_flag);
00122     oled_thread.start(&oled_disp);
00123     msec_t = timer_ms.read_ms();
00124     msec_t %= 1000;
00125     sec_t = timer_s.read();
00126     
00127     if(sec_t > 60){ 
00128         min_t = 1;
00129         sec_t %= 60;
00130     }
00131     
00132     while (1);
00133 }
00134 
00135 
00136 void bottom_led(void){
00137     
00138     int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
00139 
00140     for (int i = 0; i < WS2812_BUF; i++) {
00141         px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
00142     }
00143         
00144     for (int j=0; j<WS2812_BUF; j++) {
00145         px.SetI(j%WS2812_BUF, 0x4B);
00146     }
00147     
00148     for (int z=WS2812_BUF; z >= 0 ; z--) {
00149         ws.write_offsets(px.getBuf(),z,z,z);
00150         wait(1);
00151     }    
00152 }
00153 
00154 void remote_ctrl(void){
00155  
00156     while(1){
00157     uint8_t buf1[32];
00158     uint8_t buf2[32];
00159     int bitlength1;
00160     RemoteIR::Format format;
00161  
00162     memset(buf1, 0x00, sizeof(buf1));
00163     memset(buf2, 0x00, sizeof(buf2));
00164  
00165         bitlength1 = receive(&format, buf1, sizeof(buf1));
00166         if (bitlength1 < 0) {
00167             continue;
00168         }
00169  
00170     }
00171 }
00172  
00173 void init(void){
00174     motor.cal_start = 0;
00175     motor.cal_stop = 0; 
00176     
00177     motor.kp = 12.9;
00178     motor.kd = 2.0;
00179     motor.ki = 6000;
00180     motor.max = 180;
00181  
00182     DataComm = 0;
00183  
00184     calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
00185     calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
00186  
00187     for(int i=0; i < NUM_SENSORS; i++)
00188     {
00189       calibratedMin[i] = 1023;
00190       calibratedMax[i] = 0;
00191     }
00192 }
00193 
00194 void i2c_init(void){
00195     
00196     i2c.frequency(100000);
00197     data_write[0] = 0xE0; //1110_0000
00198     
00199     status = i2c.write(PCF8574_ADDR << 1, data_write, 1, 0);
00200    
00201     if(status != 0){
00202         pc.printf("configuration err\r\n");
00203         while(1){}    
00204     }
00205 
00206 }
00207  
00208 void oled_disp(void){
00209     myOled.begin();
00210     while(1){
00211         myOled.printf("timer: %d:%d:%d sec\r",min_t, sec_t, msec_t);
00212 //        myOled.printf("%spl: %.3f spr:%.3f\r", motor.base_spl, motor.base_spr); 
00213         myOled.display();
00214 
00215     }
00216 }
00217  
00218  
00219 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) {
00220     int cnt = 0;
00221     while (motor.getState() != ReceiverIR::Received) {
00222         cnt++;
00223         if (timeout < cnt) {
00224             return -1;
00225         }
00226     }
00227     return motor.getData(format, buf, bufsiz * 8);
00228 }
00229  
00230  
00231 void read_ir(void){
00232     ch = 0;
00233  
00234     spi_cs = 0;
00235     wait_us(2);
00236     value = spi.write(ch << 12);
00237     spi_cs = 1;
00238  
00239     wait_us(21);
00240  
00241     for(int i = 0; i < 5; i++){
00242  
00243         ch += 1;
00244  
00245         spi_cs = 0;
00246         wait_us(2);
00247         value = spi.write(ch << 12);
00248         spi_cs = 1;
00249  
00250         wait_us(21);
00251  
00252         value = value >> 6;
00253         value = value * 3300 / 0x3FF;
00254  
00255         sensor_values[i] = value;
00256     }
00257 
00258     
00259 }
00260  
00261 void calibration(void){
00262     pc.printf("calibration start \r\n");
00263     
00264     int i = 0;
00265     while(!motor.cal_stop){    
00266         read_ir();
00267         for(int j = 0; j < NUM_SENSORS; j++){
00268             if(i == 0 || max_sensor_values[j] < sensor_values[j]){
00269                 max_sensor_values[j] = sensor_values[j];
00270             }
00271  
00272             if(i == 0 || min_sensor_values[j] > sensor_values[j]){
00273                 min_sensor_values[j] = sensor_values[j];
00274             }
00275         }
00276         i = 1;
00277     }
00278  
00279     for(int i = 0; i < NUM_SENSORS; i++){
00280         if(min_sensor_values[i] < calibratedMin[i])
00281             calibratedMin[i] = min_sensor_values[i];
00282         if(max_sensor_values[i] > calibratedMax[i])
00283             calibratedMax[i] = max_sensor_values[i];
00284     }
00285     
00286     pc.printf("calibration complete\r\n");
00287     flag = 1;
00288 }
00289  
00290 void tr_ready(void){
00291     unsigned int right_max = 0.8;
00292     unsigned int left_max = 0.805;
00293 //    unsigned int sub = 0.5;
00294     while(1){
00295         // read current IR 1 ~ IR 5
00296         read_ir();
00297         
00298         // set range under 1000
00299         set_ir_val();
00300     
00301         // get current position
00302         get_pos();
00303         if(pos<1800){ //turn r
00304             motor.speed_r(right_max);
00305             motor.speed_l(left_max + 0.13);
00306         }
00307         else if(pos>2250){ //turn l
00308             motor.speed_r(right_max + 0.18);
00309             motor.speed_l(left_max - 0.1);
00310         }
00311         else{
00312             tracing_pid();
00313         }
00314     }
00315 }
00316  
00317 void set_ir_val(){
00318     for(int i = 0; i < NUM_SENSORS; i++)
00319     {
00320         unsigned int denominator;
00321  
00322         denominator = calibratedMax[i] - calibratedMin[i];
00323  
00324         signed int x = 0;
00325         if(denominator != 0)
00326             x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
00327         if(x < 0)   x = 0;
00328         else if(x > 1000)   x = 1000;
00329         
00330         sensor_values[i] = x;
00331              
00332    }
00333    
00334    for(int i = 0; i < NUM_SENSORS; i++){
00335         pc.printf("sensor_values[%d]: %d\r\n",i, sensor_values[i]);   
00336     }
00337     pc.printf("\r\n");
00338 
00339    // finish line
00340    if(sensor_values[0] < 300 && sensor_values[1] < 300 && sensor_values[2] < 300 && sensor_values[3] < 300 && sensor_values[4] < 300){
00341 //        count_stop ++;
00342 //        if(count_stop > 7){
00343             timer_flag = 1;
00344             motor.stop();
00345         
00346             bottom_led();     
00347 //        } 
00348     }
00349 //    else{
00350 //        count_stop = 0;
00351 //    }
00352 
00353 }
00354  
00355 void get_pos(){
00356     on_line = 0;
00357     avg = 0;
00358     sum = 0;
00359  
00360     for(int i = 0; i < NUM_SENSORS; i++){
00361         int val = sensor_values[i];
00362  
00363         // determine "on_line" or "out_line"
00364         if(val < 450){
00365             on_line = 1;
00366         }
00367  
00368         // under
00369         if(val > 5){
00370             avg += (long)(val) * (i * 1000);
00371             sum += val;
00372         }
00373     }
00374  
00375     // out_line
00376     if(!on_line){
00377         if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
00378             pos = 0;                             // last_vlaue = 0
00379 //            pos = 200;
00380         }
00381         else{                                    // right -> out-line (over 2000)
00382              pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
00383 //            pos = 3800;
00384         }
00385     }
00386     // on_line
00387     else{
00388         pos = avg / sum;
00389     }
00390  
00391     start = 1;
00392 }
00393  
00394 void tracing_pid(void){
00395     
00396     int proportional = pos - 2000;
00397     
00398     int derivative = proportional - last_proportional;
00399     integral += proportional;
00400  
00401     last_proportional  = proportional;
00402  
00403     int power_difference = proportional / motor.kp+ integral/motor.ki + derivative * motor.kd;
00404 
00405     const float std = 500.0;
00406  
00407     if(power_difference > motor.max)
00408         power_difference = motor.max;
00409  
00410     if(power_difference < -motor.max)
00411         power_difference = -motor.max;
00412     
00413     // bot position: right
00414     if(power_difference > 10){ //0.1 + p_d/std
00415         motor.speed_r((motor.max + power_difference)/std);
00416         motor.speed_l((motor.max - power_difference)/std + 0.005);
00417     }
00418     // bot position: left
00419     else if(power_difference < -10){
00420         motor.speed_r((motor.max + power_difference)/std);
00421         motor.speed_l((motor.max - power_difference)/std + 0.005);
00422     }
00423     else{ //online
00424         motor.speed_l(motor.init_sp_l);
00425         motor.speed_r(motor.init_sp_r);
00426     }    
00427 
00428 }
00429