Revision 99:d8123dfcf757, committed 2019-06-05

Comitter:

jinyoung_KIL

Date:

Wed Jun 05 13:56:18 2019 +0000

Parent:

98:ac298f47375b

Commit message:

20190605

Changed in this revision

--- a/RemoteIR.lib	Wed Jun 05 04:55:45 2019 +0000
+++ b/RemoteIR.lib	Wed Jun 05 13:56:18 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/jinyoung_KIL/code/RemoteIR/#96209c979a7f
+https://os.mbed.com/teams/embedded1/code/RemoteIR/#5e445169cf83

--- a/main.cpp	Wed Jun 05 04:55:45 2019 +0000
+++ b/main.cpp	Wed Jun 05 13:56:18 2019 +0000
@@ -9,7 +9,7 @@
 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(D14, D15);                                      // D14, D15
 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);    // D9, (D8: D/C - data/command change)
 DigitalOut DataComm(D8);
 
@@ -22,10 +22,10 @@
 
 unsigned int sensor_values[NUM_SENSORS];
 unsigned int max_sensor_values[NUM_SENSORS];
-unsigned int min_sensor_values[NUM_SENSORS]; 
+unsigned int min_sensor_values[NUM_SENSORS];
 
 unsigned int *calibratedMin;
-unsigned int *calibratedMax;   
+unsigned int *calibratedMax;
 
 int value;
 float bat;
@@ -39,12 +39,14 @@
 
 int ch;
 
+
+
 int readLine(unsigned int *sensor_values);
 void tr_ready(void);
 void calibration(void);
 void init(void);
-void tracing(void);
-//void tracing_pid(void);
+
+void tracing_pid(void);
 void set_ir_val(void);
 void get_pos(void);
 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
@@ -52,34 +54,34 @@
 void oled_disp(void);
 
 int main()
-{   
+{
     pc.printf("start\r\n");
     spi.format(16, 0);
     spi.frequency(2000000);
 
     init();
-    
+
     oled_thread.start(&oled_disp);
-    remote_thread.start(&remote_ctrl);    
-    while(!motor.start);
-    
+    remote_thread.start(&remote_ctrl);
+    while(!motor.cal_start);
+
     calibration();
-    
+
     while(!flag);
 
     TR_thread.start(&tr_ready);
 
-    
+
     while(!start);
-      
-    pc.printf("motor forward\r\n");  
+
+    pc.printf("motor forward\r\n");
     motor.forward();
-    
+
     while (1);
 }
 
 void remote_ctrl(void){
-    
+
     while(1){
     uint8_t buf1[32];
     uint8_t buf2[32];
@@ -93,17 +95,19 @@
         if (bitlength1 < 0) {
             continue;
         }
-        
-        //display_status("RECV", bitlength1);
-        //display_data(buf1, bitlength1);
-        //display_format(format);  
 
-    }  
+    }
 }
 
 void init(void){
+    motor.cal_start = 0;
+    motor.cal_stop = 0; 
+    
+    motor.kp = 10;
+    motor.max = 130;
+ 
     DataComm = 0;
-    
+
     calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
     calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
 
@@ -118,8 +122,9 @@
     myOled.begin();
     while(1){
         myOled.clearDisplay();
-        myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
-        myOled.display();          
+        //myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
+        myOled.printf("kp:%.2f, max:%d\r", motor.kp, motor.max);
+        myOled.display();
     }
 }
 
@@ -137,28 +142,28 @@
 
 void read_ir(void){
     ch = 0;
-        
+
     spi_cs = 0;
     wait_us(2);
     value = spi.write(ch << 12);
     spi_cs = 1;
-        
-    wait_us(21);                     
-    
+
+    wait_us(21);
+
     for(int i = 0; i < 5; i++){
-        
+
         ch += 1;
-        
+
         spi_cs = 0;
         wait_us(2);
         value = spi.write(ch << 12);
         spi_cs = 1;
-            
-        wait_us(21);                   
-    
+
+        wait_us(21);
+
         value = value >> 6;
         value = value * 3300 / 0x3FF;
-        
+
         sensor_values[i] = value;
     }
 }
