Add Kraken Motor Class to control_loop.py
Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: Idfb68b967e05a1d638ce6097b81efb86e445c756
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 7e54bdf..e179193 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -743,3 +743,33 @@
# Diameter of 1.9", weight of: 100 grams
# TODO(austin): Get a number from Scott Westbrook for the mass
self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0)
+
+
+class KrakenFOC(object):
+ """Class representing the WCP Kraken X60 motor using
+ Field Oriented Controls (FOC) communication.
+
+ All numbers based on data from
+ https://wcproducts.com/products/kraken.
+ """
+
+ def __init__(self):
+ # Stall Torque in N m
+ self.stall_torque = 9.37
+ # Stall Current in Amps
+ self.stall_current = 483.0
+ # Free Speed in rad / sec
+ self.free_speed = 5800.0 / 60.0 * 2.0 * numpy.pi
+ # Free Current in Amps
+ self.free_current = 2.0
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Motor inertia in kg m^2
+ # Diameter of 1.9", weight of: 100 grams
+ # TODO(Filip): Update motor inertia for Kraken, currently using Falcon motor inertia
+ self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0)
diff --git a/y2024/control_loops/python/drivetrain.py b/y2024/control_loops/python/drivetrain.py
index 1d1054a..64dcc47 100644
--- a/y2024/control_loops/python/drivetrain.py
+++ b/y2024/control_loops/python/drivetrain.py
@@ -18,7 +18,7 @@
# TODO(austin): Measure radius a bit better.
robot_radius=0.39,
wheel_radius=2.5 * 0.0254,
- motor_type=control_loop.Falcon(),
+ motor_type=control_loop.KrakenFOC(),
num_motors=3,
G=(14.0 / 52.0) * (26.0 / 58.0),
q_pos=0.24,