changed sleep_for to safer value to decrease chance of motor/sensor disconect

Author: jaaaaa132

src/diferential_drive.cpp

@@ -48,10 +48,6 @@ void Diferential_drive::track_position(){
 
     int16_t left_motor_pos_dif = (left_motor_pos - left_motor_last_pos) * (left_motor_inverted ? -1 : 1);
     int16_t right_motor_pos_dif = (right_motor_pos - right_motor_last_pos) * (right_motor_inverted ? -1 : 1);
-    if(right_motor_pos_dif == 0 && left_motor_pos_dif == 0) {
-      std::this_thread::sleep_for(std::chrono::microseconds(500)); 
-      continue;
-    }
     float left_wheel_dist = wheel_circumference * left_motor_pos_dif / 360;
     float right_wheel_dist = wheel_circumference * right_motor_pos_dif / 360;
     float average_distance = (left_wheel_dist + right_wheel_dist) / 2;
@@ -66,8 +62,8 @@ void Diferential_drive::track_position(){
     position = p_position;
     left_motor_last_pos = left_motor_pos;
     right_motor_last_pos = right_motor_pos;
-    angle_last = angle_gyro;
-    std::this_thread::sleep_for(std::chrono::microseconds(1000)); 
+    //angle_last = angle_gyro;
+    std::this_thread::sleep_for(std::chrono::microseconds(1500)); 
   }
 }
 
@@ -211,7 +207,7 @@ void Diferential_drive::rotate_to_abs_angle(float angle, float precision, int ma
     int speed = std::fmin(1, std::fmax(-1, angle_dif * P_const));
     left_motor->run_direct(fmax(abs(speed) * max_speed, min_speed), left_motor_inverted ? !(speed < 0) : speed < 0);
     right_motor->run_direct(fmax(abs(speed) * max_speed, min_speed), !right_motor_inverted ? !(speed < 0) : speed < 0);
-    std::this_thread::sleep_for(std::chrono::microseconds(1000)); // same as in position tracking
+    std::this_thread::sleep_for(std::chrono::microseconds(2000));
     angle_dif = normalize_angle(position.angle - angle);
   }
 
@@ -232,7 +228,7 @@ void Diferential_drive::go_until_reflection(int reflection, bool darker, int spe
   bool left_motor_stopped = false, right_motor_stopped = false;
 
   while(true){
-    // handle full sensor disconect is_connected doesnt register disconects
+    // handle full sensor disconect, is_connected() doesnt register disconects
     if(__builtin_expect(!color_left->is_connected() && !color_right->is_connected(), 0)){
       std::cout << "error in go_until_reflection no color sensor is connected" << std::endl;
       std::cerr << "error in go_until_reflection no color sensor is connected" << std::endl;
@@ -269,7 +265,7 @@ void Diferential_drive::go_until_reflection(int reflection, bool darker, int spe
       right_motor_stopped = true;
     }
 
-    std::this_thread::sleep_for(std::chrono::microseconds(1000));
+    std::this_thread::sleep_for(std::chrono::microseconds(1500));
     if(left_motor_stopped && right_motor_stopped) break;
   }
 }