Encoder PIO fully functional, drivetrain methods all fixed

Author: ksiegall

XRPLib/differential_drive.py

@@ -19,8 +19,8 @@ def get_default_differential_drive(cls):
 
         if cls._DEFAULT_DIFFERENTIAL_DRIVE_INSTANCE is None:
             cls._DEFAULT_DIFFERENTIAL_DRIVE_INSTANCE = cls(
-            EncodedMotor.get_default_left_motor(),
-            EncodedMotor.get_default_right_motor(),
+            EncodedMotor.get_default_encoded_motor(0),
+            EncodedMotor.get_default_encoded_motor(1),
             IMU.get_default_imu()
         )
 
@@ -105,7 +105,7 @@ def straight(self, distance: float, speed: float = 0.5, timeout: float = None, m
 
         if main_controller is None:
             main_controller = PID(
-                kp = 2,
+                kp = 0.5,
                 minOutput = 0.12,
                 maxOutput = speed,
                 tolerance = 0.1,
@@ -115,7 +115,7 @@ def straight(self, distance: float, speed: float = 0.5, timeout: float = None, m
         # Secondary controller to keep encoder values in sync
         if secondary_controller is None:
             secondary_controller = PID(
-                kp = 0.02, 
+                kp = 0.0175, kd=0.005, 
             )
 
         rotationsToDo = distance  / (self.wheel_diam * math.pi)
@@ -228,11 +228,8 @@ def turn(self, turn_degrees: float, speed: float = 0.5, timeout: float = None, m
             if main_controller.is_done() or time_out.is_done():
                 break
 
-
             self.set_effort(-turnSpeed - encoderCorrection, turnSpeed - encoderCorrection)
 
-            #print(turnError, turnSpeed)
-
             time.sleep(0.01)
 
         self.stop()

XRPLib/encoded_motor.py

@@ -53,17 +53,6 @@ def get_default_encoded_motor(cls, index:int = 0):
                 )
             motor = cls._DEFAULT_LEFT_MOTOR_INSTANCE
         return motor
-
-    @classmethod
-    def get_default_encoded_motor(cls):
-        """
-        Get the default XRP v2 right motor instance. This is a singleton, so only one instance of the drivetrain will ever exist.
-        Motor pins set to 14 and 15 and the encoder pins set to 12 and 13
-        """
-
-        
-
-        return cls._DEFAULT_RIGHT_MOTOR_INSTANCE
 
     def __init__(self, motor: Motor, encoder: Encoder):
 

XRPLib/encoder.py

@@ -31,7 +31,7 @@ def get_position_ticks(self):
         return ticks
 
     def get_position(self):
-        return self.getTicks() / self.ticksPerShaftRotation
+        return self.get_position_ticks() / self.ticksPerShaftRotation
 
     @rp2.asm_pio(in_shiftdir=rp2.PIO.SHIFT_LEFT, out_shiftdir=rp2.PIO.SHIFT_RIGHT)
     def _encoder():

XRPLib/resetbot.py

@@ -1,7 +1,5 @@
 from XRPLib.encoded_motor import EncodedMotor
 
 # using the EncodedMotor since the default drivetrain uses the IMU and takes 3 seconds to init
-left = EncodedMotor.get_default_left_motor()
-right = EncodedMotor.get_default_right_motor()
-left.set_effort(0.0)
-right.set_effort(0.0)
+for i in range(4):
+    EncodedMotor.get_default_encoded_motor(i).reset_encoder_position()