diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index adaa23f..394a360 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -2,6 +2,7 @@ name: dist on: + workflow_dispatch: pull_request: push: branches: diff --git a/SwerveControllerCommand/constants.py b/SwerveControllerCommand/constants.py new file mode 100644 index 0000000..3ea4be6 --- /dev/null +++ b/SwerveControllerCommand/constants.py @@ -0,0 +1,95 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +from wpilib import TimedRobot +from wpimath.geometry import Translation2d +from wpimath.kinematics import SwerveDrive4Kinematics +from wpimath.trajectory import TrapezoidProfile +import math + + +class DriveConstants: + kFrontLeftDriveMotorPort = 0 + kRearLeftDriveMotorPort = 2 + kFrontRightDriveMotorPort = 4 + kRearRightDriveMotorPort = 6 + + kFrontLeftTurningMotorPort = 1 + kRearLeftTurningMotorPort = 3 + kFrontRightTurningMotorPort = 5 + kRearRightTurningMotorPort = 7 + + kFrontLeftTurningEncoderPorts = [0, 1] + kRearLeftTurningEncoderPorts = [2, 3] + kFrontRightTurningEncoderPorts = [4, 5] + kRearRightTurningEncoderPorts = [6, 7] + + kFrontLeftTurningEncoderReversed = False + kRearLeftTurningEncoderReversed = True + kFrontRightTurningEncoderReversed = False + kRearRightTurningEncoderReversed = True + + kFrontLeftDriveEncoderPorts = [8, 9] + kRearLeftDriveEncoderPorts = [10, 11] + kFrontRightDriveEncoderPorts = [12, 13] + kRearRightDriveEncoderPorts = [14, 15] + + kFrontLeftDriveEncoderReversed = False + kRearLeftDriveEncoderReversed = True + kFrontRightDriveEncoderReversed = False + kRearRightDriveEncoderReversed = True + + kDrivePeriod = TimedRobot.kDefaultPeriod + + kTrackWidth = 0.5 + kWheelBase = 0.7 + kDriveKinematics = SwerveDrive4Kinematics( + Translation2d(kWheelBase / 2, kTrackWidth / 2), + Translation2d(kWheelBase / 2, -kTrackWidth / 2), + Translation2d(-kWheelBase / 2, kTrackWidth / 2), + Translation2d(-kWheelBase / 2, -kTrackWidth / 2), + ) + + kGyroReversed = False + + ksVolts = 1 + kvVoltSecondsPerMeter = 0.8 + kaVoltSecondsSquaredPerMeter = 0.15 + + kMaxSpeedMetersPerSecond = 3 + + +class ModuleConstants: + kMaxModuleAngularSpeedRadiansPerSecond = 2 * math.pi + kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * math.pi + + kEncoderCPR = 1024 + kWheelDiameterMeters = 0.15 + kDriveEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR + + kTurningEncoderDistancePerPulse = (2 * math.pi) / kEncoderCPR + + kPModuleTurningController = 1 + kPModuleDriveController = 1 + + +class OIConstants: + kDriverControllerPort = 0 + + +class AutoConstants: + kMaxSpeedMetersPerSecond = 3 + kMaxAccelerationMetersPerSecondSquared = 3 + kMaxAngularSpeedRadiansPerSecond = math.pi + kMaxAngularSpeedRadiansPerSecondSquared = math.pi + + kPXController = 1 + kPYController = 1 + kPThetaController = 1 + + kThetaControllerConstraints = TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared + ) diff --git a/SwerveControllerCommand/robot.py b/SwerveControllerCommand/robot.py new file mode 100644 index 0000000..3edb4e6 --- /dev/null +++ b/SwerveControllerCommand/robot.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +from commands2 import TimedCommandRobot, CommandScheduler +from robotcontainer import RobotContainer + + +class Robot(TimedCommandRobot): + def robotInit(self): + """ + Instantiate our RobotContainer. This will perform all our button bindings, and put our + autonomous chooser on the dashboard. + """ + self.robotContainer = RobotContainer() + + def robotPeriodic(self): + """ + Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + commands, running already-scheduled commands, removing finished or interrupted commands, + and running subsystem periodic() methods. This must be called from the robot's periodic + block in order for anything in the Command-based framework to work. + """ + + def disabledInit(self): + pass + + def disabledPeriodic(self): + pass + + def autonomousInit(self): + self.autonomousCommand = self.robotContainer.getAutonomousCommand() + + """ + schedule the autonomous command (example) + """ + if self.autonomousCommand is not None: + self.autonomousCommand.schedule() + + def autonomousPeriodic(self): + pass + + def teleopInit(self): + """ + This makes sure that the autonomous stops running when + teleop starts running. If you want the autonomous to + continue until interrupted by another command, remove + this line or comment it out. + """ + if self.autonomousCommand is not None: + self.autonomousCommand.cancel() + + def teleopPeriodic(self): + pass + + def testInit(self): + """ + Cancels all running commands at the start of test mode. + """ + CommandScheduler.getInstance().cancelAll() + + def testPeriodic(self): + pass diff --git a/SwerveControllerCommand/robotcontainer.py b/SwerveControllerCommand/robotcontainer.py new file mode 100644 index 0000000..450b103 --- /dev/null +++ b/SwerveControllerCommand/robotcontainer.py @@ -0,0 +1,116 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +from wpilib import XboxController +import commands2 +import constants +from subsystems.drivesubsystems import DriveSubsystem +from wpimath.trajectory import Trajectory, TrajectoryConfig, TrajectoryGenerator +from wpimath.geometry import Translation2d, Pose2d, Rotation2d +from wpimath.trajectory.constraint import DifferentialDriveVoltageConstraint +from wpimath.controller import ProfiledPIDController, PIDController, HolonomicDriveController +import math + + +class RobotContainer: + def __init__(self): + """ + The container for the robot. Contains subsystems, OI devices, and commands. + """ + self.robotDrive = DriveSubsystem() + + # Driver Controller + self.driverController = XboxController( + constants.OIConstants.kDriverControllerPort + ) + + """ + Configure default commands + """ + self.robotDrive.setDefaultCommand( + # The left stick controls translation of the robot. + # turning is controlled by the X axis of the right stick. + commands2.RunCommand( + lambda: self.robotDrive.drive( + # Multiply by max speed to map the joystick unitless inputs to actual units. + # This will map the [-1, 1] to [max speed backwards, max speed forwards], + # converting them to actual units. + self.driverController.getYChannel() + * constants.DriveConstants.kMaxSpeedMetersPerSecond, + self.driverController.getXChannel() + * constants.DriveConstants.kMaxSpeedMetersPerSecond, + self.driverController.getX(constants.Hand.kRight) + * constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond, + False, + ), + self.robotDrive, + ) + ) + + def configureButtonBindings(self): + pass + + def getAutonomousCommand(self): + """ + Create config for trajectory + """ + + + config = TrajectoryConfig( + constants.AutoConstants.kMaxSpeedMetersPerSecond, + constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared, + ) + config.setKinematics(constants.DriveConstants.kDriveKinematics) + + # An example trajectory to follow. All units in meters. + + example_trajectory = TrajectoryGenerator.generateTrajectory( + # Start at the origin facing the +X direction + Pose2d(0, 0, Rotation2d(0)), + # Pass through these two interior waypoints, making an 's' curve path + [Translation2d(1, 1), Translation2d(2, -1)], + # End 3 meters straight ahead of where we started, facing forward + Pose2d(3, 0, Rotation2d(0)), + config + ) + + theta_controller = ProfiledPIDController( + constants.AutoConstants.kPThetaController, + 0, + 0, + constants.AutoConstants.kThetaControllerConstraints, + ) + theta_controller.enableContinuousInput(-math.pi, math.pi) + + swerve_controller_command = commands2.Swerve4ControllerCommand( + example_trajectory, + self.robotDrive.getPose, + # Functional interface to feed supplier + constants.DriveConstants.kDriveKinematics, + # Position controllers + HolonomicDriveController( + PIDController(constants.AutoConstants.kPXController, 0, 0), + PIDController(constants.AutoConstants.kPYController, 0, 0), + theta_controller + ), + Rotation2d(units.degrees_to_radians(0)), + self.robotDrive.setModuleStates, + self.robotDrive + ) + + """ + Reset odometry to the initial pose of the trajectory, run path following + command, then stop at the end. + """ + return commands2.Commands.sequence( + commands2.InstantCommand( + lambda: self.robotDrive.resetOdometry( + example_trajectory.getInitialPose() + ) + ), + swerve_controller_command, + commands2.InstantCommand(lambda: self.robotDrive.drive(0, 0, 0, False)), + ) \ No newline at end of file diff --git a/SwerveControllerCommand/subsystems/drivesubsystems.py b/SwerveControllerCommand/subsystems/drivesubsystems.py new file mode 100644 index 0000000..a6d25c6 --- /dev/null +++ b/SwerveControllerCommand/subsystems/drivesubsystems.py @@ -0,0 +1,177 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +from wpilib import ADXRS450_Gyro +from swervemodule import SwerveModule +from wpimath.kinematics import ChassisSpeeds +from wpimath.kinematics import SwerveDrive4Kinematics, SwerveDrive4Odometry +from wpimath.geometry import Pose2d, Rotation2d +import constants + + +class DriveSubsystem: + def __init__(self): + """ + Robot swerve modules + """ + self.frontLeft = SwerveModule( + constants.DriveConstants.kFrontLeftDriveMotorPort, + constants.DriveConstants.kFrontLeftTurningMotorPort, + constants.DriveConstants.kFrontLeftDriveEncoderPorts, + constants.DriveConstants.kFrontLeftTurningEncoderPorts, + constants.DriveConstants.kFrontLeftDriveEncoderReversed, + constants.DriveConstants.kFrontLeftTurningEncoderReversed, + ) + + self.rearLeft = SwerveModule( + constants.DriveConstants.kRearLeftDriveMotorPort, + constants.DriveConstants.kRearLeftTurningMotorPort, + constants.DriveConstants.kRearLeftDriveEncoderPorts, + constants.DriveConstants.kRearLeftTurningEncoderPorts, + constants.DriveConstants.kRearLeftDriveEncoderReversed, + constants.DriveConstants.kRearLeftTurningEncoderReversed, + ) + + self.frontRight = SwerveModule( + constants.DriveConstants.kFrontRightDriveMotorPort, + constants.DriveConstants.kFrontRightTurningMotorPort, + constants.DriveConstants.kFrontRightDriveEncoderPorts, + constants.DriveConstants.kFrontRightTurningEncoderPorts, + constants.DriveConstants.kFrontRightDriveEncoderReversed, + constants.DriveConstants.kFrontRightTurningEncoderReversed, + ) + + self.rearRight = SwerveModule( + constants.DriveConstants.kRearRightDriveMotorPort, + constants.DriveConstants.kRearRightTurningMotorPort, + constants.DriveConstants.kRearRightDriveEncoderPorts, + constants.DriveConstants.kRearRightTurningEncoderPorts, + constants.DriveConstants.kRearRightDriveEncoderReversed, + constants.DriveConstants.kRearRightTurningEncoderReversed, + ) + + # The gyro sensor + + self.gyro = ADXRS450_Gyro() + + # Odometry class for tracking robot pose + + self.odometry = SwerveDrive4Odometry( + constants.DriveConstants.kDriveKinematics, + self.gyro.getRotation2d(), + [ + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.rearLeft.getPosition(), + self.rearRight.getPosition(), + ], + ) + + def periodic(self): + """ + Update the odometry in the periodic block + """ + self.odometry.update( + self.gyro.getRotation2d(), + [ + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.rearLeft.getPosition(), + self.rearRight.getPosition(), + ], + ) + + def getPose(self): + """ + Returns the currently-estimated pose of the robot. + """ + return self.odometry.getPoseMeters() + + def resetOdometry(self, pose): + """ + Resets the odometry to the specified pose. + """ + self.odometry.resetPosition( + self.gyro.getRotation2d(), + [ + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.rearLeft.getPosition(), + self.rearRight.getPosition(), + ], + pose, + ) + + def drive(self, xSpeed, ySpeed, rot, fieldRelative): + """ + Method to drive the robot using joystick info. + """ + swerveModuleStates = ( + constants.DriveConstants.kDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.discretize( + ( + ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, self.gyro.getRotation2d() + ) + if fieldRelative + else ChassisSpeeds(xSpeed, ySpeed, rot) + ), + constants.DriveConstants.kDrivePeriod, + ) + ) + ) + SwerveDrive4Kinematics.desaturateWheelSpeeds( + swerveModuleStates, constants.DriveConstants.kMaxSpeedMetersPerSecond + ) + self.frontLeft.setDesiredState(swerveModuleStates[0]) + self.frontRight.setDesiredState(swerveModuleStates[1]) + self.rearLeft.setDesiredState(swerveModuleStates[2]) + self.rearRight.setDesiredState(swerveModuleStates[3]) + + def setModuleStates(self, desiredStates): + """ + Sets the swerve ModuleStates. + """ + SwerveDrive4Kinematics.desaturateWheelSpeeds( + desiredStates, constants.DriveConstants.kMaxSpeedMetersPerSecond + ) + self.frontLeft.setDesiredState(desiredStates[0]) + self.frontRight.setDesiredState(desiredStates[1]) + self.rearLeft.setDesiredState(desiredStates[2]) + self.rearRight.setDesiredState(desiredStates[3]) + + + def resetEncoders(self): + """ + Resets the drive encoders to currently read a position of 0. + """ + self.frontLeft.resetEncoders() + self.rearLeft.resetEncoders() + self.frontRight.resetEncoders() + self.rearRight.resetEncoders() + + + def zeroHeading(self): + """ + Zeroes the heading of the robot. + """ + self.gyro.reset() + + + def getHeading(self): + """ + Returns the heading of the robot. + """ + return self.gyro.getRotation2d().getDegrees() + + + def getTurnRate(self): + """ + Returns the turn rate of the robot. + """ + return self.gyro.getRate() * ( + -1.0 if constants.DriveConstants.kGyroReversed else 1.0 + ) diff --git a/SwerveControllerCommand/subsystems/swervemodule.py b/SwerveControllerCommand/subsystems/swervemodule.py new file mode 100644 index 0000000..714e717 --- /dev/null +++ b/SwerveControllerCommand/subsystems/swervemodule.py @@ -0,0 +1,125 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +from wpilib import Encoder, Spark +from wpimath.geometry import Rotation2d +from wpimath.kinematics import SwerveModulePosition, SwerveModuleState +from wpimath.trajectory import TrapezoidProfile +from wpimath.controller import PIDController, ProfiledPIDController +import math +import constants + + +class SwerveModule: + """ + Represents a swerve module. + """ + + def __init__( + self, + driveMotorChannel, + turningMotorChannel, + driveEncoderChannels, + turningEncoderChannels, + driveEncoderReversed, + turningEncoderReversed, + ): + self.driveMotor = Spark(driveMotorChannel) + self.turningMotor = Spark(turningMotorChannel) + + self.driveEncoder = Encoder(*driveEncoderChannels) + self.turningEncoder = Encoder(*turningEncoderChannels) + + self.drivePIDController = PIDController( + constants.ModuleConstants.kPModuleDriveController, 0, 0 + ) + + self.turningPIDController = ProfiledPIDController( + constants.ModuleConstants.kPModuleTurningController, + 0, + 0, + TrapezoidProfile.Constraints( + constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond, + constants.ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared, + ), + ) + + # Set the distance per pulse for the drive encoder. + + self.driveEncoder.setDistancePerPulse( + constants.ModuleConstants.kDriveEncoderDistancePerPulse + ) + self.driveEncoder.setReverseDirection(driveEncoderReversed) + + # Set the distance in radians per pulse for the turning encoder. + + self.turningEncoder.setDistancePerPulse( + constants.ModuleConstants.kTurningEncoderDistancePerPulse + ) + self.turningEncoder.setReverseDirection(turningEncoderReversed) + + # Limit the PID Controller's input range between -pi and pi and set the input + # to be continuous. + + self.turningPIDController.enableContinuousInput(-math.pi, math.pi) + + def getState(self): + """ + Returns the state of the module + """ + return SwerveModuleState( + self.driveEncoder.getRate(), + Rotation2d(self.turningEncoder.getDistance()), + ) + + def getPosition(self): + """ + Gets the position of the module based on the encoder + """ + + return SwerveModulePosition( + self.driveEncoder.getDistance(), + Rotation2d(self.turningEncoder.getDistance()), + ) + + def setDesiredState(self, desiredState): + """ + Using desiredState, calculate the drive and turn output + """ + + encoderRotation = Rotation2d(self.turningEncoder.getDistance()) + + # Optimize the reference state to avoid spinning further than 90 degrees + + state = SwerveModuleState.optimize(desiredState, encoderRotation) + + # Scale speed by cosine of angle error. + + state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos() + + # Calculate the drive output from the drive PID controller. + + driveOutput = self.drivePIDController.calculate( + self.driveEncoder.getRate(), state.speedMetersPerSecond + ) + + # Calculate the turning motor output from the turning PID controller. + + turnOutput = self.turningPIDController.calculate( + self.turningEncoder.getDistance(), state.angle.getRadians() + ) + + # Set motor outputs + + self.driveMotor.set(driveOutput) + self.turningMotor.set(turnOutput) + + def resetEncoders(self): + """ + Resets the drive encoders + """ + self.driveEncoder.reset() + self.turningEncoder.reset() diff --git a/run_tests.sh b/run_tests.sh index bb4f8e0..4a4a61a 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -49,6 +49,7 @@ BASE_TESTS=" StateSpaceFlywheel StateSpaceFlywheelSysId SwerveBot + SwerveControllerCommand TankDrive TankDriveXboxController Timed/src