Revision 106:d14f340f1fe0, committed 2019-06-10

Comitter:

Jeonghoon

Date:

Mon Jun 10 11:10:06 2019 +0000

Parent:

104:94ed54c74466

Child:

107:94134c6f90e8

Commit message:

kp 0.1 kd 0 max 200 std 950 (1:15)

Changed in this revision

--- a/RemoteIR.lib	Sat Jun 08 17:25:05 2019 +0000
+++ b/RemoteIR.lib	Mon Jun 10 11:10:06 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/embedded1/code/RemoteIR/#e3836c97cf46
+https://os.mbed.com/teams/embedded1/code/RemoteIR/#bbedc231c6a9

--- a/main.cpp	Sat Jun 08 17:25:05 2019 +0000
+++ b/main.cpp	Mon Jun 10 11:10:06 2019 +0000
@@ -12,12 +12,12 @@
 #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);
+RawSerial pc(SERIAL_TX, SERIAL_RX, 115200);
 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);
@@ -28,7 +28,6 @@
 Thread TR_thread;
 Thread remote_thread;
 Thread oled_thread;
-Thread photo_obstacle_thread;
 
 
 // Timer
@@ -47,7 +46,6 @@
 unsigned int *calibratedMax;
  
 int value;
-float bat;
 int on_line = 0;
 static int pos = 0;
 unsigned int last_proportional = 0;
@@ -56,7 +54,6 @@
 volatile int flag = 0;
 volatile int start = 0;
 volatile int timer_flag = 0;
-volatile int detect = 0;
 
 int ch;
  
@@ -73,6 +70,8 @@
 int status;
 
 int count_stop;
+int err;
+int pid;
  
 int readLine(unsigned int *sensor_values);
 void tr_ready(void);
@@ -86,9 +85,9 @@
 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);
+void control(void);
  
 int main()
 {
@@ -102,7 +101,6 @@
  
     oled_thread.start(&oled_disp);
     remote_thread.start(&remote_ctrl);
-    photo_obstacle_thread.start(&photo_detect);
     
     while(!motor.cal_start);
  
@@ -171,11 +169,11 @@
     motor.cal_start = 0;
     motor.cal_stop = 0; 
     
-    motor.kp = 10.0;
-    motor.kd = 1.0;
+    motor.kp = 0.1;
+    motor.kd = 0.0;
     motor.ki = 5000;
-    motor.max = 250;
-    
+    motor.max = 200;
+    motor.standard = 500.0;
  
     DataComm = 0;
  
@@ -202,57 +200,19 @@
     }
 
 }
-
-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("%.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.printf("%.2f %.2f %.f %.f\r", motor.kp, motor.kd, motor.max, motor.standard);
+//        myOled.printf("%spl: %.3f spr:%.3f\r", motor.base_spl, motor.base_spr); 
         myOled.display();
+       // myOled.clearDisplay();
+//        wait(0.3);
+//        myOled.begin();
+//        myOled.printf("%d %.0f\r", motor.max, motor.standard);
+//        myOled.display();
+//        myOled.clearDisplay();
     }
 }
  
@@ -295,6 +255,12 @@
  
         sensor_values[i] = value;
     }
+    
+//    for(int i = 0; i < 5; i++){
+//        pc.printf("raw sensor values[%d]: %d\r\n",i, sensor_values[i]);
+//    }
+//    pc.printf("\r\n");
+    
 }
  
 void calibration(void){
@@ -331,15 +297,17 @@
     while(1){
         // read current IR 1 ~ IR 5
         read_ir();
- 
+        
         // set range under 1000
         set_ir_val();
  
         // get current position
         get_pos();
+//        pc.printf("pos: %d\r\n", pos);
+        tracing_pid();
         
-        while(detect);
-        tracing_pid();
+        //control
+//        control();
     }
 }
  
@@ -355,24 +323,35 @@
             x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
         if(x < 0)   x = 0;
         else if(x > 1000)   x = 1000;
- 
+//method2
+//        if(x < 250) x = 1; //black
+//        else x = 0;
+        
         sensor_values[i] = x;
