Diff: main.cpp

Revision:

111:dc3d8ba48f83

Parent:

110:13f123179c26

Child:

112:744fef64b8ba

--- a/main.cpp	Wed Jun 12 08:22:54 2019 +0000
+++ b/main.cpp	Thu Jun 13 02:55:34 2019 +0000
@@ -98,7 +98,7 @@
     i2c_init();
     init();
  
-    oled_thread.start(&oled_disp);
+    
     remote_thread.start(&remote_ctrl);
     
     while(!motor.cal_start);
@@ -119,6 +119,7 @@
     timer_s.start(); 
 
     while(!timer_flag);
+    oled_thread.start(&oled_disp);
     msec_t = timer_ms.read_ms();
     msec_t %= 1000;
     sec_t = timer_s.read();
@@ -176,7 +177,7 @@
     motor.kp = 10.0;
     motor.kd = 0.0;
 //    motor.ki = 5000;
-    motor.max = 100;
+    motor.max = 125;
  
     DataComm = 0;
  
@@ -297,8 +298,8 @@
     
         // get current position
         get_pos();
-//        pc.printf("pos: %d\r\n", pos);
-//        pc.printf("on_line: %d\r\n\n", on_line);
+        pc.printf("pos: %d\r\n", pos);
+        pc.printf("on_line: %d\r\n\n", on_line);
 
         tracing_pid();
     
@@ -367,9 +368,11 @@
     if(!on_line){
         if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
             pos = 0;                             // last_vlaue = 0
+//            pos = 200;
         }
         else{                                    // right -> out-line (over 2000)
              pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
+//            pos = 3800;
         }
     }
     // on_line
@@ -402,12 +405,12 @@
     // 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);
+        motor.speed_l((motor.max - power_difference)/std + 0.005);
     }
     // bot position: left
     else if(power_difference < -15){
         motor.speed_r((motor.max + power_difference)/std);
-        motor.speed_l((motor.max - power_difference)/std);
+        motor.speed_l((motor.max - power_difference)/std + 0.005);
     }
     else{ //online
         motor.speed_l(motor.init_sp_l);