add drivetrain rotation curve and remove drivetrain velocity deadzone

command/drivetrain.py

@@ -25,13 +25,19 @@
 from utils import POI
 
 
-
-def curve(x):
-    if abs(x) < 0.11:
+def deadzone(x, d=config.drivetrain_deadzone):
+    if abs(x) < d:
         return 0
     if x < 0:
-        return 1.12*(x + .11)
-    return 1.12*(x - .11)
+        return (x+d)/(1-d)
+    return (x-d)/(1-d)
+
+def curve(x, d=config.drivetrain_deadzone, c=config.drivetrain_curve):
+    if abs(x) < d:
+        return 0
+    elif x < 0:
+        return -1*math.pow((-1*(x+d)/(1-d)), c)
+    return math.pow(((x-d)/(1-d)), c)
 
 def bound_angle(degrees: float):
     degrees = degrees % 360
@@ -57,8 +63,8 @@ def execute(self) -> None:
             self.subsystem.axis_rotation.value,
         )
 
-        dx = curve(dx)
-        dy = curve(dy)
+        dx = deadzone(dx)
+        dy = deadzone(dy)
         d_theta = curve(d_theta)
 
         # dx *= self.subsystem.max_vel
@@ -219,8 +225,8 @@ def robot_angle():
             self.subsystem.ready_to_shoot = True
         else:
             self.subsystem.ready_to_shoot = False
-        dx = curve(dx)
-        dy = curve(dy)
+        dx = deadzone(dx)
+        dy = deadzone(dy)
 
         dx *= states.drivetrain_controlled_vel
         dy *= states.drivetrain_controlled_vel
@@ -316,8 +322,8 @@ def execute(self) -> None:
 
         )
 
-        dx = curve(dx)
-        dy = curve(dy)
+        dx = deadzone(dx)
+        dy = deadzone(dy)
 
         dx *= states.drivetrain_controlled_vel
         dy *= states.drivetrain_controlled_vel

config.py

@@ -254,6 +254,9 @@ class LimelightPosition:
 drivetrain_max_vel_auto: float = 4.5
 drivetrain_max_accel_auto: float = 4
 
+drivetrain_deadzone: float = 0.1
+drivetrain_curve: float = 3
+
 #Shooting
 drivetrain_aiming_offset: degrees = 2.0 # degrees
 drivetrain_aiming_move_speed_threshold: meters_per_second = 0.4

toolkit/subsystem_templates/drivetrain/swerve_drivetrain.py

@@ -180,8 +180,6 @@ class SwerveDrivetrain(Subsystem):
     track_width: meters = 1
     max_vel: meters_per_second = 20 * miles_per_hour_to_meters_per_second
     max_angular_vel: radians_per_second = 4 * rotations_per_second__to__radians_per_second
-    deadzone_velocity: meters_per_second = 0.05  # Does not run within this speed
-    deadzone_angular_velocity: radians_per_second = 5 * degrees_per_second__to__radians_per_second  # Will not turn within this speed
     start_pose: Pose2d = Pose2d(0, 0, 0)  # Starting pose of the robot from wpilib Pose (x, y, rotation)
     gyro_start_angle: radians = 0
     gyro_offset: degrees = 0
@@ -294,12 +292,6 @@ def set_robot_centric(self, vel: tuple[meters_per_second, meters_per_second], an
 
         dx, dy = vel
 
-        dx = 0 if abs(dx) < self.deadzone_velocity else dx
-
-        dy = 0 if abs(dy) < self.deadzone_velocity else dy
-
-        angular_vel = 0 if abs(angular_vel) < self.deadzone_angular_velocity else angular_vel
-
         self.chassis_speeds = ChassisSpeeds(dx, dy, angular_vel)
 
         new_states = self.kinematics.toSwerveModuleStates(self.chassis_speeds)