From 04dfbbeb8202e2beedb63f87137cb3f78db62f00 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 13:40:18 -0700 Subject: [PATCH 01/15] driveTrain example updated --- DifferentialDriveBot/drivetrain.py | 30 +++++++++++++++++++++--------- 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/DifferentialDriveBot/drivetrain.py b/DifferentialDriveBot/drivetrain.py index 6f97d873..8d86401c 100755 --- a/DifferentialDriveBot/drivetrain.py +++ b/DifferentialDriveBot/drivetrain.py @@ -23,16 +23,28 @@ class Drivetrain: ENCODER_RESOLUTION = 4096 # counts per revolution def __init__(self): - leftLeader = wpilib.PWMSparkMax(1) - leftFollower = wpilib.PWMSparkMax(2) - rightLeader = wpilib.PWMSparkMax(3) - rightFollower = wpilib.PWMSparkMax(4) + self.leftLeader = wpilib.PWMSparkMax(1) + self.leftFollower = wpilib.PWMSparkMax(2) + self.rightLeader = wpilib.PWMSparkMax(3) + self.rightFollower = wpilib.PWMSparkMax(4) + + + # Make sure both motors for each side are in the same group + self.leftLeader.addFollower(self.leftFollower) + self.rightLeader.addFollower(self.rightFollower) + + # We need to invert one side of the drivetrain so that positive voltages + # result in both sides moving forward. Depending on how your robot's + # gearbox is constructed, you might have to invert the left side instead. + self.rightLeader.setInverted(True) + + # turns the motors into a group so that we can set voltage to both at once + self.robotDrive = wpilib.drive.DifferentialDrive(self.leftLeader, self.rightLeader) + self.leftEncoder = wpilib.Encoder(0, 1) self.rightEncoder = wpilib.Encoder(2, 3) - self.leftGroup = wpilib.MotorControllerGroup(leftLeader, leftFollower) - self.rightGroup = wpilib.MotorControllerGroup(rightLeader, rightFollower) self.gyro = wpilib.AnalogGyro(0) @@ -51,7 +63,6 @@ def __init__(self): # We need to invert one side of the drivetrain so that positive voltages # result in both sides moving forward. Depending on how your robot's # gearbox is constructed, you might have to invert the left side instead. - self.rightGroup.setInverted(True) # Set the distance per pulse for the drive encoders. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder @@ -84,8 +95,9 @@ def setSpeeds(self, speeds: wpimath.kinematics.DifferentialDriveWheelSpeeds): self.rightEncoder.getRate(), speeds.right ) - self.leftGroup.setVoltage(leftOutput + leftFeedforward) - self.rightGroup.setVoltage(rightOutput + rightFeedforward) + # Controls the left and right sides of the robot using the calculated outputs + self.robotDrive.tankDrive(leftOutput + leftFeedforward, rightOutput + rightFeedforward) + def drive(self, xSpeed, rot): """Drives the robot with the given linear velocity and angular velocity.""" From beea8b9d55ba7b0f972b8aeaa6ac74523f487f01 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 13:59:01 -0700 Subject: [PATCH 02/15] Revert tankdrive usage --- DifferentialDriveBot/drivetrain.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/DifferentialDriveBot/drivetrain.py b/DifferentialDriveBot/drivetrain.py index 8d86401c..2988619f 100755 --- a/DifferentialDriveBot/drivetrain.py +++ b/DifferentialDriveBot/drivetrain.py @@ -38,9 +38,6 @@ def __init__(self): # gearbox is constructed, you might have to invert the left side instead. self.rightLeader.setInverted(True) - # turns the motors into a group so that we can set voltage to both at once - self.robotDrive = wpilib.drive.DifferentialDrive(self.leftLeader, self.rightLeader) - self.leftEncoder = wpilib.Encoder(0, 1) self.rightEncoder = wpilib.Encoder(2, 3) @@ -96,8 +93,9 @@ def setSpeeds(self, speeds: wpimath.kinematics.DifferentialDriveWheelSpeeds): ) # Controls the left and right sides of the robot using the calculated outputs - self.robotDrive.tankDrive(leftOutput + leftFeedforward, rightOutput + rightFeedforward) - + self.leftLeader.setVoltage(leftOutput + leftFeedforward) + self.rightLeader.setVoltage(rightOutput + rightFeedforward) + def drive(self, xSpeed, rot): """Drives the robot with the given linear velocity and angular velocity.""" From ca31989853ec449b0fbd037690be8879e129daab Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:01:53 -0700 Subject: [PATCH 03/15] update hatchbot drive --- HatchbotTraditional/subsystems/drivesubsystem.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/HatchbotTraditional/subsystems/drivesubsystem.py b/HatchbotTraditional/subsystems/drivesubsystem.py index 1ec25b25..e28c4a82 100644 --- a/HatchbotTraditional/subsystems/drivesubsystem.py +++ b/HatchbotTraditional/subsystems/drivesubsystem.py @@ -20,10 +20,13 @@ def __init__(self) -> None: self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) + self.left1.addFollower(self.left2) + self.right1.addFollower(self.right2) + # The robot's drive self.drive = wpilib.drive.DifferentialDrive( - wpilib.MotorControllerGroup(self.left1, self.left2), - wpilib.MotorControllerGroup(self.right1, self.right2), + self.left1, + self.right1, ) # The left-side drive encoder From eda9954f6d70f1091bca7401c2893e21564cf2d1 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:09:18 -0700 Subject: [PATCH 04/15] Both hatchbot examples updated and added invert --- HatchbotInlined/subsystems/drivesubsystem.py | 12 ++++++++++-- HatchbotTraditional/subsystems/drivesubsystem.py | 8 +++++--- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/HatchbotInlined/subsystems/drivesubsystem.py b/HatchbotInlined/subsystems/drivesubsystem.py index 1ec25b25..c4f769cd 100644 --- a/HatchbotInlined/subsystems/drivesubsystem.py +++ b/HatchbotInlined/subsystems/drivesubsystem.py @@ -20,10 +20,18 @@ def __init__(self) -> None: self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) + self.left1.addFollower(self.left2) + self.right1.addFollower(self.right2) + + # We need to invert one side of the drivetrain so that positive speeds + # result in both sides moving forward. Depending on how your robot's + # drivetrain is constructed, you might have to invert the left side instead. + self.right1.setInverted(True) + # The robot's drive self.drive = wpilib.drive.DifferentialDrive( - wpilib.MotorControllerGroup(self.left1, self.left2), - wpilib.MotorControllerGroup(self.right1, self.right2), + self.left1, + self.right1 ) # The left-side drive encoder diff --git a/HatchbotTraditional/subsystems/drivesubsystem.py b/HatchbotTraditional/subsystems/drivesubsystem.py index e28c4a82..54167bf4 100644 --- a/HatchbotTraditional/subsystems/drivesubsystem.py +++ b/HatchbotTraditional/subsystems/drivesubsystem.py @@ -20,13 +20,15 @@ def __init__(self) -> None: self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) - self.left1.addFollower(self.left2) - self.right1.addFollower(self.right2) + # We need to invert one side of the drivetrain so that positive speeds + # result in both sides moving forward. Depending on how your robot's + # drivetrain is constructed, you might have to invert the left side instead. + self.right1.setInverted(True) # The robot's drive self.drive = wpilib.drive.DifferentialDrive( self.left1, - self.right1, + self.right1 ) # The left-side drive encoder From aeba4982ee9b6e404fa8036a606fb5c6ba907f98 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:23:28 -0700 Subject: [PATCH 05/15] update formating for drive in hatchbot examples --- HatchbotInlined/subsystems/drivesubsystem.py | 5 +---- HatchbotTraditional/subsystems/drivesubsystem.py | 5 +---- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/HatchbotInlined/subsystems/drivesubsystem.py b/HatchbotInlined/subsystems/drivesubsystem.py index c4f769cd..ef93671e 100644 --- a/HatchbotInlined/subsystems/drivesubsystem.py +++ b/HatchbotInlined/subsystems/drivesubsystem.py @@ -29,10 +29,7 @@ def __init__(self) -> None: self.right1.setInverted(True) # The robot's drive - self.drive = wpilib.drive.DifferentialDrive( - self.left1, - self.right1 - ) + self.drive = wpilib.drive.DifferentialDrive(self.left1, self.right1) # The left-side drive encoder self.leftEncoder = wpilib.Encoder( diff --git a/HatchbotTraditional/subsystems/drivesubsystem.py b/HatchbotTraditional/subsystems/drivesubsystem.py index 54167bf4..f25b03be 100644 --- a/HatchbotTraditional/subsystems/drivesubsystem.py +++ b/HatchbotTraditional/subsystems/drivesubsystem.py @@ -26,10 +26,7 @@ def __init__(self) -> None: self.right1.setInverted(True) # The robot's drive - self.drive = wpilib.drive.DifferentialDrive( - self.left1, - self.right1 - ) + self.drive = wpilib.drive.DifferentialDrive(self.left1, self.right1) # The left-side drive encoder self.leftEncoder = wpilib.Encoder( From d4bd21cb1f39fd3de5e4ce1f25c34d0d9e272981 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:30:37 -0700 Subject: [PATCH 06/15] remove whitespace --- DifferentialDriveBot/drivetrain.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/DifferentialDriveBot/drivetrain.py b/DifferentialDriveBot/drivetrain.py index 2988619f..13f48c9c 100755 --- a/DifferentialDriveBot/drivetrain.py +++ b/DifferentialDriveBot/drivetrain.py @@ -28,7 +28,6 @@ def __init__(self): self.rightLeader = wpilib.PWMSparkMax(3) self.rightFollower = wpilib.PWMSparkMax(4) - # Make sure both motors for each side are in the same group self.leftLeader.addFollower(self.leftFollower) self.rightLeader.addFollower(self.rightFollower) @@ -38,11 +37,9 @@ def __init__(self): # gearbox is constructed, you might have to invert the left side instead. self.rightLeader.setInverted(True) - self.leftEncoder = wpilib.Encoder(0, 1) self.rightEncoder = wpilib.Encoder(2, 3) - self.gyro = wpilib.AnalogGyro(0) self.leftPIDController = wpimath.controller.PIDController(1.0, 0.0, 0.0) From a4f51076da261912e8fc25e4058f1c4afe8536c9 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:33:40 -0700 Subject: [PATCH 07/15] removed redundant comment and whitespace missed --- DifferentialDriveBot/drivetrain.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/DifferentialDriveBot/drivetrain.py b/DifferentialDriveBot/drivetrain.py index 13f48c9c..f9018799 100755 --- a/DifferentialDriveBot/drivetrain.py +++ b/DifferentialDriveBot/drivetrain.py @@ -54,10 +54,6 @@ def __init__(self): self.gyro.reset() - # We need to invert one side of the drivetrain so that positive voltages - # result in both sides moving forward. Depending on how your robot's - # gearbox is constructed, you might have to invert the left side instead. - # Set the distance per pulse for the drive encoders. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder # resolution. @@ -93,7 +89,6 @@ def setSpeeds(self, speeds: wpimath.kinematics.DifferentialDriveWheelSpeeds): self.leftLeader.setVoltage(leftOutput + leftFeedforward) self.rightLeader.setVoltage(rightOutput + rightFeedforward) - def drive(self, xSpeed, rot): """Drives the robot with the given linear velocity and angular velocity.""" wheelSpeeds = self.kinematics.toWheelSpeeds( From 2f4e96b1ecb403438cc9dff7da43efbf256c5656 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:40:24 -0700 Subject: [PATCH 08/15] Update both ramsete drive examples --- RamseteCommand/subsystems/driveSubsystem.py | 33 +++++++++++---------- RamseteController/drivetrain.py | 18 +++++------ 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/RamseteCommand/subsystems/driveSubsystem.py b/RamseteCommand/subsystems/driveSubsystem.py index 99d75deb..9449c302 100644 --- a/RamseteCommand/subsystems/driveSubsystem.py +++ b/RamseteCommand/subsystems/driveSubsystem.py @@ -6,7 +6,7 @@ from commands2 import Subsystem -from wpilib import MotorControllerGroup, PWMSparkMax, Encoder, ADXRS450_Gyro +from wpilib import PWMSparkMax, Encoder, ADXRS450_Gyro from wpilib.drive import DifferentialDrive from wpimath.kinematics import DifferentialDriveOdometry, DifferentialDriveWheelSpeeds @@ -19,19 +19,23 @@ def __init__(self): super().__init__() # The motors on the left side of the drive. - self.leftMotors = MotorControllerGroup( - PWMSparkMax(constants.kLeftMotor1Port), - PWMSparkMax(constants.kLeftMotor2Port), - ) + self.left1 = PWMSparkMax(constants.kLeftMotor1Port) + self.left2 = PWMSparkMax(constants.kLeftMotor2Port) # The motors on the right side of the drive. - self.rightMotors = MotorControllerGroup( - PWMSparkMax(constants.kRightMotor1Port), - PWMSparkMax(constants.kRightMotor2Port), - ) + self.right1 = PWMSparkMax(constants.kRightMotor1Port) + self.right2 = PWMSparkMax(constants.kRightMotor2Port) + + self.left1.addFollower(self.left2) + self.right1.addFollower(self.right2) # The robot's drive - self.drive = DifferentialDrive(self.leftMotors, self.rightMotors) + self.drive = DifferentialDrive(self.left1, self.right1) + + # We need to invert one side of the drivetrain so that positive voltages + # result in both sides moving forward. Depending on how your robot's + # gearbox is constructed, you might have to invert the left side instead. + self.right1.setInverted(True) # The left-side drive encoder self.leftEncoder = Encoder( @@ -50,10 +54,7 @@ def __init__(self): # The gyro sensor self.gyro = ADXRS450_Gyro() - # We need to invert one side of the drivetrain so that positive voltages - # result in both sides moving forward. Depending on how your robot's - # gearbox is constructed, you might have to invert the left side instead. - self.rightMotors.setInverted(True) + # Sets the distance per pulse for the encoders self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) @@ -101,8 +102,8 @@ def arcadeDrive(self, fwd, rot): def tankDriveVolts(self, leftVolts, rightVolts): """Controls the left and right sides of the drive directly with voltages.""" - self.leftMotors.setVoltage(leftVolts) - self.rightMotors.setVoltage(rightVolts) + self.left1.setVoltage(leftVolts) + self.right1.setVoltage(rightVolts) self.drive.feed() def resetEncoders(self): diff --git a/RamseteController/drivetrain.py b/RamseteController/drivetrain.py index 7f5e2553..58111621 100644 --- a/RamseteController/drivetrain.py +++ b/RamseteController/drivetrain.py @@ -24,16 +24,16 @@ class Drivetrain: kEncoderResolution = 4096 # counts per revolution def __init__(self): - leftLeader = wpilib.PWMSparkMax(1) - leftFollower = wpilib.PWMSparkMax(2) - rightLeader = wpilib.PWMSparkMax(3) - rightFollower = wpilib.PWMSparkMax(4) + self.leftLeader = wpilib.PWMSparkMax(1) + self.leftFollower = wpilib.PWMSparkMax(2) + self.rightLeader = wpilib.PWMSparkMax(3) + self.rightFollower = wpilib.PWMSparkMax(4) self.leftEncoder = wpilib.Encoder(0, 1) self.rightEncoder = wpilib.Encoder(2, 3) - self.leftGroup = wpilib.MotorControllerGroup(leftLeader, leftFollower) - self.rightGroup = wpilib.MotorControllerGroup(rightLeader, rightFollower) + self.leftLeader.addFollower(self.leftFollower) + self.rightLeader.addFollower(self.rightFollower) self.gyro = wpilib.AnalogGyro(0) @@ -52,7 +52,7 @@ def __init__(self): # We need to invert one side of the drivetrain so that positive voltages # result in both sides moving forward. Depending on how your robot's # gearbox is constructed, you might have to invert the left side instead. - self.rightGroup.setInverted(True) + self.rightLeader.setInverted(True) # Set the distance per pulse for the drive encoders. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder @@ -85,8 +85,8 @@ def setSpeeds(self, speeds: wpimath.kinematics.DifferentialDriveWheelSpeeds): self.rightEncoder.getRate(), speeds.right ) - self.leftGroup.setVoltage(leftOutput + leftFeedforward) - self.rightGroup.setVoltage(rightOutput + rightFeedforward) + self.leftLeader.setVoltage(leftOutput + leftFeedforward) + self.rightLeader.setVoltage(rightOutput + rightFeedforward) def drive(self, xSpeed, rot): """Drives the robot with the given linear velocity and angular velocity.""" From ada7b17e73e557a7340aeae68c3efcbb40f16b17 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:42:20 -0700 Subject: [PATCH 09/15] remove whitespace --- RamseteCommand/subsystems/driveSubsystem.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/RamseteCommand/subsystems/driveSubsystem.py b/RamseteCommand/subsystems/driveSubsystem.py index 9449c302..f81486a9 100644 --- a/RamseteCommand/subsystems/driveSubsystem.py +++ b/RamseteCommand/subsystems/driveSubsystem.py @@ -54,8 +54,6 @@ def __init__(self): # The gyro sensor self.gyro = ADXRS450_Gyro() - - # Sets the distance per pulse for the encoders self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) From 304d74058486b26f110f1135a2edfe9f7c43319c Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:50:06 -0700 Subject: [PATCH 10/15] update armbot --- ArmBot/subsystems/drivesubsystem.py | 35 ++++++++++++++--------------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/ArmBot/subsystems/drivesubsystem.py b/ArmBot/subsystems/drivesubsystem.py index e704dc62..e1bb9b56 100644 --- a/ArmBot/subsystems/drivesubsystem.py +++ b/ArmBot/subsystems/drivesubsystem.py @@ -4,8 +4,8 @@ # the WPILib BSD license file in the root directory of this project. # -import wpilib -import wpilib.drive +from wpilib import PWMSparkMax, Encoder +from wpilib.drive import DifferentialDrive import commands2 import constants @@ -15,31 +15,30 @@ class DriveSubsystem(commands2.Subsystem): # Creates a new DriveSubsystem def __init__(self) -> None: super().__init__() + + # The motors on the left side of the drive. + self.left1 = PWMSparkMax(constants.DriveConstants.kLeftMotor1Port) + self.left2 = PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) - # The motors on the left side of the drive - self.left_motors = wpilib.MotorControllerGroup( - wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor1Port), - wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port), - ) + # The motors on the right side of the drive. + self.right1 = PWMSparkMax(constants.DriveConstants.kRightMotor1Port) + self.right2 = PWMSparkMax(constants.DriveConstants.kRightMotor2Port) - # The motors on the right side of the drive - self.right_motors = wpilib.MotorControllerGroup( - wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port), - wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port), - ) + self.left1.addFollower(self.left2) + self.right1.addFollower(self.right2) # The robot's drive - self.drive = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) + self.drive = DifferentialDrive(self.left1, self.right1) # The left-side drive encoder - self.left_encoder = wpilib.Encoder( + self.left_encoder = Encoder( constants.DriveConstants.kLeftEncoderPorts[0], constants.DriveConstants.kLeftEncoderPorts[1], constants.DriveConstants.kLeftEncoderReversed, ) # The right-side drive encoder - self.right_encoder = wpilib.Encoder( + self.right_encoder = Encoder( constants.DriveConstants.kRightEncoderPorts[0], constants.DriveConstants.kRightEncoderPorts[1], constants.DriveConstants.kRightEncoderReversed, @@ -56,7 +55,7 @@ def __init__(self) -> None: # We need to invert one side of the drivetrain so that positive voltages # result in both sides moving forward. Depending on how your robot's # gearbox is constructed, you might have to invert the left side instead. - self.right_motors.setInverted(True) + self.right1.setInverted(True) def arcadeDrive(self, fwd: float, rot: float) -> None: """Drives the robot using arcade controls. @@ -80,14 +79,14 @@ def getAverageEncoderDistance(self) -> float: self.left_encoder.getDistance() + self.right_encoder.getDistance() ) / 2.0 - def getLeftEncoder(self) -> wpilib.Encoder: + def getLeftEncoder(self) -> Encoder: """Gets the left drive encoder. :returns: the left drive encoder """ return self.left_encoder - def getRightEncoder(self) -> wpilib.Encoder: + def getRightEncoder(self) -> Encoder: """Gets the right drive encoder. :returns: the right drive encoder From 18f43f269fa24418935f3687d641a7d206e13495 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:54:26 -0700 Subject: [PATCH 11/15] update offboardArmBot --- ArmBotOffboard/subsystems/drivesubsystem.py | 41 +++++++++++---------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/ArmBotOffboard/subsystems/drivesubsystem.py b/ArmBotOffboard/subsystems/drivesubsystem.py index 92c9eff0..d14ba103 100644 --- a/ArmBotOffboard/subsystems/drivesubsystem.py +++ b/ArmBotOffboard/subsystems/drivesubsystem.py @@ -4,42 +4,43 @@ # the WPILib BSD license file in the root directory of this project. # +import typing + import commands2 import commands2.cmd +from wpilib import PWMSparkMax, Encoder +from wpilib.drive import DifferentialDrive + import constants -import wpilib -import wpilib.drive -import typing class DriveSubsystem(commands2.Subsystem): def __init__(self) -> None: super().__init__() - # The motors on the left side of the drive. - self.left = wpilib.MotorControllerGroup( - wpilib.PWMSparkMax(constants.kLeftMotor1Port), - wpilib.PWMSparkMax(constants.kLeftMotor2Port), - ) + # The motors on the left side of the drive. + self.left1 = PWMSparkMax(constants.kLeftMotor1Port) + self.left2 = PWMSparkMax(constants.kLeftMotor2Port) # The motors on the right side of the drive. - self.right = wpilib.MotorControllerGroup( - wpilib.PWMSparkMax(constants.kRightMotor1Port), - wpilib.PWMSparkMax(constants.kRightMotor2Port), - ) + self.right1 = PWMSparkMax(constants.kRightMotor1Port) + self.right2 = PWMSparkMax(constants.kRightMotor2Port) + + self.left1.addFollower(self.left2) + self.right1.addFollower(self.right2) # The robot's drive - self.drive = wpilib.drive.DifferentialDrive(self.left, self.right) + self.drive = DifferentialDrive(self.left1, self.right1) # The left-side drive encoder - self.left_encoder = wpilib.Encoder( + self.left_encoder = Encoder( constants.kLeftEncoderPorts[0], constants.kLeftEncoderPorts[1], constants.kLeftEncoderReversed, ) # The right-side drive encoder - self.right_encoder = wpilib.Encoder( + self.right_encoder = Encoder( constants.kRightEncoderPorts[0], constants.kRightEncoderPorts[1], constants.kRightEncoderReversed, @@ -52,7 +53,7 @@ def __init__(self) -> None: # We need to invert one side of the drivetrain so that positive voltages # result in both sides moving forward. Depending on how your robot's # gearbox is constructed, you might have to invert the left side instead. - self.right.setInverted(True) + self.right1.setInverted(True) def arcadeDriveCommand( self, fwd: typing.Callable[[], float], rot: typing.Callable[[], float] @@ -71,8 +72,8 @@ def arcadeDriveCommand( def resetEncoders(self) -> None: """Resets the drive encoders to currently read a position of 0.""" - self.left.reset() - self.right.reset() + self.left_encoder.reset() + self.right_encoder.reset() def getAverageEncoderDistance(self) -> float: """Gets the average distance of the two encoders. @@ -82,7 +83,7 @@ def getAverageEncoderDistance(self) -> float: """ return (self.left_encoder.getDistance() + self.right_encoder.getDistance()) / 2 - def getLeftEncoder(self) -> wpilib.Encoder: + def getLeftEncoder(self) -> Encoder: """Gets the left drive encoder. Returns: @@ -90,7 +91,7 @@ def getLeftEncoder(self) -> wpilib.Encoder: """ return self.left_encoder - def getRightEncoder(self) -> wpilib.Encoder: + def getRightEncoder(self) -> Encoder: """Gets the right drive encoder. Returns: From 90c4297aebcd189a864818eba2e5aaa942d15c94 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:57:36 -0700 Subject: [PATCH 12/15] update frisbeebot --- FrisbeeBot/subsystems/drivesubsystem.py | 33 ++++++++++++------------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/FrisbeeBot/subsystems/drivesubsystem.py b/FrisbeeBot/subsystems/drivesubsystem.py index dfacd12e..e367ac8e 100644 --- a/FrisbeeBot/subsystems/drivesubsystem.py +++ b/FrisbeeBot/subsystems/drivesubsystem.py @@ -4,8 +4,8 @@ # the WPILib BSD license file in the root directory of this project. # -import wpilib -import wpilib.drive +from wpilib import PWMSparkMax, Encoder +from wpilib.drive import DifferentialDrive import commands2 import constants @@ -16,30 +16,29 @@ def __init__(self) -> None: """Creates a new DriveSubsystem""" super().__init__() - # The motors on the left side of the drive. - self.leftMotors = wpilib.MotorControllerGroup( - wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor1Port), - wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port), - ) + # The motors on the left side of the drive. + self.left1 = PWMSparkMax(constants.DriveConstants.kLeftMotor1Port) + self.left2 = PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) # The motors on the right side of the drive. - self.rightMotors = wpilib.MotorControllerGroup( - wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port), - wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port), - ) + self.right1 = PWMSparkMax(constants.DriveConstants.kRightMotor1Port) + self.right2 = PWMSparkMax(constants.DriveConstants.kRightMotor2Port) + + self.left1.addFollower(self.left2) + self.right1.addFollower(self.right2) # The robot's drive - self.drive = wpilib.drive.DifferentialDrive(self.leftMotors, self.rightMotors) + self.drive = DifferentialDrive(self.left1, self.right1) # The left-side drive encoder - self.leftEncoder = wpilib.Encoder( + self.leftEncoder = Encoder( constants.DriveConstants.kLeftEncoderPorts[0], constants.DriveConstants.kLeftEncoderPorts[1], constants.DriveConstants.kLeftEncoderReversed, ) # The right-side drive encoder - self.rightEncoder = wpilib.Encoder( + self.rightEncoder = Encoder( constants.DriveConstants.kRightEncoderPorts[0], constants.DriveConstants.kRightEncoderPorts[1], constants.DriveConstants.kRightEncoderReversed, @@ -48,7 +47,7 @@ def __init__(self) -> None: # We need to invert one side of the drivetrain so that positive voltages # result in both sides moving forward. Depending on how your robot's # gearbox is constructed, you might have to invert the left side instead. - self.rightMotors.setInverted(True) + self.right1.setInverted(True) # Sets the distance per pulse for the encoders self.leftEncoder.setDistancePerPulse( @@ -80,7 +79,7 @@ def getAverageEncoderDistance(self): """ return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 - def getLeftEncoder(self) -> wpilib.Encoder: + def getLeftEncoder(self) -> Encoder: """ Gets the left drive encoder. @@ -88,7 +87,7 @@ def getLeftEncoder(self) -> wpilib.Encoder: """ return self.leftEncoder - def getRightEncoder(self) -> wpilib.Encoder: + def getRightEncoder(self) -> Encoder: """ Gets the right drive encoder. From aaf00154d400e7049517ca36af1562499af5e711 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 14:59:34 -0700 Subject: [PATCH 13/15] update GyroDriveCommands --- .../subsystems/drivesubsystem.py | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/GyroDriveCommands/subsystems/drivesubsystem.py b/GyroDriveCommands/subsystems/drivesubsystem.py index da649dd1..85a7a0c5 100644 --- a/GyroDriveCommands/subsystems/drivesubsystem.py +++ b/GyroDriveCommands/subsystems/drivesubsystem.py @@ -4,8 +4,8 @@ # the WPILib BSD license file in the root directory of this project. # -import wpilib -import wpilib.drive +from wpilib import PWMSparkMax, Encoder, ADXRS450_Gyro +from wpilib.drive import DifferentialDrive import commands2 import math @@ -17,30 +17,30 @@ def __init__(self) -> None: """Creates a new DriveSubsystem""" super().__init__() - # The motors on the left side of the drive. - self.leftMotors = wpilib.MotorControllerGroup( - wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor1Port), - wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port), - ) + + # The motors on the left side of the drive. + self.left1 = PWMSparkMax(constants.DriveConstants.kLeftMotor1Port) + self.left2 = PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) # The motors on the right side of the drive. - self.rightMotors = wpilib.MotorControllerGroup( - wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port), - wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port), - ) + self.right1 = PWMSparkMax(constants.DriveConstants.kRightMotor1Port) + self.right2 = PWMSparkMax(constants.DriveConstants.kRightMotor2Port) + + self.left1.addFollower(self.left2) + self.right1.addFollower(self.right2) # The robot's drive - self.drive = wpilib.drive.DifferentialDrive(self.leftMotors, self.rightMotors) + self.drive = DifferentialDrive(self.left1, self.right1) # The left-side drive encoder - self.leftEncoder = wpilib.Encoder( + self.leftEncoder = Encoder( constants.DriveConstants.kLeftEncoderPorts[0], constants.DriveConstants.kLeftEncoderPorts[1], constants.DriveConstants.kLeftEncoderReversed, ) # The right-side drive encoder - self.rightEncoder = wpilib.Encoder( + self.rightEncoder = Encoder( constants.DriveConstants.kRightEncoderPorts[0], constants.DriveConstants.kRightEncoderPorts[1], constants.DriveConstants.kRightEncoderReversed, @@ -49,7 +49,7 @@ def __init__(self) -> None: # We need to invert one side of the drivetrain so that positive voltages # result in both sides moving forward. Depending on how your robot's # gearbox is constructed, you might have to invert the left side instead. - self.rightMotors.setInverted(True) + self.right1.setInverted(True) # Sets the distance per pulse for the encoders self.leftEncoder.setDistancePerPulse( @@ -59,7 +59,7 @@ def __init__(self) -> None: constants.DriveConstants.kEncoderDistancePerPulse ) - self.gyro = wpilib.ADXRS450_Gyro() + self.gyro = ADXRS450_Gyro() def arcadeDrive(self, fwd: float, rot: float): """ @@ -83,7 +83,7 @@ def getAverageEncoderDistance(self): """ return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 - def getLeftEncoder(self) -> wpilib.Encoder: + def getLeftEncoder(self) -> Encoder: """ Gets the left drive encoder. @@ -91,7 +91,7 @@ def getLeftEncoder(self) -> wpilib.Encoder: """ return self.leftEncoder - def getRightEncoder(self) -> wpilib.Encoder: + def getRightEncoder(self) -> Encoder: """ Gets the right drive encoder. From f3a69f027dd7f678f38476c3d2fe394b06980544 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 15:03:36 -0700 Subject: [PATCH 14/15] Add singular space --- ArmBotOffboard/subsystems/drivesubsystem.py | 2 +- FrisbeeBot/subsystems/drivesubsystem.py | 2 +- GyroDriveCommands/subsystems/drivesubsystem.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArmBotOffboard/subsystems/drivesubsystem.py b/ArmBotOffboard/subsystems/drivesubsystem.py index d14ba103..4dcfef62 100644 --- a/ArmBotOffboard/subsystems/drivesubsystem.py +++ b/ArmBotOffboard/subsystems/drivesubsystem.py @@ -18,7 +18,7 @@ class DriveSubsystem(commands2.Subsystem): def __init__(self) -> None: super().__init__() - # The motors on the left side of the drive. + # The motors on the left side of the drive. self.left1 = PWMSparkMax(constants.kLeftMotor1Port) self.left2 = PWMSparkMax(constants.kLeftMotor2Port) diff --git a/FrisbeeBot/subsystems/drivesubsystem.py b/FrisbeeBot/subsystems/drivesubsystem.py index e367ac8e..7b37de29 100644 --- a/FrisbeeBot/subsystems/drivesubsystem.py +++ b/FrisbeeBot/subsystems/drivesubsystem.py @@ -16,7 +16,7 @@ def __init__(self) -> None: """Creates a new DriveSubsystem""" super().__init__() - # The motors on the left side of the drive. + # The motors on the left side of the drive. self.left1 = PWMSparkMax(constants.DriveConstants.kLeftMotor1Port) self.left2 = PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) diff --git a/GyroDriveCommands/subsystems/drivesubsystem.py b/GyroDriveCommands/subsystems/drivesubsystem.py index 85a7a0c5..ea87acd6 100644 --- a/GyroDriveCommands/subsystems/drivesubsystem.py +++ b/GyroDriveCommands/subsystems/drivesubsystem.py @@ -18,7 +18,7 @@ def __init__(self) -> None: super().__init__() - # The motors on the left side of the drive. + # The motors on the left side of the drive. self.left1 = PWMSparkMax(constants.DriveConstants.kLeftMotor1Port) self.left2 = PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) From 3c82906b267854c11cbb04ae2f62c2cb4048c672 Mon Sep 17 00:00:00 2001 From: Ramon Ledesma Date: Wed, 23 Apr 2025 15:05:44 -0700 Subject: [PATCH 15/15] remove whitespace --- ArmBot/subsystems/drivesubsystem.py | 2 +- GyroDriveCommands/subsystems/drivesubsystem.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/ArmBot/subsystems/drivesubsystem.py b/ArmBot/subsystems/drivesubsystem.py index e1bb9b56..201943db 100644 --- a/ArmBot/subsystems/drivesubsystem.py +++ b/ArmBot/subsystems/drivesubsystem.py @@ -15,7 +15,7 @@ class DriveSubsystem(commands2.Subsystem): # Creates a new DriveSubsystem def __init__(self) -> None: super().__init__() - + # The motors on the left side of the drive. self.left1 = PWMSparkMax(constants.DriveConstants.kLeftMotor1Port) self.left2 = PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) diff --git a/GyroDriveCommands/subsystems/drivesubsystem.py b/GyroDriveCommands/subsystems/drivesubsystem.py index ea87acd6..b63efe47 100644 --- a/GyroDriveCommands/subsystems/drivesubsystem.py +++ b/GyroDriveCommands/subsystems/drivesubsystem.py @@ -17,7 +17,6 @@ def __init__(self) -> None: """Creates a new DriveSubsystem""" super().__init__() - # The motors on the left side of the drive. self.left1 = PWMSparkMax(constants.DriveConstants.kLeftMotor1Port) self.left2 = PWMSparkMax(constants.DriveConstants.kLeftMotor2Port)