Fix speed control indeterminant timestep issue

Author: ksiegall

.picowgo

@@ -38,7 +38,7 @@ def circle():
 # Follow the perimeter of a square with variable sidelength
 def square(sidelength):
     for sides in range(4):
-        drivetrain.straight(sidelength, 80)
+        drivetrain.straight(sidelength, 0.8)
         drivetrain.turn(90)
     # Alternatively:
     # polygon(sidelength, 4)

Examples/drive_examples.py

@@ -203,16 +203,16 @@ def straight(self, distance: float, max_effort: float = 0.5, timeout: float = No
         return not time_out.is_done()
 
 
-    def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = None, main_controller: Controller = None, secondary_controller: Controller = None, use_imu:bool = True) -> bool:
+    def turn(self, turn_degrees: float, max_speed: float = 40, timeout: float = None, main_controller: Controller = None, secondary_controller: Controller = None, use_imu:bool = True) -> bool:
         """
         Turn the robot some relative heading given in turnDegrees, and exit function when the robot has reached that heading.
         Speed is bounded from -1 (turn counterclockwise the relative heading at full speed) to 1 (turn clockwise the relative heading at full speed)
         Uses the IMU to determine the heading of the robot and P control for the motor controller.
 
         :param turnDegrees: The number of angle for the robot to turn (In Degrees)
         :type turnDegrees: float
-        :param max_effort: The max effort for which the robot to travel (Bounded from -1 to 1). Default is half effort forward.
-        :type max_effort: float
+        :param max_speed: The max speed for each wheel of the robot to spin, in cm/s. Default is 40 cm/s.
+        :type max_speed: float
         :param timeout: The amount of time before the robot stops trying to turn and continues to the next step (In Seconds)
         :type timeout: float
         :param main_controller: The main controller, for handling the angle turned
@@ -225,8 +225,8 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No
         :rtype: bool
         """
 
-        if max_effort < 0:
-            max_effort *= -1
+        if max_speed < 0:
+            max_speed *= -1
             turn_degrees *= -1
 
         time_out = Timeout(timeout)
@@ -238,7 +238,7 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No
                 kp = .015,
                 kd = 0.0012,
                 minOutput = 0.25,
-                maxOutput = max_effort,
+                maxOutput = max_speed,
                 tolerance = 0.5,
                 toleranceCount = 3
             )
@@ -273,7 +273,7 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No
             if main_controller.is_done() or time_out.is_done():
                 break
 
-            self.set_effort(-turnSpeed - encoderCorrection, turnSpeed - encoderCorrection)
+            self.set_speed(-turnSpeed - encoderCorrection, turnSpeed - encoderCorrection)
 
             time.sleep(0.01)
 

XRPLib/differential_drive.py

@@ -131,6 +131,7 @@ def set_speed(self, speed_rpm: float = None):
         # Convert from rev per min to counts per 20ms (60 sec/min, 50 Hz)
         self.target_speed = speed_rpm*self._encoder.resolution/(60*50)
         self.speedController.clear_history()
+        self.prev_position = self.get_position_counts()
 
     def set_speed_controller(self, new_controller: Controller):
         """

XRPLib/encoded_motor.py

@@ -117,4 +117,5 @@ def clear_history(self):
         self.prevError = 0
         self.prevIntegral = 0
         self.prevOutput = 0
-        self.times = 0
+        self.times = 0
+        self.prevTime = None