Moving from just a wheel to the swerve drive

Larry2/commands/drive_left_front.py

@@ -0,0 +1,29 @@
+import typing
+import commands2
+from subsystems.swerve_drive import SwerveDrive
+import wpilib
+import constants
+import conversions
+import math
+import ctre
+
+class DriveLeftFront(commands2.CommandBase):
+    def __init__(self,  swerveDrive: SwerveDrive, x: typing.Callable[[], float], y: typing.Callable[[], float]) -> None:
+        super().__init__()
+        self.drive = swerveDrive
+        #self.units = conversions.convertDegreesToTalonFXUnits(conversions.convertJoystickInputToDegrees(x(), y()))
+        self.x = x
+        self.y = y
+        self.addRequirements([self.drive])
+
+    def execute(self) -> None:
+        self.angle = conversions.convertJoystickInputToDegrees(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        self.magnitude = math.hypot(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        self.drive.turnWheel(self.drive.leftFrontSwerveModule, self.angle, self.magnitude)
+
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        return False

Larry2/robotcontainer.py

@@ -2,17 +2,17 @@
 import wpilib
 import commands2
 from commands2.button import JoystickButton
-from commands.drive_swerve_wheel_by_joystick import DriveSwerveWheelByJoysticks
-from commands.turn_to_specific_point import TurnToSpecificPoint
 
 import constants
 import ctre
 from wpilib import XboxController
 
 #import subsystems
-from subsystems.swerve_wheel import SwerveWheel
+from subsystems.swerve_drive import SwerveDrive
 #import commands
 from commands.joysticks import Joysticks
+from commands.turn_to_specific_point import TurnToSpecificPoint
+from commands.drive_left_front import DriveLeftFront
 
 class RobotContainer:
     def __init__(self) -> None:
@@ -24,7 +24,7 @@ def __init__(self) -> None:
         self.timer = wpilib.Timer
 
         #init subsystems
-        self.swerveDrive = SwerveWheel()
+        self.swerveDrive = SwerveDrive()
         #auto chooser
         #self.chooser = wpilib.SendableChooser()
 
@@ -39,9 +39,10 @@ def __init__(self) -> None:
 
         self.configureButtonBindings()
 
-        self.swerveDrive.setDefaultCommand(TurnToSpecificPoint(self.swerveDrive,  lambda: self.driverController.getLeftX(), lambda: self.driverController.getLeftY()))
+        self.swerveDrive.setDefaultCommand(DriveLeftFront(self.swerveDrive,  lambda: self.driverController.getLeftX(), lambda: self.driverController.getLeftY()))
+        #self.swerveDrive.setDefaultCommand(TurnToSpecificPoint(self.swerveDrive,  lambda: self.driverController.getLeftX(), lambda: self.driverController.getLeftY()))
         #self.swerveDrive.setDefaultCommand(Joysticks(self.swerveDrive, lambda: self.driverController.getLeftX(), lambda: self.driverController.getLeftY(), lambda: self.driverController.getRightX(), lambda: self.driverController.getRightY()))
-        ##self.swerveDrive.setDefaultCommand(DriveSwerveWheelByJoysticks(self.swerveDrive, lambda: self.driverController.getLeftX(), lambda: self.driverController.getRightY()))
+
     def configureButtonBindings(self):
         """This is where our trigger bindings for commands go"""
     """    

Larry2/subsystems/swerve_drive.py

@@ -0,0 +1,79 @@
+import commands2
+import ctre
+import constants
+import wpilib
+import conversions
+import math
+from subsystems.swerve_wheel import SwerveWheel
+
+class SwerveDrive(commands2.SubsystemBase):
+    def __init__(self) -> None:
+        super().__init__()
+        # init motors
+        self.leftFrontDirection = ctre.TalonFX(constants.kleftFrontDirectionID)
+        self.leftFrontSpeed = ctre.TalonFX(constants.kleftFrontSpeedID)
+
+        self.leftRearDirection = ctre.TalonFX(constants.kleftRearDirectionID)
+        self.leftRearSpeed = ctre.TalonFX(constants.kleftRearSpeedID)
+
+        self.rightFrontDirection = ctre.TalonFX(constants.krightFrontDirectionID)
+        self.rightFrontSpeed = ctre.TalonFX(constants.krightFrontSpeedID)
+
+        self.rightRearDirection = ctre.TalonFX(constants.krightRearDirectionID)
+        self.rightRearSpeed = ctre.TalonFX(constants.krightRearSpeedID)
+
+        # init swerve modules
+        self.leftFrontSwerveModule = SwerveWheel(self.leftFrontDirection, self.leftFrontSpeed)
+        self.leftRearSwerveModule = SwerveWheel(self.leftRearDirection, self.leftRearSpeed)
+
+        self.rightFrontSwerveModule = SwerveWheel(self.rightFrontDirection, self.rightFrontSpeed)
+        self.rightRearSwerveModule = SwerveWheel(self.rightRearDirection, self.rightRearSpeed)
+
+    def turnWheel(self, module: SwerveWheel, direction: float, magnitude: float):
+        self.units = conversions.convertDegreesToTalonFXUnits(direction)
+        if magnitude >= 1.0:
+            magnitude = 1.0
+        # find current angle
+        currentAngle = conversions.convertTalonFXUnitsToDegrees(module.directionMotor.getSelectedSensorPosition())
+        # see if the abs value is greater than 180
+        if math.fabs(direction) >= 180.0:
+            # find the abs value of the opposite angle
+            opposAngle = math.fabs(direction) - 180.0
+        else:
+            # find the abs value of the opposite angle
+            opposAngle = math.fabs(direction) + 180.0
+        # print some stats for debugging
+        wpilib.SmartDashboard.putNumber(" Original Angle -", direction)
+        wpilib.SmartDashboard.putNumber(" Abs Opposit Angle -", opposAngle)
+        # check if the joystick is in use
+        if magnitude != 0.0:
+            # if the original angle is closer
+            if math.fabs(currentAngle - direction) <= math.fabs(currentAngle - opposAngle):
+                #turn to the original angle
+                if direction == 0.0:
+                    if (2048*constants.ksteeringGearRatio) - self.units < module.directionMotor.getSelectedSensorPosition():
+                        module.turn(2048*constants.ksteeringGearRatio)
+                    else:
+                        module.turn(0.0)
+                else:
+                    module.turn(self.units*constants.ksteeringGearRatio)
+                #move in the normal way
+                module.move(magnitude)
+            else: # the opposite angle is closer
+                #turn to the other angle
+                if direction == 0.0:
+                    if (2048*constants.ksteeringGearRatio) - conversions.convertDegreesToTalonFXUnits(opposAngle) < module.directionMotor.getSelectedSensorPosition():
+                        module.turn(2048*constants.ksteeringGearRatio)
+                    else:
+                        module.turn(0.0)
+                else:
+                #change direction of the speed motor
+                    module.turn(conversions.convertDegreesToTalonFXUnits(opposAngle)*constants.ksteeringGearRatio)
+                #move in the opposite directionj
+                module.move(-magnitude)
+
+    def stopAllMotors(self):
+        self.leftFrontSwerveModule.stopAllMotors()
+        self.leftRearSwerveModule.stopAllMotors()
+        self.rightFrontSwerveModule.stopAllMotors()
+        self.rightRearSwerveModule.stopAllMotors()

Larry2/subsystems/swerve_wheel.py

@@ -6,10 +6,10 @@
 
 from conversions import convertDegreesToTalonFXUnits
 class SwerveWheel(commands2.SubsystemBase):
-    def __init__(self) -> None:
+    def __init__(self, directionMotor: ctre.TalonFX, speedMotor:ctre.TalonFX) -> None:
         super().__init__()
-        self.directionMotor = ctre.TalonFX(kleftFrontDirectionID)
-        self.speedMotor = ctre.TalonFX(kleftFrontSpeedID)
+        self.directionMotor = directionMotor
+        self.speedMotor = speedMotor
 
         self.directionMotor.configSelectedFeedbackSensor(ctre.FeedbackDevice.IntegratedSensor, 0, ktimeoutMs)