Revision 104:94ed54c74466, committed 2019-06-08

Comitter:

Jeonghoon

Date:

Sat Jun 08 17:25:05 2019 +0000

Parent:

103:b417a6c65a6f

Child:

105:bbded1accbb9

Child:

106:d14f340f1fe0

Commit message:

20190609 update (timer, obstacle)

Changed in this revision

--- a/RemoteIR.lib	Fri Jun 07 17:10:18 2019 +0000
+++ b/RemoteIR.lib	Sat Jun 08 17:25:05 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/embedded1/code/RemoteIR/#6d2532146627
+https://os.mbed.com/teams/embedded1/code/RemoteIR/#e3836c97cf46

--- a/main.cpp	Fri Jun 07 17:10:18 2019 +0000
+++ b/main.cpp	Sat Jun 08 17:25:05 2019 +0000
@@ -2,22 +2,39 @@
 //#include "motor.h"
 #include "ReceiverIR.h"
 #include "Adafruit_SSD1306.h"   // OLED
+#include "WS2812.h"             // botom led
+#include "PixelArray.h"         // bottom led
+
+
 #define NUM_SENSORS 5
+#define PCF8574_ADDR 0x20 //0b0010_0000
+
+#define WS2812_BUF 10
+#define NUM_COLORS 6
+#define NUM_LEDS_PER_COLOR 4
  
 //Motor motor(D6, D5, PA_0, PA_1, PB_0, PA_4, 0.3);
 ReceiverIR motor(D4, 0.2, D6, D5, PA_0, PA_1, PB_0, PA_4);
 SPI spi(D11, D12, D13);
 DigitalOut spi_cs(D10, 1);
 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
-I2C i2c(D14, D15);                                      // D14, D15
+I2C i2c(I2C_SDA, I2C_SCL);                                    // D14, D15
 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);    // D9, (D8: D/C - data/command change)
 DigitalOut DataComm(D8);
  
-Serial blue(PA_11, PA_12, 9600);
+PixelArray px(WS2812_BUF);
+WS2812 ws(D7, WS2812_BUF, 6, 17, 9, 14);
  
 Thread TR_thread;
 Thread remote_thread;
 Thread oled_thread;
+Thread photo_obstacle_thread;
+
+
+// Timer
+Timer timer_ms;
+Timer timer_s; 
+Timer timer_pid;
  
 unsigned long avg = 0;
 unsigned int sum = 0;
@@ -38,15 +55,30 @@
  
 volatile int flag = 0;
 volatile int start = 0;
- 
+volatile int timer_flag = 0;
+volatile int detect = 0;
+
 int ch;
  
- 
+int min_t=0;
+int sec_t=0;
+int msec_t=0;  
+
+int currTime = 0;
+int prevTime = 0;
+int dt ;
+
+char data_write[1] ;
+char data_read[1] ;
+int status;
+
+int count_stop;
  
 int readLine(unsigned int *sensor_values);
 void tr_ready(void);
 void calibration(void);
 void init(void);
+void i2c_init(void);
  
 void tracing_pid(void);
 void set_ir_val(void);
@@ -54,17 +86,24 @@
 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
 void remote_ctrl(void);
 void oled_disp(void);
+void photo_detect(void);
+ 
+void bottom_led(void);
  
 int main()
 {
+    
     pc.printf("start\r\n");
     spi.format(16, 0);
     spi.frequency(2000000);
- 
+    
+    i2c_init();
     init();
  
     oled_thread.start(&oled_disp);
     remote_thread.start(&remote_ctrl);
+    photo_obstacle_thread.start(&photo_detect);
+    
     while(!motor.cal_start);
  
     calibration();
@@ -75,13 +114,40 @@
  
  
     while(!start);
- 
+    
     pc.printf("motor forward\r\n");
+    
     motor.forward();
- 
+    timer_ms.start();
+    timer_s.start(); 
+
+    while(!timer_flag);
+    msec_t = timer_ms.read_ms();
+    msec_t %= 1000;
+    sec_t = timer_s.read();
+    
     while (1);
 }
- 
+
+
+void bottom_led(void){
+    
+    int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
+
+    for (int i = 0; i < WS2812_BUF; i++) {
+        px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
+    }
+        
+    for (int j=0; j<WS2812_BUF; j++) {
+        px.SetI(j%WS2812_BUF, 0x4B);
+    }
+    
+    for (int z=WS2812_BUF; z >= 0 ; z--) {
+        ws.write_offsets(px.getBuf(),z,z,z);
+        wait(1);
+    }    
+}
+
 void remote_ctrl(void){
  
     while(1){
@@ -105,10 +171,11 @@
     motor.cal_start = 0;
     motor.cal_stop = 0; 
     
-    motor.kp = 10;
-    motor.kd = 1;
+    motor.kp = 10.0;
+    motor.kd = 1.0;
     motor.ki = 5000;
-    motor.max = 150;
+    motor.max = 250;
+    
  
     DataComm = 0;
  
@@ -121,13 +188,70 @@
       calibratedMax[i] = 0;
     }
 }
