Auto speed changing

src/axle_manager/axle_manager/hdc2460_node.py

@@ -18,9 +18,9 @@ def __init__(self):
                 ('serialBitRate',115200),
                 ('leftChannel',1),
                 ('rightChannel',2),
-                ('maxSpeed',500),
-                ('accelRate',31860),
-                ('brakeRate',10620),
+                ('maxSpeed',250), #Max Pulses per Second (500 ~ 1 meter/second) #500 (changed 12/8/2025)
+                ('accelRate',2500), #0.1 * RPM/s #31860 (changed 12/8/2025)
+                ('brakeRate',2500), #0.1 * RPM/s #10620 (changed 12/8/2025)
                 ('pivotDevice',"FAC"),
                 ('pivotExtendChannel',1),
                 ('pivotRetractChannel',2),
@@ -50,11 +50,11 @@ def __init__(self):
         rightChannel_param = self.get_parameter('rightChannel').value
         rightChannel = int(rightChannel_param) if rightChannel_param is not None else 2
         maxSpeed_param = self.get_parameter('maxSpeed').value
-        maxSpeed = int(maxSpeed_param) if maxSpeed_param is not None else 500
+        maxSpeed = int(maxSpeed_param) if maxSpeed_param is not None else 250 #500 (changed 12/8/2025)
         accelRate_param = self.get_parameter('accelRate').value
-        accelRate = int(accelRate_param) if accelRate_param is not None else 31860
+        accelRate = int(accelRate_param) if accelRate_param is not None else 15930 #31860 (changed 12/8/2025)
         brakeRate_param = self.get_parameter('brakeRate').value
-        brakeRate = int(brakeRate_param) if brakeRate_param is not None else 10620
+        brakeRate = int(brakeRate_param) if brakeRate_param is not None else 5310 #10620 (changed 12/8/2025)
         self.pivotDevice = str(self.get_parameter('pivotDevice').value)
         pivotLeft_param = self.get_parameter('pivotExtendChannel').value
         self.pivotLeft = int(pivotLeft_param) if pivotLeft_param is not None else 1

src/control_pkg/control_pkg/drive_commands.py

@@ -31,10 +31,7 @@ def end(self):
         self.drive(float(0))
 
 # measured in m/s
-top_speed = 1
-# measured in m/s/s
-acceleration = 3
-deceleration = 1
+TOP_SPEED = 0.5
 
 class DriveDistanceCommand(DriveTimeCommand):
     """
@@ -44,16 +41,13 @@ def __init__(self, speed: float, distance: float, drive: Callable[[float], None]
         super().__init__(speed=speed, drive_time=self.calculate_time(speed, distance), drive=drive)
 
     def calculate_time(self, speed, distance) -> float:
-        real_speed = abs(speed) * top_speed
+        real_speed = abs(speed) * TOP_SPEED
+
+        if distance < real_speed:
+            return sqrt((distance / real_speed))
 
-        acceleration_time = real_speed / acceleration
-        deceleration_time = real_speed / deceleration
-
-        if distance < real_speed * (acceleration_time + deceleration_time) / 2:
-            return sqrt((2 * distance * acceleration * deceleration) / (acceleration + deceleration)) / acceleration
-
-        hold_time = ((2 * distance / real_speed) - acceleration_time - deceleration_time) / 2
-        return acceleration_time + hold_time
+        # Time to hold in place
+        return distance / real_speed
 
 max_turn = 18.25
 
@@ -78,21 +72,21 @@ def initialize(self):
         path_segment_1.add_commands(
             TurnToDegreesCommand(path[0] * 18.624, self.get_pivot_position, self.drive_pivot),
             WaitCommand(2),
-            DriveDistanceCommand(top_speed * path[1], path[2], self.drive)
+            DriveDistanceCommand(TOP_SPEED * path[1], path[2], self.drive)
         )
 
         path_segment_2 = SequentialCommandGroup()
         path_segment_2.add_commands(
             TurnToDegreesCommand(path[3] * 18.624, self.get_pivot_position, self.drive_pivot),
             WaitCommand(2),
-            DriveDistanceCommand(top_speed * path[4], path[5], self.drive)
+            DriveDistanceCommand(TOP_SPEED * path[4], path[5], self.drive)
         )
 
         path_segment_3 = SequentialCommandGroup()
         path_segment_3.add_commands(
             TurnToDegreesCommand(path[6] * 18.624, self.get_pivot_position, self.drive_pivot),
             WaitCommand(2),
-            DriveDistanceCommand(top_speed * path[7], path[8], self.drive)
+            DriveDistanceCommand(TOP_SPEED * path[7], path[8], self.drive)
         )
 
         needed_segments: list[SequentialCommandGroup] = []