@@ -166,55 +171,52 @@
 void calibration(void){
     pc.printf("calibration start \r\n");
     
-    for(int i = 0; i < 10; i++){
+    int i = 0;
+    while(!motor.cal_stop){    
         read_ir();
         for(int j = 0; j < NUM_SENSORS; j++){
             if(i == 0 || max_sensor_values[j] < sensor_values[j]){
                 max_sensor_values[j] = sensor_values[j];
             }
-    
+
             if(i == 0 || min_sensor_values[j] > sensor_values[j]){
                 min_sensor_values[j] = sensor_values[j];
             }
         }
-        wait(0.5);
+        i = 1;
     }
-    
+
     for(int i = 0; i < NUM_SENSORS; i++){
         if(min_sensor_values[i] < calibratedMin[i])
             calibratedMin[i] = min_sensor_values[i];
         if(max_sensor_values[i] > calibratedMax[i])
             calibratedMax[i] = max_sensor_values[i];
     }
-    pc.printf("calibration complete\r\n");  
-    flag =1;   
+    
+    pc.printf("calibration complete\r\n");
+    flag = 1;
 }
 
 void tr_ready(void){
-    
+
     while(1){
         // read current IR 1 ~ IR 5
         read_ir();
-        
+
         // set range under 1000
         set_ir_val();
-               
+
         // get current position
         get_pos();
+
+        tracing_pid();
         
-//        pc.printf("pos = %d\r\n", pos);
-        // start tracing_pid_pid
-        //tracing_pid();
-        tracing();      
-//        pc.printf("pos = %d\r\n", pos);
-//        pc.printf("on_line = %d\r\n", on_line);
-                        
-//        wait(1);
-    }             
+        
+    }
 }
 
 void set_ir_val(){
-    for(int i=0; i < NUM_SENSORS; i++)
+    for(int i = 0; i < NUM_SENSORS; i++)
     {
         unsigned int denominator;
 
@@ -225,120 +227,100 @@
             x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
         if(x < 0)   x = 0;
         else if(x > 1000)   x = 1000;
+
+        sensor_values[i] = x;
         
-        sensor_values[i] = x;
    }
-//   for(int i = 0; i <NUM_SENSORS; i++){
-//        pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
-//    }
+   
+   // 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);        
+    }
+        
+   for(int i = 0; i < NUM_SENSORS; i++){
+
+        pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
+    }
+    
+    pc.printf("\r\n");
 }
 
 void get_pos(){
     on_line = 0;
     avg = 0;
-    sum = 0;   
+    sum = 0;
 
     for(int i = 0; i < NUM_SENSORS; i++){
         int val = sensor_values[i];
-        
-        // determine "on_line" or "out_line"        
-        if(val < 800){
+
+        // determine "on_line" or "out_line"
+        if(val < 500){
             on_line = 1;
         }
-    
-        // under 
+
+        // under
         if(val > 5){
             avg += (long)(val) * (i * 1000);
-            sum += val; 
-        }    
+            sum += val;
+        }
     }
-    
+
     // out_line
     if(!on_line){
-        if(pos < (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
+        if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
             pos = 0;                             // last_vlaue = 0
-        }   
-        else{                                           // right -> out-line (over 2000)
+        }
+        else{                                    // right -> out-line (over 2000)
              pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
         }
     }
     // on_line
     else{
-        pos = avg / sum;        
-    }
-    pc.printf("position: %d\r\n", pos);
-    start = 1;
-}
-
-void tracing(){
-    // totally right
-    if(pos == 4000){
-        motor.speedup_r(3);
-    }
-    // detect IR1 on line
-    else if(pos >= 2300){
-        motor.speedup_r(3);    
-    }
-    // detect IR2 on line
-    else if(pos >= 2100){
-        motor.speedup_r(1);
+        pos = avg / sum;
     }
-    // detect IR3 on line
-    else if(pos >= 1900){
-        motor.speed_r(0.09);
-        motor.speed_l(0.085);
-    }
-    // detect IR4 on line
-    else if(pos >= 1700){
-        motor.speedup_l(1);    
-    }
-    // detect IR5 on line
-    else if(pos >= 1200){
-        motor.speedup_l(3);    
-    }
-    // totally left
-    else if(pos == 0){
-        motor.speedup_l(3);            
-    }
-    else {
-        pc.printf("error pos: %d\r\n", pos);
-        pc.printf("error\r\n");
-    }
-    wait(0.001);
 
+    start = 1;
 }
 
 void tracing_pid(void){
 
+    
     int proportional = pos - 2000;
     
     int derivative = proportional - last_proportional;
     integral += proportional;
-    
+
     last_proportional  = proportional;
-    
-    int power_difference = proportional / 20 + integral / 10000 + derivative * 10;
-//    int power_difference = proportional / ;
-    
-    const int max = 150;
-    
-    if(power_difference > max)
-        power_difference = max;
-        
-    if(power_difference < -max)
-        power_difference = -max;
+
+    //int power_difference = proportional / 20 + integral / 10000 + derivative * 10;
+    int power_difference = proportional / motor.kp;
+
+    const float std = 1000;
+
+    if(power_difference > motor.max)
+        power_difference = motor.max;
+
+    if(power_difference < -motor.max)
+        power_difference = -motor.max;
     
-    if(power_difference < 0){
-        motor.speed_l((max + power_difference)/255);
-        motor.speed_r(max/255);
+    // bot position: right
+    if(power_difference > 15){
+        motor.speed_r((motor.max + power_difference)/std);
+        motor.speed_l(motor.max/33);
+    }
+    // bot position: left
+    else if(power_difference < -15){
+        motor.speed_r(motor.max/std);
+        motor.speed_l((motor.max - power_difference)/std);
     }
-    else{
-        motor.speed_l(max/255);
-        motor.speed_r((max - power_difference)/255);
+    else{ //online
+        motor.speed_l(motor.init_sp_l);
+        motor.speed_r(motor.init_sp_r);
     }
-    
-    pc.printf("proportional: %d\r\n", proportional);
-    pc.printf("power_difference: %d\r\n", power_difference);
-    pc.printf("derivative: %d\r\n", derivative);    
-    pc.printf("integral: %d\r\n",integral); 
-}
+
+    // pc.printf("proportional: %d\r\n", proportional);
+     //pc.printf("(max + power_difference): %d\r\n", motor.max + power_difference);
+    // pc.printf("derivative: %d\r\n", derivative);
+    // pc.printf("integral: %d\r\n",integral);
+}
\ No newline at end of file

--- a/motor.lib	Wed Jun 05 04:55:45 2019 +0000
+++ b/motor.lib	Wed Jun 05 13:56:18 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/jinyoung_KIL/code/motor/#72fcb2468532
+https://os.mbed.com/teams/embedded1/code/motor/#72fcb2468532