+
+void i2c_init(void){
+    
+    i2c.frequency(100000);
+    data_write[0] = 0xE0; //1110_0000
+    
+    status = i2c.write(PCF8574_ADDR << 1, data_write, 1, 0);
+   
+    if(status != 0){
+        pc.printf("configuration err\r\n");
+        while(1){}    
+    }
+
+}
+
+void photo_detect(void){
+        
+        while(1){
+            pc.printf("detect: %d\r\n", detect);
+            
+            i2c.read(((PCF8574_ADDR << 1) | 0x01), data_read, 1, 0);
+        
+            int dsl = (int)data_read[0] & 0x80; //1000_0000
+            int dsr = (int)data_read[0] & 0x40; //0100_0000
+        
+            if(dsl == 0){  
+                detect = 1;
+                motor.stop();
+                wait_ms(30);
+                motor.speed_l(0.6);
+                motor.speed_r(0);
+                wait_ms(300);
+                motor.speed_l(0.2);
+                motor.speed_r(0.2);
+                wait_ms(50);                
+                motor.speed_l(0);
+                motor.speed_r(0.7); 
+                wait_ms(200);              
+                detect = 0;
+            }
+            else if(dsr == 0){
+                detect = 1;
+                motor.speed_l(0);
+                motor.speed_r(0.4);       
+                wait_ms(300);
+                motor.speed_l(0.2);
+                motor.speed_r(0.2);
+                wait_ms(50);                
+                motor.speed_l(0.6);
+                motor.speed_r(0);                 
+                wait_ms(200);              
+                detect = 0;
+            }
+
+        }
+}
  
 void oled_disp(void){
     myOled.begin();
     while(1){
         myOled.clearDisplay();
-        //myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
-        myOled.printf("%.0f %.1f %d %d\r", motor.kp, motor.kd, motor.ki, motor.max);
+        //myOled.printf("%.2f\r", motor.kd);
+        myOled.printf("time: %u:%u.%u\r", min_t, sec_t, msec_t);
+//        myOled.printf("%.2f %.2f %d %d\r", motor.kp, motor.kd, motor.ki, motor.max);
         myOled.display();
     }
 }
@@ -213,10 +337,9 @@
  
         // get current position
         get_pos();
- 
+        
+        while(detect);
         tracing_pid();
-        
-        
     }
 }
  
@@ -239,16 +362,18 @@
    
    // finish line
    if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
-        motor.stop();
-        wait(5);        
-    }
+        count_stop ++;
+        if(count_stop > 7){
+            timer_flag = 1;
+            motor.stop();
         
-   for(int i = 0; i < NUM_SENSORS; i++){
- 
-        pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
+            bottom_led();     
+        }  
     }
-    
-    pc.printf("\r\n");
+    else{
+        count_stop = 0;
+    }
+
 }
  
 void get_pos(){
@@ -289,40 +414,28 @@
 }
  
 void tracing_pid(void){
- 
     
     int proportional = pos - 2000;
     
     int derivative = proportional - last_proportional;
+
     integral += proportional;
- 
+    
     last_proportional  = proportional;
  
-    int power_difference = proportional / motor.kp + integral / motor.ki + derivative * motor.kd;
-    //int power_difference = proportional / motor.kp;
-    //int power_difference = proportional/motor.kp + derivative * motor.kd; 
-    const float std = 600.0;
- 
+    int power_difference = proportional / motor.kp + derivative * motor.kd;
+                                                                                                                                                                                                                                                                                                                                                                                                                         
+    const float std = 1000.0;
+    
+    
     if(power_difference > motor.max)
         power_difference = motor.max;
  
     if(power_difference < -motor.max)
         power_difference = -motor.max;
     
-    // bot position: right
-    if(power_difference > 15){ //0.1 + p_d/std
-        motor.speed_r((motor.max + power_difference)/std);
-        motor.speed_l((motor.max - power_difference)/std);
-    }
-    // bot position: left
-    else if(power_difference < -15){
-        motor.speed_r((motor.max + power_difference)/std);
-        motor.speed_l((motor.max - power_difference)/std);
-    }
-    else{ //online
-        motor.speed_l(motor.init_sp_l);
-        motor.speed_r(motor.init_sp_r);
-    }
- 
+        
+    motor.speed_r((motor.max + power_difference)/std);
+    motor.speed_l((motor.max - power_difference)/std); 
 
 }
\ No newline at end of file