-        
+             
    }
    
+//   for(int i = 0; i < NUM_SENSORS; i++){
+//        pc.printf("(after)sensor_values[%d] = %d\r\n", i, sensor_values[i]);
+//    }
+//   
+//    pc.printf("\r\n");
+//    wait(0.5);
+
+
    // finish line
-   if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
-        count_stop ++;
-        if(count_stop > 7){
-            timer_flag = 1;
-            motor.stop();
-        
-            bottom_led();     
-        }  
-    }
-    else{
-        count_stop = 0;
-    }
+   //if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
+//        count_stop ++;
+//        if(count_stop > 7){
+//            timer_flag = 1;
+//            motor.stop();
+//        
+//            bottom_led();     
+//        }  
+//    }
+//    else{
+//        count_stop = 0;
+//    }
 
 }
  
@@ -398,11 +377,13 @@
  
     // out_line
     if(!on_line){
-        if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
-            pos = 0;                             // last_vlaue = 0
+        //if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
+        if(pos <= 1800){       // left -> out-line (under 2000)
+            pos = 300;                             // last_vlaue = 0
         }
         else{                                    // right -> out-line (over 2000)
-             pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
+//             pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
+            pos = 3800;
         }
     }
     // on_line
@@ -415,18 +396,16 @@
  
 void tracing_pid(void){
     
-    int proportional = pos - 2000;
+    int proportional = pos - 2050;
     
-    int derivative = proportional - last_proportional;
+    int derivative = proportional - last_proportional; 
 
     integral += proportional;
     
     last_proportional  = proportional;
  
-    int power_difference = proportional / motor.kp + derivative * motor.kd;
-                                                                                                                                                                                                                                                                                                                                                                                                                         
-    const float std = 1000.0;
-    
+    int power_difference = proportional*motor.kp + derivative * motor.kd;
+                                                                                                                                                                                                                                                                                                                                                                                                                             
     
     if(power_difference > motor.max)
         power_difference = motor.max;
@@ -434,8 +413,49 @@
     if(power_difference < -motor.max)
         power_difference = -motor.max;
     
-        
-    motor.speed_r((motor.max + power_difference)/std);
-    motor.speed_l((motor.max - power_difference)/std); 
+    //완전 online일 때 init_spl, init_spr  
+    motor.speed_r((motor.max + power_difference)/motor.standard);
+    motor.speed_l((motor.max - power_difference)/motor.standard); 
 
+}
+//method2
+void control(void){
+    if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 0) && (sensor_values[3] == 0) && (sensor_values[4] == 1)){
+        err = 4;
+}
+    else if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 0) && (sensor_values[3] == 1) && (sensor_values[4] == 1)){
+        err = 3;
+    }
+    else if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 0) && (sensor_values[3] == 1) && (sensor_values[4] == 0)){
+        err = 2;
+    }
+    else if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 1) && (sensor_values[3] == 1) && (sensor_values[4] == 0)){
+        err = 1;
+    }
+    else if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 1) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+        err = 0;
+    }
+    else if((sensor_values[0] == 0) && (sensor_values[1] == 1) && (sensor_values[2] == 1) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+        err = -1;
+    }
+    else if((sensor_values[0] == 0) && (sensor_values[1] == 1) && (sensor_values[2] == 0) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+        err = -2;
+    }
+    else if((sensor_values[0] == 1) && (sensor_values[1] == 1) && (sensor_values[2] == 0) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+        err = -3;
+    }
+    else if((sensor_values[0] == 1) && (sensor_values[1] == 0) && (sensor_values[2] == 0) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+        err = -4;
+    } 
+    pid = err*motor.kp;
+    
+    if(motor.base_spr < pid){
+        pid = motor.base_spr;
+    }
+    if( pid < - motor.base_spl){
+        pid = -motor.base_spl;
+    }
+
+    motor.speed_r(motor.base_spr - pid);
+    motor.speed_l(motor.base_spl + pid); 
 }
\ No newline at end of file

--- a/motor.lib	Sat Jun 08 17:25:05 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/teams/embedded1/code/motor/#72fcb2468532