From 9d59fdef651b84c26ebf3d2a36e745d42777e3cb Mon Sep 17 00:00:00 2001 From: Astatin3 Date: Tue, 10 Jan 2023 18:54:58 -0700 Subject: [PATCH 01/40] Update RobotGyro.java --- src/main/java/frc4388/utility/RobotGyro.java | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index ef239ca..2d712c1 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -28,6 +28,11 @@ public class RobotGyro implements Gyro { private double m_lastPigeonAngle; private double m_deltaPigeonAngle; + private double diff_Pitch = 0; + private double diff_Yaw = 0; + private double diff_Roll = 0; + + /** * Creates a Gyro based on a pigeon * @param gyro the gyroscope to use for Gyro @@ -46,6 +51,12 @@ public class RobotGyro implements Gyro { m_isGyroAPigeon = false; } + // Sets a + // + public void ResetAng() { + + } + /** * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note * that the getRate() method for a navX will likely be much more accurate than for a pigeon. @@ -100,7 +111,9 @@ public class RobotGyro implements Gyro { */ private double[] getPigeonAngles() { double[] angles = new double[3]; - m_pigeon.getYawPitchRoll(angles); + m_pigeon.getPitch(angles); + m_pigeon.getYaw(angles); + m_pigeon.getRoll(angles); return angles; } From 01450785940114c4e74198f6691039adc655e9b9 Mon Sep 17 00:00:00 2001 From: Astatin3 Date: Tue, 10 Jan 2023 18:59:06 -0700 Subject: [PATCH 02/40] Update RobotGyro.java --- src/main/java/frc4388/utility/RobotGyro.java | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 2d712c1..54b232d 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -28,8 +28,8 @@ public class RobotGyro implements Gyro { private double m_lastPigeonAngle; private double m_deltaPigeonAngle; - private double diff_Pitch = 0; private double diff_Yaw = 0; + private double diff_Pitch = 0; private double diff_Roll = 0; @@ -54,7 +54,9 @@ public class RobotGyro implements Gyro { // Sets a // public void ResetAng() { - + diff_Yaw = m_pigeon.getYaw(); + diff_Pitch = m_pigeon.getPitch(); + diff_Roll = m_pigeon.getRoll(); } /** @@ -110,11 +112,10 @@ public class RobotGyro implements Gyro { * Roll is within [-90,+90] degrees. */ private double[] getPigeonAngles() { - double[] angles = new double[3]; - m_pigeon.getPitch(angles); - m_pigeon.getYaw(angles); - m_pigeon.getRoll(angles); - return angles; + double yaw = m_pigeon.getYaw(); + double pitch = m_pigeon.getPitch(); + double roll = m_pigeon.getRoll(); + return double[(yaw - diff_Yaw), (pitch - diff_Pitch), (roll - diff_Roll)]; } @Override From b9e406bec464d4bb155a5baa0a558a8bf13d95ff Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 10 Jan 2023 19:09:27 -0700 Subject: [PATCH 03/40] gyro wrapper class done --- src/main/java/frc4388/utility/RobotGyro.java | 30 +++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 54b232d..d3761c6 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -28,10 +28,9 @@ public class RobotGyro implements Gyro { private double m_lastPigeonAngle; private double m_deltaPigeonAngle; - private double diff_Yaw = 0; - private double diff_Pitch = 0; - private double diff_Roll = 0; - + private double yawZero = 0; + private double pitchZero = 0; + private double rollZero = 0; /** * Creates a Gyro based on a pigeon @@ -51,12 +50,15 @@ public class RobotGyro implements Gyro { m_isGyroAPigeon = false; } - // Sets a - // - public void ResetAng() { - diff_Yaw = m_pigeon.getYaw(); - diff_Pitch = m_pigeon.getPitch(); - diff_Roll = m_pigeon.getRoll(); + /** + * Resets yaw, pitch, and roll. + */ + public void resetAllAngles() { + if (!m_isGyroAPigeon) return; + + yawZero = m_pigeon.getYaw(); + pitchZero = m_pigeon.getPitch(); + rollZero = m_pigeon.getRoll(); } /** @@ -112,10 +114,10 @@ public class RobotGyro implements Gyro { * Roll is within [-90,+90] degrees. */ private double[] getPigeonAngles() { - double yaw = m_pigeon.getYaw(); - double pitch = m_pigeon.getPitch(); - double roll = m_pigeon.getRoll(); - return double[(yaw - diff_Yaw), (pitch - diff_Pitch), (roll - diff_Roll)]; + double[] ypr = new double[3]; + m_pigeon.getYawPitchRoll(ypr); + + return new double[] {(ypr[0] - yawZero), (ypr[1] - pitchZero), (ypr[2] - rollZero)}; } @Override From e89116d82e09adb17010985b3ae6db88b422bee6 Mon Sep 17 00:00:00 2001 From: Astatin3 Date: Thu, 12 Jan 2023 18:49:21 -0700 Subject: [PATCH 04/40] added pid controller --- src/main/java/frc4388/robot/Robot.java | 10 +++++- .../java/frc4388/robot/RobotContainer.java | 3 ++ src/main/java/frc4388/robot/RobotMap.java | 3 ++ .../robot/commands/AutoBalanceTF2.java | 36 +++++++++++++++++++ src/main/java/frc4388/utility/RobotGyro.java | 17 ++++----- 5 files changed, 60 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/AutoBalanceTF2.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e145b38..8db637a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,9 +7,13 @@ package frc4388.robot; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; /** @@ -106,6 +110,8 @@ public class Robot extends TimedRobot { m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); + + m_robotContainer.gyroRef.reset(); } /** @@ -113,7 +119,9 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - + SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); + SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); + SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 99f5cb1..16c43d3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; +import frc4388.utility.RobotGyro; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -36,6 +37,8 @@ public class RobotContainer { private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + public RobotGyro gyroRef = m_robotMap.gyro; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5f17853..23312a8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -22,6 +23,8 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { + private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + public RobotGyro gyro = new RobotGyro(m_pigeon2); public RobotMap() { configureLEDMotorControllers(); diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java new file mode 100644 index 0000000..33a672e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java @@ -0,0 +1,36 @@ +// 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. + +package frc4388.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class AutoBalanceTF2 extends PIDCommand { + /** Creates a new AutoBalanceTF2. */ + public AutoBalanceTF2() { + super( + // The controller that the command will use + new PIDController(0, 0, 0), + // This should return the measurement + () -> 0, + // This should return the setpoint (can also be a constant) + () -> 0, + // This uses the output + output -> { + // Use the output here + }); + // Use addRequirements() here to declare subsystem dependencies. + // Configure additional PID options by calling `getController` here. + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index d3761c6..eb8621c 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -8,6 +8,7 @@ package frc4388.utility; import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; import com.kauailabs.navx.frc.AHRS; @@ -21,14 +22,13 @@ import edu.wpi.first.math.MathUtil; public class RobotGyro implements Gyro { private RobotTime m_robotTime = RobotTime.getInstance(); - private PigeonIMU m_pigeon = null; + private WPI_Pigeon2 m_pigeon = null; private AHRS m_navX = null; public boolean m_isGyroAPigeon; //true if pigeon, false if navX private double m_lastPigeonAngle; private double m_deltaPigeonAngle; - private double yawZero = 0; private double pitchZero = 0; private double rollZero = 0; @@ -36,7 +36,7 @@ public class RobotGyro implements Gyro { * Creates a Gyro based on a pigeon * @param gyro the gyroscope to use for Gyro */ - public RobotGyro(PigeonIMU gyro) { + public RobotGyro(WPI_Pigeon2 gyro) { m_pigeon = gyro; m_isGyroAPigeon = true; } @@ -53,10 +53,9 @@ public class RobotGyro implements Gyro { /** * Resets yaw, pitch, and roll. */ - public void resetAllAngles() { + public void resetZeroValues() { if (!m_isGyroAPigeon) return; - yawZero = m_pigeon.getYaw(); pitchZero = m_pigeon.getPitch(); rollZero = m_pigeon.getRoll(); } @@ -90,7 +89,7 @@ public class RobotGyro implements Gyro { @Override public void calibrate() { if (m_isGyroAPigeon) { - m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); + m_pigeon.calibrate(); } else { m_navX.calibrate(); } @@ -98,6 +97,8 @@ public class RobotGyro implements Gyro { @Override public void reset() { + resetZeroValues(); + if (m_isGyroAPigeon) { m_pigeon.setYaw(0); } else { @@ -117,7 +118,7 @@ public class RobotGyro implements Gyro { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); - return new double[] {(ypr[0] - yawZero), (ypr[1] - pitchZero), (ypr[2] - rollZero)}; + return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; } @Override @@ -182,7 +183,7 @@ public class RobotGyro implements Gyro { } } - public PigeonIMU getPigeon(){ + public WPI_Pigeon2 getPigeon(){ return m_pigeon; } From 2c5aa4b3532c8bfb38a3455d6c13f8be2166343a Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 12 Jan 2023 18:51:28 -0700 Subject: [PATCH 05/40] Commit moment --- src/main/java/frc4388/robot/Robot.java | 11 ++++++++++- src/main/java/frc4388/utility/RobotGyro.java | 15 +++++++++------ 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e145b38..2eee91d 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,9 +7,13 @@ package frc4388.robot; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; /** @@ -25,6 +29,9 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; + private WPI_Pigeon2 pigeon = new WPI_Pigeon2(14); + private RobotGyro gyro = new RobotGyro(pigeon); + /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -113,7 +120,9 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - + SmartDashboard.putNumber("Gyro Yaw", gyro.getYaw()); + SmartDashboard.putNumber("Gyro Pitch", gyro.getPitch()); + SmartDashboard.putNumber("Gyro Roll", gyro.getRoll()); } /** diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index d3761c6..669122b 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -7,8 +7,7 @@ package frc4388.utility; -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.kauailabs.navx.frc.AHRS; // import edu.wpi.first.wpilibj.GyroBase; @@ -21,7 +20,7 @@ import edu.wpi.first.math.MathUtil; public class RobotGyro implements Gyro { private RobotTime m_robotTime = RobotTime.getInstance(); - private PigeonIMU m_pigeon = null; + private WPI_Pigeon2 m_pigeon = null; private AHRS m_navX = null; public boolean m_isGyroAPigeon; //true if pigeon, false if navX @@ -36,7 +35,7 @@ public class RobotGyro implements Gyro { * Creates a Gyro based on a pigeon * @param gyro the gyroscope to use for Gyro */ - public RobotGyro(PigeonIMU gyro) { + public RobotGyro(WPI_Pigeon2 gyro) { m_pigeon = gyro; m_isGyroAPigeon = true; } @@ -90,7 +89,7 @@ public class RobotGyro implements Gyro { @Override public void calibrate() { if (m_isGyroAPigeon) { - m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); + m_pigeon.calibrate(); } else { m_navX.calibrate(); } @@ -129,6 +128,10 @@ public class RobotGyro implements Gyro { } } + public double getYaw() { + return this.getAngle(); + } + /** * Gets an absolute heading of the robot * @return heading from -180 to 180 degrees @@ -182,7 +185,7 @@ public class RobotGyro implements Gyro { } } - public PigeonIMU getPigeon(){ + public WPI_Pigeon2 getPigeon(){ return m_pigeon; } From e8f60f771c8b5781d3cb35b7d794f59dddcf2717 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 13 Jan 2023 18:59:50 -0700 Subject: [PATCH 06/40] SwerveModule created --- src/main/java/frc4388/robot/Constants.java | 23 +++++++++++++ .../robot/subsystems/SwerveModule.java | 34 +++++++++++++++++++ 2 files changed, 57 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/SwerveModule.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d50cd10..8eb768f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,6 +23,29 @@ public final class Constants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } + + public static final class SwerveDriveConstants { + public static final class IDs { + public static final int DRIVE_PIGEON_ID = -1; + + public static final int LEFT_FRONT_WHEEL_ID = -1; + public static final int RIGHT_FRONT_WHEEL_ID = -1; + public static final int LEFT_BACK_WHEEL_ID = -1; + public static final int RIGHT_BACK_STEER_ID = -1; + + public static final int LEFT_FRONT_STEER_ID = -1; + public static final int RIGHT_FRONT_STEER_ID = -1; + public static final int LEFT_BACK_STEER_ID = -1; + public static final int RIGHT_BACK_WHEEL_ID = -1; + + public static final int LEFT_FRONT_ENCODER_ID = -1; + public static final int RIGHT_FRONT_ENCODER_ID = -1; + public static final int LEFT_BACK_ENCODER_ID = -1; + public static final int RIGHT_BACK_ENCODER_ID = -1; + } + + + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..9867da5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -0,0 +1,34 @@ +// 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. + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.SwerveDriveConstants; + +public class SwerveModule extends SubsystemBase { + private WPI_TalonFX driveMotor; + private WPI_TalonFX angleMotor; + private CANCoder canCoder; + + + /** Creates a new SwerveModule. */ + public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { + this.driveMotor = driveMotor; + this.angleMotor = angleMotor; + this.canCoder = canCoder; + + TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From ee55e04fc738f441c17ffd144ceb6482dd223295 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 13 Jan 2023 19:00:04 -0700 Subject: [PATCH 07/40] Gains class implemented --- src/main/java/frc4388/utility/Gains.java | 77 ++++++++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 src/main/java/frc4388/utility/Gains.java diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java new file mode 100644 index 0000000..7cda1e0 --- /dev/null +++ b/src/main/java/frc4388/utility/Gains.java @@ -0,0 +1,77 @@ +// 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. + +package frc4388.utility; + +/** Add your docs here. */ +public class Gains { + public double kP; + public double kI; + public double kD; + public double kF; + public int kIZone; + public double kPeakOutput; + public double kMaxOutput; + public double kMinOutput; + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = kPeakOutput; + this.kMaxOutput = kPeakOutput; + this.kMinOutput = -kPeakOutput; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = 1.0; + this.kMaxOutput = 1.0; + this.kMinOutput = -1.0; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kMaxOutput = kMaxOutput; + this.kMinOutput = kMinOutput; + this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput); + } +} \ No newline at end of file From eab188b5208c314a3a3f74212f487db43e12133e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 13 Jan 2023 19:39:24 -0700 Subject: [PATCH 08/40] SwerveModule done --- src/main/java/frc4388/robot/Constants.java | 23 +++-- .../frc4388/robot/subsystems/DiffDrive.java | 85 ------------------- .../robot/subsystems/SwerveModule.java | 56 +++++++++++- 3 files changed, 69 insertions(+), 95 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/DiffDrive.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8eb768f..7f5b739 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,6 +7,7 @@ package frc4388.robot; +import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; /** @@ -18,12 +19,6 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final class DriveConstants { - public static final int DRIVE_PIGEON_ID = 6; - - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - public static final class SwerveDriveConstants { public static final class IDs { public static final int DRIVE_PIGEON_ID = -1; @@ -44,7 +39,21 @@ public final class Constants { public static final int RIGHT_BACK_ENCODER_ID = -1; } - + public static final class PIDConstants { + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + } + + public static final class Conversions { + public static final int CANCODER_TICKS_PER_ROTATION = 4096; + } + + + public static final double MAX_SPEED_FEET_PER_SECOND = 16; // TODO: find the actual value + + public static final int SWERVE_TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java deleted file mode 100644 index 64861bc..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,85 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.DriveConstants; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotTime; - -/** - * Add your docs here. - */ -public class DiffDrive extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private WPI_TalonFX m_leftFrontMotor; - private WPI_TalonFX m_rightFrontMotor; - private WPI_TalonFX m_leftBackMotor; - private WPI_TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - private RobotGyro m_gyro; - - /** - * Add your docs here. - */ - public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor, - WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_driveTrain = driveTrain; - m_gyro = gyro; - } - - @Override - public void periodic() { - m_gyro.updatePigeonDeltas(); - - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - // SmartDashboard.putData(m_gyro); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 9867da5..ee495fe 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,17 +4,27 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.CANCoderConfiguration; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { private WPI_TalonFX driveMotor; private WPI_TalonFX angleMotor; private CANCoder canCoder; + + public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; /** Creates a new SwerveModule. */ @@ -24,11 +34,51 @@ public class SwerveModule extends SubsystemBase { this.canCoder = canCoder; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + angleConfig.slot0.kP = swerveGains.kP; + angleConfig.slot0.kI = swerveGains.kI; + angleConfig.slot0.kD = swerveGains.kD; + // use the CANcoder as the remote sensor for the primary TalonFX PID + angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID(); + angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + angleMotor.configAllSettings(angleConfig); + + CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); + canCoderConfig.magnetOffsetDegrees = offset; + canCoder.configAllSettings(canCoderConfig); } - @Override - public void periodic() { - // This method will be called once per scheduler run + /** + * Get the angle of a SwerveModule as a Rotation2d + * @return the angle of a SwerveModule as a Rotation2d + */ + public Rotation2d getAngle() { + // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. + return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } + + /** + * Set the speed and rotation of the SwerveModule from a SwerveModuleState object + * @param desiredState a SwerveModuleState representing the desired new state of the module + */ + public void setDesiredState(SwerveModuleState desiredState) { + Rotation2d currentRotation = this.getAngle(); + + SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + + // calculate the difference between our current rotational position and our new rotational position + Rotation2d rotationDelta = state.angle.minus(currentRotation); + + // calculate the new absolute position of the SwerveModule based on the difference in rotation + double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + + // convert the CANCoder from its position reading to ticks + double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient(); + angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + + double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); + } + } From da6ccb19d1f950086d7628445627a110a66e2bfc Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 13 Jan 2023 20:24:38 -0700 Subject: [PATCH 09/40] IT WORKS! --- src/main/java/frc4388/robot/Robot.java | 40 ++++++++++++++++--- src/main/java/frc4388/robot/RobotMap.java | 3 ++ .../robot/commands/AutoBalanceTF2.java | 31 ++++++++++++-- 3 files changed, 66 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index c86560c..c6f72b1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,12 +7,16 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.commands.AutoBalanceTF2; import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; @@ -29,8 +33,34 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - private WPI_Pigeon2 pigeon = new WPI_Pigeon2(14); - private RobotGyro gyro = new RobotGyro(pigeon); + public static class MicroBot extends SubsystemBase { + public WPI_Pigeon2 pigeon = new WPI_Pigeon2(14); + public RobotGyro gyro = new RobotGyro(pigeon); + + public TalonSRX frontLeft = new TalonSRX(2); + public TalonSRX backLeft = new TalonSRX(3); + public TalonSRX backRight = new TalonSRX(5); + public TalonSRX frontRight = new TalonSRX(4); + + public MicroBot() { + frontRight.configFactoryDefault(); + frontLeft.configFactoryDefault(); + backLeft.configFactoryDefault(); + backRight.configFactoryDefault(); + + frontLeft.setInverted(true); + backLeft.setInverted(true); + } + + public void setOutput(double output) { + frontRight.set(ControlMode.PercentOutput, output); + frontLeft.set(ControlMode.PercentOutput, output); + backLeft.set(ControlMode.PercentOutput, output); + backRight.set(ControlMode.PercentOutput, output); + } + } + + private MicroBot bot = new MicroBot(); /** * This function is run when the robot is first started up and should be @@ -41,6 +71,9 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + + bot.setDefaultCommand(new AutoBalanceTF2(bot.frontLeft, + bot.frontRight, bot.backLeft, bot.backRight, bot.gyro, bot)); } /** @@ -122,9 +155,6 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - SmartDashboard.putNumber("Gyro Yaw", gyro.getYaw()); - SmartDashboard.putNumber("Gyro Pitch", gyro.getPitch()); - SmartDashboard.putNumber("Gyro Roll", gyro.getRoll()); SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 23312a8..a7c4517 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -9,6 +9,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.WPI_Pigeon2; @@ -26,6 +27,8 @@ public class RobotMap { private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); public RobotGyro gyro = new RobotGyro(m_pigeon2); + private TalonSRX backleft = new TalonSRX(-1); + public RobotMap() { configureLEDMotorControllers(); configureDriveMotorControllers(); diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java index 33a672e..4299737 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java +++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java @@ -4,26 +4,51 @@ package frc4388.robot.commands; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.PIDCommand; +import frc4388.robot.Robot; +import frc4388.utility.RobotGyro; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutoBalanceTF2 extends PIDCommand { + // private TalonSRX frontLeft; + // private TalonSRX frontRight; + // private TalonSRX backLeft; + // private TalonSRX backRight; + /** Creates a new AutoBalanceTF2. */ - public AutoBalanceTF2() { + public AutoBalanceTF2(TalonSRX frontLeft, TalonSRX frontRight, TalonSRX backLeft, TalonSRX backRight, RobotGyro gyro, Robot.MicroBot bot) { super( // The controller that the command will use - new PIDController(0, 0, 0), + new PIDController(.7, .02, .1), // This should return the measurement - () -> 0, + () -> Math.abs(gyro.getPitch()) < 3 ? 0 : gyro.getPitch(), // This should return the setpoint (can also be a constant) () -> 0, // This uses the output output -> { // Use the output here + double out2 = -MathUtil.clamp(output / 20, -1, 1); + if (Math.abs(gyro.getPitch()) < 3) out2 = 0; + + SmartDashboard.putNumber("Output", out2); + frontLeft.set(ControlMode.PercentOutput, out2); + frontRight.set(ControlMode.PercentOutput, out2); + backRight.set(ControlMode.PercentOutput, out2); + backLeft.set(ControlMode.PercentOutput, out2); }); + + gyro.reset(); + addRequirements(bot); // Use addRequirements() here to declare subsystem dependencies. // Configure additional PID options by calling `getController` here. } From c454a08eb14ba6d0fe646fd1a24f71b972a3ee58 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 14 Jan 2023 10:37:09 -0700 Subject: [PATCH 10/40] Added PelvicInflamatoryDisease --- src/main/java/frc4388/robot/Robot.java | 3 +- .../robot/commands/AutoBalanceTF2.java | 61 ++++++--------- .../commands/PelvicInflamitoryDisease.java | 59 ++++++++++++++ src/main/java/frc4388/utility/Gains.java | 77 +++++++++++++++++++ 4 files changed, 162 insertions(+), 38 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java create mode 100644 src/main/java/frc4388/utility/Gains.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index c6f72b1..7e548a1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -72,8 +72,7 @@ public class Robot extends TimedRobot { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - bot.setDefaultCommand(new AutoBalanceTF2(bot.frontLeft, - bot.frontRight, bot.backLeft, bot.backRight, bot.gyro, bot)); + bot.setDefaultCommand(new AutoBalanceTF2(bot)); } /** diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java index 4299737..1c6f63c 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java +++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java @@ -16,46 +16,35 @@ import edu.wpi.first.wpilibj2.command.PIDCommand; import frc4388.robot.Robot; import frc4388.utility.RobotGyro; -// NOTE: Consider using this command inline, rather than writing a subclass. For more +// NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoBalanceTF2 extends PIDCommand { - // private TalonSRX frontLeft; - // private TalonSRX frontRight; - // private TalonSRX backLeft; - // private TalonSRX backRight; +public class AutoBalanceTF2 extends PelvicInflamitoryDisease { + Robot.MicroBot bot; - /** Creates a new AutoBalanceTF2. */ - public AutoBalanceTF2(TalonSRX frontLeft, TalonSRX frontRight, TalonSRX backLeft, TalonSRX backRight, RobotGyro gyro, Robot.MicroBot bot) { - super( - // The controller that the command will use - new PIDController(.7, .02, .1), - // This should return the measurement - () -> Math.abs(gyro.getPitch()) < 3 ? 0 : gyro.getPitch(), - // This should return the setpoint (can also be a constant) - () -> 0, - // This uses the output - output -> { - // Use the output here - double out2 = -MathUtil.clamp(output / 20, -1, 1); - if (Math.abs(gyro.getPitch()) < 3) out2 = 0; + /** Creates a new AutoBalanceTF2. */ + // ! finish integrating PelvicInflamatoryDisease + public AutoBalanceTF2(Robot.MicroBot bot) { + super(.7, .02, .1, 0); + addRequirements(bot); + this.bot = bot; + } - SmartDashboard.putNumber("Output", out2); - frontLeft.set(ControlMode.PercentOutput, out2); - frontRight.set(ControlMode.PercentOutput, out2); - backRight.set(ControlMode.PercentOutput, out2); - backLeft.set(ControlMode.PercentOutput, out2); - }); + @Override + public double getError() { + return bot.gyro.getPitch(); + } - gyro.reset(); - addRequirements(bot); - // Use addRequirements() here to declare subsystem dependencies. - // Configure additional PID options by calling `getController` here. - } + @Override + public void runWithOutput(double output) { + double out2 = -MathUtil.clamp(output / 20, -1, 1); + if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0; + bot.setOutput(out2); + } - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java new file mode 100644 index 0000000..c596201 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java @@ -0,0 +1,59 @@ +// 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. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.utility.Gains; + +public abstract class PelvicInflamitoryDisease extends CommandBase { + protected Gains gains; + private double output = 0; + + /** Creates a new PelvicInflamitoryDisease. */ + public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) { + gains = new Gains(kp, ki, kd, kf, 0); + } + + public PelvicInflamitoryDisease(Gains gains) { + this.gains = gains; + } + + /** produces the error from the setpoint */ + public abstract double getError(); + /** figure it out bitch */ + public abstract void runWithOutput(double output); + + // Called when the command is initially scheduled. + @Override + public void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double error = getError(); + cumError += error; + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + runWithOutput(output); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java new file mode 100644 index 0000000..7cda1e0 --- /dev/null +++ b/src/main/java/frc4388/utility/Gains.java @@ -0,0 +1,77 @@ +// 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. + +package frc4388.utility; + +/** Add your docs here. */ +public class Gains { + public double kP; + public double kI; + public double kD; + public double kF; + public int kIZone; + public double kPeakOutput; + public double kMaxOutput; + public double kMinOutput; + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = kPeakOutput; + this.kMaxOutput = kPeakOutput; + this.kMinOutput = -kPeakOutput; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = 1.0; + this.kMaxOutput = 1.0; + this.kMinOutput = -1.0; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kMaxOutput = kMaxOutput; + this.kMinOutput = kMinOutput; + this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput); + } +} \ No newline at end of file From dc899d42bcc7ad9f86f943963818643e9e4fe729 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 11:42:03 -0700 Subject: [PATCH 11/40] SwerveDrive class done --- src/main/java/frc4388/robot/Constants.java | 37 +++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 78 +++++++++++++++++++ .../robot/subsystems/SwerveModule.java | 25 +++++- 3 files changed, 123 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/SwerveDrive.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7f5b739..ba1b8ea 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,22 +21,22 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { public static final class IDs { - public static final int DRIVE_PIGEON_ID = -1; + public static final int DRIVE_PIGEON_ID = -1; // TODO: find actual ID - public static final int LEFT_FRONT_WHEEL_ID = -1; - public static final int RIGHT_FRONT_WHEEL_ID = -1; - public static final int LEFT_BACK_WHEEL_ID = -1; - public static final int RIGHT_BACK_STEER_ID = -1; - - public static final int LEFT_FRONT_STEER_ID = -1; - public static final int RIGHT_FRONT_STEER_ID = -1; - public static final int LEFT_BACK_STEER_ID = -1; - public static final int RIGHT_BACK_WHEEL_ID = -1; - - public static final int LEFT_FRONT_ENCODER_ID = -1; - public static final int RIGHT_FRONT_ENCODER_ID = -1; - public static final int LEFT_BACK_ENCODER_ID = -1; - public static final int RIGHT_BACK_ENCODER_ID = -1; + public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find actual ID + + public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_STEER_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find actual ID + + public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find actual ID } public static final class PIDConstants { @@ -47,10 +47,15 @@ public final class Constants { public static final class Conversions { public static final int CANCODER_TICKS_PER_ROTATION = 4096; + public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; } + public static final double MAX_SPEED_FEET_PER_SECOND = -1; // TODO: find the actual value - public static final double MAX_SPEED_FEET_PER_SECOND = 16; // TODO: find the actual value + public static final double WIDTH = -1; // TODO: find the actual value + public static final double HEIGHT = -1; // TODO: find the actual value + public static final double HALF_WIDTH = WIDTH / 2.d; + public static final double HALF_HEIGHT = HEIGHT / 2.d; public static final int SWERVE_TIMEOUT_MS = 30; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java new file mode 100644 index 0000000..025e5f9 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -0,0 +1,78 @@ +// 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. + +package frc4388.robot.subsystems; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.RobotGyro; + +public class SwerveDrive extends SubsystemBase { + + private SwerveModule leftFront; + private SwerveModule rightFront; + private SwerveModule leftBack; + private SwerveModule rightBack; + + private SwerveModule[] modules; + + private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); + + private RobotGyro gyro; + + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + this.leftFront = leftFront; + this.rightFront = rightFront; + this.leftBack = leftBack; + this.rightBack = rightBack; + + this.modules = new SwerveModule[] {leftFront, rightFront, leftBack, rightBack}; + + this.gyro = gyro; + } + + public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND; + double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND; + + SwerveModuleState[] states = kinematics.toSwerveModuleStates( + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) + ); + + SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + setModuleStates(states); + } + + /** + * Set each module of the swerve drive to the corresponding desired state. + * + * @param desiredStates Array of module states to set. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + for (int i = 0; i < desiredStates.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = desiredStates[i]; + module.setDesiredState(state); + } + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index ee495fe..ea9bc9b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -26,7 +26,6 @@ public class SwerveModule extends SubsystemBase { public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; - /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { this.driveMotor = driveMotor; @@ -49,6 +48,30 @@ public class SwerveModule extends SubsystemBase { canCoder.configAllSettings(canCoderConfig); } + /** + * Get the drive motor of the SwerveModule + * @return the drive motor of the SwerveModule + */ + public WPI_TalonFX getDriveMotor() { + return this.driveMotor; + } + + /** + * Get the angle motor of the SwerveModule + * @return the angle motor of the SwerveModule + */ + public WPI_TalonFX getAngleMotor() { + return this.angleMotor; + } + + /** + * Get the CANcoder of the SwerveModule + * @return the CANcoder of the SwerveModule + */ + public CANCoder getEncoder() { + return this.canCoder; + } + /** * Get the angle of a SwerveModule as a Rotation2d * @return the angle of a SwerveModule as a Rotation2d From a7a366a9b20780b9510a22605a076aaa33271fa9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 12:58:18 -0700 Subject: [PATCH 12/40] SwerveDrive RobotMap done --- src/main/java/frc4388/robot/Constants.java | 15 ++- src/main/java/frc4388/robot/RobotMap.java | 98 +++++++++++++++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 1 - 3 files changed, 102 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ba1b8ea..403ea2c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -50,14 +50,27 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; } + public static final class Configurations { + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + + public static final double LEFT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double RIGHT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double LEFT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double RIGHT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value + } + public static final double MAX_SPEED_FEET_PER_SECOND = -1; // TODO: find the actual value + // dimensions public static final double WIDTH = -1; // TODO: find the actual value public static final double HEIGHT = -1; // TODO: find the actual value public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; - public static final int SWERVE_TIMEOUT_MS = 30; + // misc + public static final int TIMEOUT_MS = 30; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5f17853..8943fc6 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,11 +10,13 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.PigeonIMU; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; /** @@ -23,18 +25,94 @@ import frc4388.utility.RobotGyro; */ public class RobotMap { - public RobotMap() { - configureLEDMotorControllers(); - configureDriveMotorControllers(); - } + public RobotMap() { + configureLEDMotorControllers(); + configureDriveMotors(); + } - /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + /* LED Subsystem */ + public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - void configureLEDMotorControllers() { + void configureLEDMotorControllers() { - } + } - void configureDriveMotorControllers() { - } + // swerve drive subsystem + public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); + public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); + public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + + public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + + public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); + public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); + public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + + public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); + public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); + public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + + + void configureDriveMotors() { + // config factory default + leftFrontWheel.configFactoryDefault(); + leftFrontSteer.configFactoryDefault(); + + rightFrontWheel.configFactoryDefault(); + rightFrontSteer.configFactoryDefault(); + + leftBackWheel.configFactoryDefault(); + leftBackSteer.configFactoryDefault(); + + rightBackWheel.configFactoryDefault(); + rightBackSteer.configFactoryDefault(); + + // config open loop ramp + leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config closed loop ramp + leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config neutral deadband + leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // config magnet offset + leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + leftBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + rightBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 025e5f9..0f32721 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -60,7 +60,6 @@ public class SwerveDrive extends SubsystemBase { /** * Set each module of the swerve drive to the corresponding desired state. - * * @param desiredStates Array of module states to set. */ public void setModuleStates(SwerveModuleState[] desiredStates) { From b1f0799f7e6cad9e9cda5208b0d590687224faf5 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 13:20:04 -0700 Subject: [PATCH 13/40] finished RobotMap, started RobotContainer work --- src/main/java/frc4388/robot/Constants.java | 120 +++++++++--------- .../java/frc4388/robot/RobotContainer.java | 3 + src/main/java/frc4388/robot/RobotMap.java | 18 +++ .../frc4388/robot/subsystems/SwerveDrive.java | 6 +- 4 files changed, 86 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 403ea2c..4b1f668 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -19,69 +19,71 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final class SwerveDriveConstants { - public static final class IDs { - public static final int DRIVE_PIGEON_ID = -1; // TODO: find actual ID + public static final class SwerveDriveConstants { + public static final class IDs { + public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find actual ID - public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find actual ID + public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_STEER_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find actual ID - public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_STEER_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find actual ID - - public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find actual ID - } - - public static final class PIDConstants { - public static final int SWERVE_SLOT_IDX = 0; - public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); - } - - public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 4096; - public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; - } - - public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value - - public static final double LEFT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value - public static final double RIGHT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value - public static final double LEFT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value - public static final double RIGHT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value - } - - public static final double MAX_SPEED_FEET_PER_SECOND = -1; // TODO: find the actual value - - // dimensions - public static final double WIDTH = -1; // TODO: find the actual value - public static final double HEIGHT = -1; // TODO: find the actual value - public static final double HALF_WIDTH = WIDTH / 2.d; - public static final double HALF_HEIGHT = HEIGHT / 2.d; - - // misc - public static final int TIMEOUT_MS = 30; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find actual ID + public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find actual ID + public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find actual ID + public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find actual ID } + + public static final class PIDConstants { + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + } + + public static final class Conversions { + public static final int CANCODER_TICKS_PER_ROTATION = 4096; + public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; + } + + public static final class Configurations { + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + + public static final double LEFT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double RIGHT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double LEFT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value + public static final double RIGHT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value + } + + public static final double MAX_SPEED_FEET_PER_SECOND = -1; // TODO: find the actual value + + // dimensions + public static final double WIDTH = -1; // TODO: find the actual value + public static final double HEIGHT = -1; // TODO: find the actual value + public static final double HALF_WIDTH = WIDTH / 2.d; + public static final double HALF_HEIGHT = HEIGHT / 2.d; + + // misc + public static final int TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + } + + public static final class GyroConstants { + public static final int ID = -1; // TODO: find actual ID + } - public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; + public static final class LEDConstants { + public static final int LED_SPARK_ID = 0; - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - } + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + } - public static final class OIConstants { - public static final int XBOX_DRIVER_ID = 0; - public static final int XBOX_OPERATOR_ID = 1; - } + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 99f5cb1..02cd2b3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -30,7 +31,9 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); private final LED m_robotLED = new LED(m_robotMap.LEDController); + /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 8943fc6..308a377 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -12,11 +12,14 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import frc4388.robot.Constants.GyroConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; /** @@ -25,9 +28,18 @@ import frc4388.utility.RobotGyro; */ public class RobotMap { + public SwerveModule leftFront; + public SwerveModule rightFront; + public SwerveModule leftBack; + public SwerveModule rightBack; + + public WPI_Pigeon2 gyro; + public RobotMap() { configureLEDMotorControllers(); configureDriveMotors(); + + gyro = new WPI_Pigeon2(GyroConstants.ID); } /* LED Subsystem */ @@ -114,5 +126,11 @@ public class RobotMap { leftBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); rightBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + // initialize SwerveModules + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0f32721..f30e8ff 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -31,10 +33,10 @@ public class SwerveDrive extends SubsystemBase { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - private RobotGyro gyro; + private WPI_Pigeon2 gyro; /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, WPI_Pigeon2 gyro) { this.leftFront = leftFront; this.rightFront = rightFront; this.leftBack = leftBack; From 5999580117ae054efa3a5e2ea4fa6fe010a74a02 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 13:29:41 -0700 Subject: [PATCH 14/40] SwerveDrive RobotContainer + JoystickButtons done --- src/main/java/frc4388/robot/RobotContainer.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 02cd2b3..9674e56 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -49,6 +49,12 @@ public class RobotContainer { // drives the robot with a two-axis input from the driver controller // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + + m_robotSwerveDrive.setDefaultCommand( + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), + getDriverController().getLeftYAxis(), + -getDriverController().getRightXAxis(), false), m_robotSwerveDrive) + ); } /** @@ -64,8 +70,8 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))) + .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN))); } /** From 158513c92ddaa988721509806fef58e9c4bb7d29 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 14 Jan 2023 14:26:17 -0700 Subject: [PATCH 15/40] more worky --- .../robot/commands/AutoBalanceTF2.java | 20 ++++++++----------- .../commands/PelvicInflamitoryDisease.java | 2 +- 2 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java index 1c6f63c..d06631c 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java +++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java @@ -4,17 +4,8 @@ package frc4388.robot.commands; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.sensors.WPI_Pigeon2; - import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.PIDCommand; import frc4388.robot.Robot; -import frc4388.utility.RobotGyro; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -23,9 +14,8 @@ public class AutoBalanceTF2 extends PelvicInflamitoryDisease { Robot.MicroBot bot; /** Creates a new AutoBalanceTF2. */ - // ! finish integrating PelvicInflamatoryDisease public AutoBalanceTF2(Robot.MicroBot bot) { - super(.7, .02, .1, 0); + super(.7, .1, 15, 0); addRequirements(bot); this.bot = bot; } @@ -37,11 +27,17 @@ public class AutoBalanceTF2 extends PelvicInflamitoryDisease { @Override public void runWithOutput(double output) { - double out2 = -MathUtil.clamp(output / 20, -1, 1); + double out2 = MathUtil.clamp(output / 40, -.5, .5); if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0; bot.setOutput(out2); } + @Override + public void initialize() { + super.initialize(); + this.bot.gyro.reset(); + } + // Returns true when the command should end. @Override public boolean isFinished() { diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java index c596201..e377c0a 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java @@ -36,7 +36,7 @@ public abstract class PelvicInflamitoryDisease extends CommandBase { @Override public void execute() { double error = getError(); - cumError += error; + cumError += error * .02; // 20 ms double delta = error - prevError; output = error * gains.kP; From 3aab9c6c4bc7d686c8d7d3413537fd222d33fb16 Mon Sep 17 00:00:00 2001 From: Ryan <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 14 Jan 2023 14:36:38 -0700 Subject: [PATCH 16/40] interrupt button --- src/main/java/frc4388/robot/RobotMap.java | 3 --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 4 ++-- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index e1a44a5..31200e1 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -38,13 +38,10 @@ public class RobotMap { public SwerveModule leftBack; public SwerveModule rightBack; - public WPI_Pigeon2 gyro; public RobotMap() { configureLEDMotorControllers(); configureDriveMotors(); - - gyro = new WPI_Pigeon2(GyroConstants.ID); } /* LED Subsystem */ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f30e8ff..d6cb9e9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -33,10 +33,10 @@ public class SwerveDrive extends SubsystemBase { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - private WPI_Pigeon2 gyro; + private RobotGyro gyro; /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, WPI_Pigeon2 gyro) { + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { this.leftFront = leftFront; this.rightFront = rightFront; this.leftBack = leftBack; From 874e18156c4b5c23f52ae4d1dfb20d9bf6b0cd64 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 14:57:53 -0700 Subject: [PATCH 17/40] fix buttons --- src/main/java/frc4388/robot/RobotContainer.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9674e56..197cbd0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,7 +33,6 @@ public class RobotContainer { /* Subsystems */ private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); private final LED m_robotLED = new LED(m_robotMap.LEDController); - /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -70,8 +69,8 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))) - .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN))); + .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) + .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); } /** From 745ab7bc3509af4b2bdc1ddf8aea865e12ed6ca6 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 14 Jan 2023 15:23:48 -0700 Subject: [PATCH 18/40] ;;;; --- .../commands/PelvicInflamitoryDisease.java | 72 +++++++++---------- 1 file changed, 33 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java index e377c0a..df24402 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java @@ -8,52 +8,46 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; public abstract class PelvicInflamitoryDisease extends CommandBase { - protected Gains gains; - private double output = 0; +;;;;protected Gains gains; +;;;;private double output = 0; - /** Creates a new PelvicInflamitoryDisease. */ - public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) { - gains = new Gains(kp, ki, kd, kf, 0); - } +;;;;/** Creates a new PelvicInflamitoryDisease. */ +;;;;public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) { +;;;;;;;;gains = new Gains(kp, ki, kd, kf, 0); +;;;;} - public PelvicInflamitoryDisease(Gains gains) { - this.gains = gains; - } +;;;;public PelvicInflamitoryDisease(Gains gains) { +;;;;;;;;this.gains = gains; +;;;;} - /** produces the error from the setpoint */ - public abstract double getError(); - /** figure it out bitch */ - public abstract void runWithOutput(double output); +;;;;/** produces the error from the setpoint */ +;;;;public abstract double getError(); +;;;;/** figure it out bitch */ +;;;;public abstract void runWithOutput(double output); - // Called when the command is initially scheduled. - @Override - public void initialize() { - output = 0; - } +;;;;// Called when the command is initially scheduled. +;;;;@Override public void initialize() { +;;;;;;;;output = 0; +;;;;} - private double prevError, cumError = 0; - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double error = getError(); - cumError += error * .02; // 20 ms - double delta = error - prevError; +;;;;private double prevError, cumError = 0; +;;;;// Called every time the scheduler runs while the command is scheduled. +;;;;@Override public void execute() { +;;;;;;;;double error = getError(); +;;;;;;;;cumError += error * .02; // 20 ms +;;;;;;;;double delta = error - prevError; - output = error * gains.kP; - output += cumError * gains.kI; - output += delta * gains.kD; - output += gains.kF; +;;;;;;;;output = error * gains.kP; +;;;;;;;;output += cumError * gains.kI; +;;;;;;;;output += delta * gains.kD; +;;;;;;;;output += gains.kF; - runWithOutput(output); - } +;;;;;;;;runWithOutput(output); +;;;;} - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} +;;;;// Called once the command ends or is interrupted. +;;;;@Override public void end(boolean interrupted) {} - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } +;;;;// Returns true when the command should end. +;;;;@Override public boolean isFinished() { return false; } } From 9adfdb8791c6fcaf2b703cc50b14b4966a866ca4 Mon Sep 17 00:00:00 2001 From: Ryan <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 14 Jan 2023 15:30:38 -0700 Subject: [PATCH 19/40] converstions and getState --- .../java/frc4388/robot/RobotContainer.java | 9 +++++++- .../robot/subsystems/SwerveModule.java | 23 +++++++++++++++++++ 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7e6699b..a42a463 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -34,6 +34,7 @@ public class RobotContainer { /* Subsystems */ private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); private final LED m_robotLED = new LED(m_robotMap.LEDController); + /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -67,13 +68,19 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - // test command to spin the robot while pressing A on the driver controller + + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> gyroRef.reset())); /* Operator Buttons */ // activates "Lit Mode" new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); + + //New interupt button + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .onTrue(new InstantCommand()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index ea9bc9b..89a986a 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; @@ -81,6 +82,26 @@ public class SwerveModule extends SubsystemBase { return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } + public void stop() { + driveMotor.set(0); + angleMotor.set(0); + } + + public void rotateToAngle(double angle){ + angleMotor.set(TalonFXControlMode.Position, angle); + } + + /** + * Get state of swerve module + * @return speed in m/s and angle in degrees + */ + public SwerveModuleState getState() { + return new SwerveModuleState( + Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + getAngle() + ); + } + /** * Set the speed and rotation of the SwerveModule from a SwerveModuleState object * @param desiredState a SwerveModuleState representing the desired new state of the module @@ -104,4 +125,6 @@ public class SwerveModule extends SubsystemBase { driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); } + + } From 244be35526dbd5ae23fade3c611b47195e6e622b Mon Sep 17 00:00:00 2001 From: Ryan <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 14 Jan 2023 15:33:12 -0700 Subject: [PATCH 20/40] fixes --- .../{AutoBalanceTF2.java => AutoBalance.java} | 0 .../commands/PelvicInflamitoryDisease.java | 76 +++++++++---------- 2 files changed, 38 insertions(+), 38 deletions(-) rename src/main/java/frc4388/robot/commands/{AutoBalanceTF2.java => AutoBalance.java} (100%) diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalance.java similarity index 100% rename from src/main/java/frc4388/robot/commands/AutoBalanceTF2.java rename to src/main/java/frc4388/robot/commands/AutoBalance.java diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java index df24402..7fba701 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java @@ -8,46 +8,46 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; public abstract class PelvicInflamitoryDisease extends CommandBase { -;;;;protected Gains gains; -;;;;private double output = 0; + protected Gains gains; + private double output = 0; -;;;;/** Creates a new PelvicInflamitoryDisease. */ -;;;;public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) { -;;;;;;;;gains = new Gains(kp, ki, kd, kf, 0); -;;;;} + /** Creates a new PelvicInflamitoryDisease. */ + public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) { + gains = new Gains(kp, ki, kd, kf, 0); + } -;;;;public PelvicInflamitoryDisease(Gains gains) { -;;;;;;;;this.gains = gains; -;;;;} + public PelvicInflamitoryDisease(Gains gains) { + this.gains = gains; + } -;;;;/** produces the error from the setpoint */ -;;;;public abstract double getError(); -;;;;/** figure it out bitch */ -;;;;public abstract void runWithOutput(double output); + /** produces the error from the setpoint */ + public abstract double getError(); + /** figure it out bitch */ + public abstract void runWithOutput(double output); -;;;;// Called when the command is initially scheduled. -;;;;@Override public void initialize() { -;;;;;;;;output = 0; -;;;;} - -;;;;private double prevError, cumError = 0; -;;;;// Called every time the scheduler runs while the command is scheduled. -;;;;@Override public void execute() { -;;;;;;;;double error = getError(); -;;;;;;;;cumError += error * .02; // 20 ms -;;;;;;;;double delta = error - prevError; - -;;;;;;;;output = error * gains.kP; -;;;;;;;;output += cumError * gains.kI; -;;;;;;;;output += delta * gains.kD; -;;;;;;;;output += gains.kF; - -;;;;;;;;runWithOutput(output); -;;;;} - -;;;;// Called once the command ends or is interrupted. -;;;;@Override public void end(boolean interrupted) {} - -;;;;// Returns true when the command should end. -;;;;@Override public boolean isFinished() { return false; } +// Called when the command is initially scheduled. +@Override public void initialize() { +output = 0; +} + +private double prevError, cumError = 0; +// Called every time the scheduler runs while the command is scheduled. +@Override public void execute() { +double error = getError(); +cumError += error * .02; // 20 ms +double delta = error - prevError; + +output = error * gains.kP; +output += cumError * gains.kI; +output += delta * gains.kD; +output += gains.kF; + + runWithOutput(output); + } + + // Called once the command ends or is interrupted. + @Override public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override public boolean isFinished() { return false; } } From 7fa1e05e6e5e0fed75a239ed3e91e5260d6887c4 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 14 Jan 2023 15:35:21 -0700 Subject: [PATCH 21/40] conversions --- src/main/java/frc4388/robot/Constants.java | 16 ++++++++++++++++ .../java/frc4388/robot/commands/AutoBalance.java | 4 ++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4b1f668..3b29a1c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,6 +46,22 @@ public final class Constants { public static final class Conversions { public static final int CANCODER_TICKS_PER_ROTATION = 4096; public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; + + public static final double MOTOR_REV_PER_WHEEL_REV = -1; // TODO: find actual ID + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: find actual ID + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + + public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; + + public static final double TICK_TIME_TO_SECONDS = 10; + public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + + // public static final double + } public static final class Configurations { diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index d06631c..62c3160 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -10,11 +10,11 @@ import frc4388.robot.Robot; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoBalanceTF2 extends PelvicInflamitoryDisease { +public class AutoBalance extends PelvicInflamitoryDisease { Robot.MicroBot bot; /** Creates a new AutoBalanceTF2. */ - public AutoBalanceTF2(Robot.MicroBot bot) { + public AutoBalance(Robot.MicroBot bot) { super(.7, .1, 15, 0); addRequirements(bot); this.bot = bot; From 93b6bcc65d4e645d048a55b6470c9bd3a6b5af7b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 14 Jan 2023 15:36:02 -0700 Subject: [PATCH 22/40] fix AutoBalance renaming --- src/main/java/frc4388/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 7e548a1..3f93b48 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.commands.AutoBalanceTF2; +import frc4388.robot.commands.AutoBalance; import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; @@ -72,7 +72,7 @@ public class Robot extends TimedRobot { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - bot.setDefaultCommand(new AutoBalanceTF2(bot)); + bot.setDefaultCommand(new AutoBalance(bot)); } /** From 9043b6aa8b94d3ce3dda46529a9c1d9ca85fe0f7 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 15 Jan 2023 15:15:22 -0700 Subject: [PATCH 23/40] more functionality for SwerveModule --- .../java/frc4388/robot/subsystems/SwerveModule.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 89a986a..fbf4bb8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -125,6 +125,15 @@ public class SwerveModule extends SubsystemBase { driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); } - + public void reset() { + canCoder.setPositionToAbsolute(); + } -} + public double getCurrent() { + return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + } + + public double getVoltage() { + return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + } +} \ No newline at end of file From acd660662ef48e2461c00c3973067721b0bd2557 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 15 Jan 2023 15:17:54 -0700 Subject: [PATCH 24/40] fixed some indentation stuff --- .../commands/PelvicInflamitoryDisease.java | 37 +++++++++++-------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java index 7fba701..0f6bc9a 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java @@ -25,29 +25,34 @@ public abstract class PelvicInflamitoryDisease extends CommandBase { /** figure it out bitch */ public abstract void runWithOutput(double output); -// Called when the command is initially scheduled. -@Override public void initialize() { -output = 0; -} + // Called when the command is initially scheduled. + @Override + public void initialize() { + output = 0; + } -private double prevError, cumError = 0; -// Called every time the scheduler runs while the command is scheduled. -@Override public void execute() { -double error = getError(); -cumError += error * .02; // 20 ms -double delta = error - prevError; + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double error = getError(); + cumError += error * .02; // 20 ms + double delta = error - prevError; -output = error * gains.kP; -output += cumError * gains.kI; -output += delta * gains.kD; -output += gains.kF; + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; runWithOutput(output); } // Called once the command ends or is interrupted. - @Override public void end(boolean interrupted) {} + @Override + public void end(boolean interrupted) {} // Returns true when the command should end. - @Override public boolean isFinished() { return false; } + @Override + public boolean isFinished() { return false; } } From eff000bb6336afd5b616e9484a5280644b2de463 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 15 Jan 2023 15:58:40 -0700 Subject: [PATCH 25/40] minor fixes, swerve auto constants --- src/main/java/frc4388/robot/Constants.java | 49 ++++++++++++------- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- 2 files changed, 31 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3b29a1c..a7dc11e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,6 +7,9 @@ package frc4388.robot; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -21,20 +24,20 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { public static final class IDs { - public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find actual ID + public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID + public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID + public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find the actual ID + public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find the actual ID - public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_STEER_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find actual ID + public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find the actual ID + public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find the actual ID + public static final int LEFT_BACK_STEER_ID = -1; // TODO: find the actual ID + public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find the actual ID - public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find actual ID - public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find actual ID - public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find actual ID - public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find actual ID + public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID + public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID + public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find the actual ID + public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find the actual ID } public static final class PIDConstants { @@ -43,13 +46,24 @@ public final class Constants { public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); } + public static final class AutoConstants { + public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0); + public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0); + public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, + new TrapezoidProfile.Constraints(0.0, 0.0) + ); + + public static final double PATH_MAX_VEL = -1; // TODO: find the actual value + public static final double PATH_MAX_ACC = -1; // TODO: find the actual value + } + public static final class Conversions { public static final int CANCODER_TICKS_PER_ROTATION = 4096; - public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; + public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; // 2022's robot: 11 m/s for fast, 2 m/s for slow - public static final double MOTOR_REV_PER_WHEEL_REV = -1; // TODO: find actual ID + public static final double MOTOR_REV_PER_WHEEL_REV = -1; // TODO: find the actual value public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: find actual ID + public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; @@ -59,9 +73,6 @@ public final class Constants { public static final double TICK_TIME_TO_SECONDS = 10; public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; - - // public static final double - } public static final class Configurations { @@ -89,7 +100,7 @@ public final class Constants { } public static final class GyroConstants { - public static final int ID = -1; // TODO: find actual ID + public static final int ID = -1; // TODO: find the actual ID } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d6cb9e9..83ebdfd 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -42,7 +42,7 @@ public class SwerveDrive extends SubsystemBase { this.leftBack = leftBack; this.rightBack = rightBack; - this.modules = new SwerveModule[] {leftFront, rightFront, leftBack, rightBack}; + this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; this.gyro = gyro; } From e1c02a1cc751c3029187810cf143d0a2d499c817 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 15 Jan 2023 16:06:05 -0700 Subject: [PATCH 26/40] imports hehe --- src/main/java/frc4388/robot/RobotMap.java | 11 ++--------- .../java/frc4388/robot/subsystems/SwerveDrive.java | 4 ---- .../java/frc4388/robot/subsystems/SwerveModule.java | 1 - src/main/java/frc4388/utility/RobotGyro.java | 3 --- 4 files changed, 2 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 31200e1..08fcc1a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,17 +7,11 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import frc4388.robot.Constants.GyroConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -28,10 +22,9 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); - public RobotGyro gyro = new RobotGyro(m_pigeon2); + private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + public RobotGyro gyro = new RobotGyro(m_pigeon2); - private TalonSRX backleft = new TalonSRX(-1); public SwerveModule leftFront; public SwerveModule rightFront; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 83ebdfd..69d5e31 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,14 +4,10 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.sensors.WPI_Pigeon2; - -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index fbf4bb8..7d3553a 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -7,7 +7,6 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 3261421..a037914 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -8,9 +8,6 @@ package frc4388.utility; import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; import com.kauailabs.navx.frc.AHRS; // import edu.wpi.first.wpilibj.GyroBase; From 2d2317af4d0ee2f4ffdc47f1c9174203df02001d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 15 Jan 2023 16:32:41 -0700 Subject: [PATCH 27/40] high and low speed gears --- src/main/java/frc4388/robot/Constants.java | 4 +++- .../java/frc4388/robot/subsystems/SwerveDrive.java | 11 +++++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a7dc11e..9b39fd4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -59,7 +59,9 @@ public final class Constants { public static final class Conversions { public static final int CANCODER_TICKS_PER_ROTATION = 4096; - public static final double JOYSTICK_TO_METERS_PER_SECOND = 5.0; // 2022's robot: 11 m/s for fast, 2 m/s for slow + + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value public static final double MOTOR_REV_PER_WHEEL_REV = -1; // TODO: find the actual value public static final double TICKS_PER_MOTOR_REV = 2048; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 69d5e31..5a7c8cd 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -31,6 +31,8 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { this.leftFront = leftFront; @@ -44,8 +46,8 @@ public class SwerveDrive extends SubsystemBase { } public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND; - double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND; + double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; SwerveModuleState[] states = kinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) @@ -72,4 +74,9 @@ public class SwerveDrive extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run } + + public void highSpeed(boolean shift) { + this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + } + } From 01973134339624293f132dd3ce66df284fbbfe1c Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 15 Jan 2023 20:25:07 -0700 Subject: [PATCH 28/40] odometry --- .../frc4388/robot/commands/AutoBalance.java | 2 +- ...se.java => PelvicInflammatoryDisease.java} | 6 +- .../frc4388/robot/subsystems/SwerveDrive.java | 78 ++++++++++++++++++- .../robot/subsystems/SwerveModule.java | 9 +++ 4 files changed, 89 insertions(+), 6 deletions(-) rename src/main/java/frc4388/robot/commands/{PelvicInflamitoryDisease.java => PelvicInflammatoryDisease.java} (87%) diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 62c3160..a8cff7d 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -10,7 +10,7 @@ import frc4388.robot.Robot; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoBalance extends PelvicInflamitoryDisease { +public class AutoBalance extends PelvicInflammatoryDisease { Robot.MicroBot bot; /** Creates a new AutoBalanceTF2. */ diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java similarity index 87% rename from src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java rename to src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 0f6bc9a..1ab08d5 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -7,16 +7,16 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; -public abstract class PelvicInflamitoryDisease extends CommandBase { +public abstract class PelvicInflammatoryDisease extends CommandBase { protected Gains gains; private double output = 0; /** Creates a new PelvicInflamitoryDisease. */ - public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) { + public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) { gains = new Gains(kp, ki, kd, kf, 0); } - public PelvicInflamitoryDisease(Gains gains) { + public PelvicInflammatoryDisease(Gains gains) { this.gains = gains; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 5a7c8cd..7f065f3 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,9 +4,12 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,14 +25,25 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule[] modules; + private RobotGyro gyro; + private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - - private RobotGyro gyro; + + private SwerveDriveOdometry odometry = new SwerveDriveOdometry( + kinematics, + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + } + ); public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default @@ -70,11 +84,71 @@ public class SwerveDrive extends SubsystemBase { } } + /** + * Updates the odometry of the SwerveDrive. + */ + public void updateOdometry() { + odometry.update( + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + } + ); + } + + /** + * Gets the odometry of the SwerveDrive. + * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). + */ + public Pose2d getOdometry() { + return odometry.getPoseMeters(); + } + + /** + * Sets the odometry of the SwerveDrive. + * @param pose Pose to set the odometry to. + */ + public void setOdometry(Pose2d pose) { + odometry.resetPosition( + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + }, + pose + ); + } + + /** + * Resets the odometry of the SwerveDrive to 0. + */ + public void resetOdometry() { + odometry.resetPosition( + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + }, + new Pose2d() + ); + } + @Override public void periodic() { // This method will be called once per scheduler run } + /** + * Shifts gear from high to low, or vice versa. + * @param shift true to shift to high, false to shift to low + */ public void highSpeed(boolean shift) { this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 7d3553a..5d5fb80 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -13,6 +13,7 @@ import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderConfiguration; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -101,6 +102,14 @@ public class SwerveModule extends SubsystemBase { ); } + /** + * Returns the current position of the SwerveModule + * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. + */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); + } + /** * Set the speed and rotation of the SwerveModule from a SwerveModuleState object * @param desiredState a SwerveModuleState representing the desired new state of the module From 3c9a29d94ba73eb80e24d3aaf55c02e0ba02491a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 19 Jan 2023 17:49:39 -0700 Subject: [PATCH 29/40] note --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 7f065f3..cb4ea38 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -126,6 +126,7 @@ public class SwerveDrive extends SubsystemBase { /** * Resets the odometry of the SwerveDrive to 0. + * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions. */ public void resetOdometry() { odometry.resetPosition( From 6b631e914dc21eebe5c29a66d4b9a30e5b5f99e9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 20 Jan 2023 17:50:24 -0700 Subject: [PATCH 30/40] yes --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 1 + src/main/java/frc4388/robot/subsystems/SwerveModule.java | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index cb4ea38..676d800 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -144,6 +144,7 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + updateOdometry(); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 5d5fb80..ea39ba4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -87,7 +87,7 @@ public class SwerveModule extends SubsystemBase { angleMotor.set(0); } - public void rotateToAngle(double angle){ + public void rotateToAngle(double angle) { angleMotor.set(TalonFXControlMode.Position, angle); } From bf4088d9dbe7e4eafaaba0314eda22c82ec09a26 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 20 Jan 2023 21:04:58 -0700 Subject: [PATCH 31/40] swerve shtuff --- src/main/java/frc4388/robot/Constants.java | 52 ++++--- src/main/java/frc4388/robot/Robot.java | 12 +- .../java/frc4388/robot/RobotContainer.java | 31 ++-- src/main/java/frc4388/robot/RobotMap.java | 22 +-- .../frc4388/robot/subsystems/SwerveDrive.java | 142 ++++++++++-------- .../robot/subsystems/SwerveModule.java | 6 +- 6 files changed, 150 insertions(+), 115 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9b39fd4..d5774a1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,20 +24,21 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { public static final class IDs { - public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID - public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID - public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find the actual ID - public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find the actual ID + public static final int LEFT_FRONT_WHEEL_ID = 2; + public static final int LEFT_FRONT_STEER_ID = 3; + public static final int LEFT_FRONT_ENCODER_ID = 10; - public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find the actual ID - public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find the actual ID - public static final int LEFT_BACK_STEER_ID = -1; // TODO: find the actual ID - public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find the actual ID + public static final int RIGHT_FRONT_WHEEL_ID = 4; + public static final int RIGHT_FRONT_STEER_ID = 5; + public static final int RIGHT_FRONT_ENCODER_ID = 11; - public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID - public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID - public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find the actual ID - public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find the actual ID + public static final int LEFT_BACK_WHEEL_ID = 6; + public static final int LEFT_BACK_STEER_ID = 7; + public static final int LEFT_BACK_ENCODER_ID = 12; + + public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_STEER_ID = 9; + public static final int RIGHT_BACK_ENCODER_ID = 13; } public static final class PIDConstants { @@ -63,7 +64,9 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value - public static final double MOTOR_REV_PER_WHEEL_REV = -1; // TODO: find the actual value + public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value + public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value + public static final double TICKS_PER_MOTOR_REV = 2048; public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; @@ -82,17 +85,22 @@ public final class Constants { public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value - public static final double LEFT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value - public static final double RIGHT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value - public static final double LEFT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value - public static final double RIGHT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value + // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // TODO: find the actual value + // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // TODO: find the actual value + // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // TODO: find the actual value + // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // TODO: find the actual value + + public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // TODO: find the actual value + public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // TODO: find the actual value + public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // TODO: find the actual value + public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // TODO: find the actual value } - public static final double MAX_SPEED_FEET_PER_SECOND = -1; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value // dimensions - public static final double WIDTH = -1; // TODO: find the actual value - public static final double HEIGHT = -1; // TODO: find the actual value + public static final double WIDTH = 18.5; // TODO: find the actual value + public static final double HEIGHT = 18.5; // TODO: find the actual value public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; @@ -102,11 +110,11 @@ public final class Constants { } public static final class GyroConstants { - public static final int ID = -1; // TODO: find the actual ID + public static final int ID = 14; // TODO: find the actual ID } public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; + // public static final int LED_SPARK_ID = 0; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 3f93b48..2598bfa 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -60,7 +60,7 @@ public class Robot extends TimedRobot { } } - private MicroBot bot = new MicroBot(); + // private MicroBot bot = new MicroBot(); /** * This function is run when the robot is first started up and should be @@ -72,7 +72,7 @@ public class Robot extends TimedRobot { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - bot.setDefaultCommand(new AutoBalance(bot)); + // bot.setDefaultCommand(new AutoBalance(bot)); } /** @@ -146,7 +146,7 @@ public class Robot extends TimedRobot { } m_robotTime.startMatchTime(); - m_robotContainer.gyroRef.reset(); + // m_robotContainer.gyroRef.reset(); } /** @@ -154,9 +154,9 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); - SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); - SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); + // SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); + // SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); + // SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a42a463..03dcb0a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -32,15 +32,15 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); - private final LED m_robotLED = new LED(m_robotMap.LEDController); + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); + // private final LED m_robotLED = new LED(m_robotMap.LEDController); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - public RobotGyro gyroRef = m_robotMap.gyro; + // public RobotGyro gyroRef = m_robotMap.gyro; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -51,13 +51,20 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), - getDriverController().getLeftYAxis(), - -getDriverController().getRightXAxis(), false), m_robotSwerveDrive) + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-0.3*getDriverController().getLeftXAxis(), + 0.3*getDriverController().getLeftYAxis(), + -0.3*getDriverController().getRightXAxis(), false), m_robotSwerveDrive) ); + + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.runAllSteerMotors(0.2*getDriverController().getLeftYAxis()), m_robotSwerveDrive) + // ); + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.leftBack.angleMotor.set(0.2*getDriverController().getRightYAxis()), m_robotSwerveDrive) + // ); } /** @@ -69,14 +76,14 @@ public class RobotContainer { private void configureButtonBindings() { /* Driver Buttons */ - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> gyroRef.reset())); + // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> gyroRef.reset())); /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) - .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); + // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + // .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) + // .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); //New interupt button new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 08fcc1a..176e881 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -22,8 +22,8 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); - public RobotGyro gyro = new RobotGyro(m_pigeon2); + // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + // public RobotGyro gyro = new RobotGyro(m_pigeon2); public SwerveModule leftFront; @@ -38,7 +38,7 @@ public class RobotMap { } /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() { @@ -103,17 +103,17 @@ public class RobotMap { rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // config neutral deadband - leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // config magnet offset leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 676d800..4b3333f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -18,14 +18,14 @@ import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { - private SwerveModule leftFront; - private SwerveModule rightFront; - private SwerveModule leftBack; - private SwerveModule rightBack; + public SwerveModule leftFront; + public SwerveModule rightFront; + public SwerveModule leftBack; + public SwerveModule rightBack; private SwerveModule[] modules; - private RobotGyro gyro; + // private RobotGyro gyro; private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); @@ -34,21 +34,21 @@ public class SwerveDrive extends SubsystemBase { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - private SwerveDriveOdometry odometry = new SwerveDriveOdometry( - kinematics, - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - } - ); + // private SwerveDriveOdometry odometry = new SwerveDriveOdometry( + // kinematics, + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) { this.leftFront = leftFront; this.rightFront = rightFront; this.leftBack = leftBack; @@ -56,17 +56,23 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; - this.gyro = gyro; + for (SwerveModule m : this.modules) { + m.reset(); + } + + // this.gyro = gyro; } public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + double xSpeedMetersPerSecond = xSpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + double ySpeedMetersPerSecond = ySpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - SwerveModuleState[] states = kinematics.toSwerveModuleStates( - fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) - ); + // SwerveModuleState[] states = kinematics.toSwerveModuleStates( + // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) + // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) + //); + + SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)); SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); setModuleStates(states); @@ -87,64 +93,78 @@ public class SwerveDrive extends SubsystemBase { /** * Updates the odometry of the SwerveDrive. */ - public void updateOdometry() { - odometry.update( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - } - ); - } + // public void updateOdometry() { + // odometry.update( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); + // } /** * Gets the odometry of the SwerveDrive. * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). */ - public Pose2d getOdometry() { - return odometry.getPoseMeters(); - } + // public Pose2d getOdometry() { + // return odometry.getPoseMeters(); + // } /** * Sets the odometry of the SwerveDrive. * @param pose Pose to set the odometry to. */ - public void setOdometry(Pose2d pose) { - odometry.resetPosition( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - }, - pose - ); - } + // public void setOdometry(Pose2d pose) { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // pose + // ); + // } /** * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions. */ - public void resetOdometry() { - odometry.resetPosition( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - }, - new Pose2d() - ); + // public void resetOdometry() { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // new Pose2d() + // ); + // } + + public void runAllDriveMotors(double output) { + this.leftFront.driveMotor.set(output); + this.rightFront.driveMotor.set(output); + this.leftBack.driveMotor.set(output); + this.rightBack.driveMotor.set(output); + } + + public void runAllSteerMotors(double output) { + this.leftFront.angleMotor.set(output); + this.rightFront.angleMotor.set(output); + this.leftBack.angleMotor.set(output); + this.rightBack.angleMotor.set(output); } @Override public void periodic() { // This method will be called once per scheduler run - updateOdometry(); + // updateOdometry(); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index ea39ba4..4988964 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -21,8 +21,8 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; + public WPI_TalonFX driveMotor; + public WPI_TalonFX angleMotor; private CANCoder canCoder; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; @@ -78,7 +78,7 @@ public class SwerveModule extends SubsystemBase { * @return the angle of a SwerveModule as a Rotation2d */ public Rotation2d getAngle() { - // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. + // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } From b24bc6c6ab0bfcda1cb01998ff9c888c5b9723a5 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 21 Jan 2023 15:16:22 -0700 Subject: [PATCH 32/40] offsets (INCOMPLETE DO NOT TOUCH) --- src/main/java/frc4388/robot/Constants.java | 25 +++++--- .../java/frc4388/robot/RobotContainer.java | 32 +++++++--- src/main/java/frc4388/robot/RobotMap.java | 37 +++++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 59 +++++++++++++++---- .../robot/subsystems/SwerveModule.java | 28 +++++---- .../java/frc4388/utility/RobotEncoder.java | 23 ++++++++ 6 files changed, 149 insertions(+), 55 deletions(-) create mode 100644 src/main/java/frc4388/utility/RobotEncoder.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d5774a1..3862e4f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,6 +23,9 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { + + public static final double ROTATION_SPEED = 2.0; + public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; public static final int LEFT_FRONT_STEER_ID = 3; @@ -85,15 +88,21 @@ public final class Constants { public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value - // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // TODO: find the actual value - // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // TODO: find the actual value - // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // TODO: find the actual value - // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // TODO: find the actual value + // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2022 SwerveDrive values + + // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2023 translated values (don't work) + + // public static final double LEFT_FRONT_ENCODER_OFFSET = 0.0 + 90.0; // * 2023 experimentally-derived values (mostly work) + // public static final double RIGHT_FRONT_ENCODER_OFFSET = 0.0; // * 2023 experimentally-derived values (mostly work) + // public static final double LEFT_BACK_ENCODER_OFFSET = 0.0 + 24.0; // * 2023 experimentally-derived values (mostly work) + // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0 + 45.0 + 180.0; // * 2023 experimentally-derived values (mostly work) - public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // TODO: find the actual value - public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // TODO: find the actual value - public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // TODO: find the actual value - public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // TODO: find the actual value } public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 03dcb0a..60379ea 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -53,18 +53,26 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-0.3*getDriverController().getLeftXAxis(), - 0.3*getDriverController().getLeftYAxis(), - -0.3*getDriverController().getRightXAxis(), false), m_robotSwerveDrive) - ); + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-0.3*getDriverController().getLeftXAxis(), + // 0.3*getDriverController().getLeftYAxis(), + // -0.3*getDriverController().getRightXAxis(), false), m_robotSwerveDrive) + // ); // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.runAllSteerMotors(0.2*getDriverController().getLeftYAxis()), m_robotSwerveDrive) - // ); - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.leftBack.angleMotor.set(0.2*getDriverController().getRightYAxis()), m_robotSwerveDrive) + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, + // -0.1, + // 0, false), m_robotSwerveDrive) // ); + + m_robotSwerveDrive.setDefaultCommand( + new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + -0.3 * getDriverController().getLeftXAxis(), + 0.3 * getDriverController().getLeftYAxis(), + 0.3 * getDriverController().getRightXAxis(), + 0.3 * getDriverController().getRightYAxis(), + true), + m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); } /** @@ -85,6 +93,12 @@ public class RobotContainer { // .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) // .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileTrue(new RunCommand(() -> m_robotSwerveDrive.rotateCANCodersToAngle(0), m_robotSwerveDrive)); + + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetCANCoders(0), m_robotSwerveDrive)); + //New interupt button new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .onTrue(new InstantCommand()); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 176e881..040cf4b 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; +import frc4388.utility.RobotEncoder; import frc4388.utility.RobotGyro; /** @@ -48,18 +49,22 @@ public class RobotMap { public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + //public final RobotEncoder leftFrontEncoder = new RobotEncoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID, 268.900); public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + //public final RobotEncoder rightFrontEncoder = new RobotEncoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID, 266.700); public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + //public final RobotEncoder leftBackEncoder = new RobotEncoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID, 268.285); public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + //public final RobotEncoder rightBackEncoder = new RobotEncoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID, 86.965); void configureDriveMotors() { @@ -103,29 +108,29 @@ public class RobotMap { rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // config neutral deadband - // leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // config magnet offset - leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); - rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); - leftBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); - rightBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + // leftFrontEncoder.configMagnetOffset(90.0); //180.0); //270);//271.58203125);//SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + // rightFrontEncoder.configMagnetOffset(0.0); //180.0); //270);//271.58203125);//SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + // leftBackEncoder.configMagnetOffset(23.99414); //0.0); //90.0);//92.98828125);//SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + // rightBackEncoder.configMagnetOffset(225.0); //180.0); //270.0);//267.01171875);//SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); // initialize SwerveModules - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -180.0);//, 270);//, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -267.0);//, 90);//, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -245.0);//, 90);//, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -42.0);//, 273);//, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4b3333f..b066f9c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -12,7 +13,9 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; @@ -55,26 +58,43 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; - - for (SwerveModule m : this.modules) { - m.reset(); - } - - // this.gyro = gyro; } public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - double xSpeedMetersPerSecond = xSpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - double ySpeedMetersPerSecond = ySpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - + + Translation2d speed = new Translation2d(-xSpeed, ySpeed); + double mag = speed.getNorm(); + speed = speed.times(mag * speedAdjust); + + double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // SwerveModuleState[] states = kinematics.toSwerveModuleStates( // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) //); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)); + SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); - SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + setModuleStates(states); + } + + public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { + // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; + Translation2d speed = new Translation2d(-leftX, leftY); + speed = speed.times(speed.getNorm() * speedAdjust); + // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + double xSpeedMetersPerSecond = -speed.getX(); + double ySpeedMetersPerSecond = speed.getY(); + // chassisSpeeds = fieldRelative + // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + + ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + + SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -83,6 +103,7 @@ public class SwerveDrive extends SubsystemBase { * @param desiredStates Array of module states to set. */ public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; @@ -161,10 +182,26 @@ public class SwerveDrive extends SubsystemBase { this.rightBack.angleMotor.set(output); } + public void rotateCANCodersToAngle(double angle) { + for (SwerveModule module : this.modules) { + module.rotateToAngle(angle); + } + } + + public void resetCANCoders(double position) { + for (SwerveModule module : this.modules) { + module.reset(position); + } + } + @Override public void periodic() { // This method will be called once per scheduler run // updateOdometry(); + SmartDashboard.putNumber("LeftFront CC", this.modules[0].getAngle().getDegrees()); + SmartDashboard.putNumber("RightFront CC", this.modules[1].getAngle().getDegrees()); + SmartDashboard.putNumber("LeftBack CC", this.modules[2].getAngle().getDegrees()); + SmartDashboard.putNumber("RightBack CC", this.modules[3].getAngle().getDegrees()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 4988964..dff4429 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -19,19 +19,21 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.RobotEncoder; public class SwerveModule extends SubsystemBase { public WPI_TalonFX driveMotor; public WPI_TalonFX angleMotor; - private CANCoder canCoder; + // private CANCoder canCoder; + private CANCoder encoder; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; /** Creates a new SwerveModule. */ - public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { + public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, /*CANCoder canCoder*/CANCoder encoder, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; - this.canCoder = canCoder; + this.encoder = encoder; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); angleConfig.slot0.kP = swerveGains.kP; @@ -39,14 +41,16 @@ public class SwerveModule extends SubsystemBase { angleConfig.slot0.kD = swerveGains.kD; // use the CANcoder as the remote sensor for the primary TalonFX PID - angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID(); + angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleConfig); CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); canCoderConfig.magnetOffsetDegrees = offset; - canCoder.configAllSettings(canCoderConfig); + encoder.configAllSettings(canCoderConfig); + + // canCoderConfig.magnetOffsetDegrees = 270; } /** @@ -70,7 +74,7 @@ public class SwerveModule extends SubsystemBase { * @return the CANcoder of the SwerveModule */ public CANCoder getEncoder() { - return this.canCoder; + return this.encoder; } /** @@ -79,7 +83,7 @@ public class SwerveModule extends SubsystemBase { */ public Rotation2d getAngle() { // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. - return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); + return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); } public void stop() { @@ -126,15 +130,17 @@ public class SwerveModule extends SubsystemBase { double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; // convert the CANCoder from its position reading to ticks - double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient(); + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); + driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); } - public void reset() { - canCoder.setPositionToAbsolute(); + public void reset(double position) { + // encoder.setPosition(position); + encoder.setPositionToAbsolute(); + // canCoder.setPosition(1024); } public double getCurrent() { diff --git a/src/main/java/frc4388/utility/RobotEncoder.java b/src/main/java/frc4388/utility/RobotEncoder.java new file mode 100644 index 0000000..2e397ac --- /dev/null +++ b/src/main/java/frc4388/utility/RobotEncoder.java @@ -0,0 +1,23 @@ +// 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. + +package frc4388.utility; + +import com.ctre.phoenix.sensors.CANCoder; + +/** Add your docs here. */ +public class RobotEncoder extends CANCoder { + private double offset; + + public RobotEncoder(int id, double offset) { + super(id); + + this.offset = offset; + } + + @Override + public double getAbsolutePosition() { + return super.getAbsolutePosition() - this.offset; + } +} From 2376d2636fd2a6396fef418309626286faf303d3 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 24 Jan 2023 17:57:48 -0700 Subject: [PATCH 33/40] wpilib example swerve --- src/main/java/frc4388/robot/Robot.java | 17 ++++++++++++++++ .../java/frc4388/robot/RobotContainer.java | 20 ++++++++++--------- .../frc4388/robot/subsystems/SwerveDrive.java | 15 +++++++++++++- 3 files changed, 42 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2598bfa..31a45f5 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -11,11 +11,15 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.sensors.WPI_Pigeon2; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AutoBalance; import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; @@ -60,6 +64,10 @@ public class Robot extends TimedRobot { } } + private final SlewRateLimiter xLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter yLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3); + // private MicroBot bot = new MicroBot(); /** @@ -157,6 +165,15 @@ public class Robot extends TimedRobot { // SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); // SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); // SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); + this.drive(false); + } + + private void drive(boolean fieldRelative) { + final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis(), 0.02) * Units.feetToMeters(Math.PI)); + + m_robotContainer.m_robotSwerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 60379ea..3180343 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -32,7 +32,7 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); // private final LED m_robotLED = new LED(m_robotMap.LEDController); @@ -65,16 +65,18 @@ public class RobotContainer { // 0, false), m_robotSwerveDrive) // ); - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput( - -0.3 * getDriverController().getLeftXAxis(), - 0.3 * getDriverController().getLeftYAxis(), - 0.3 * getDriverController().getRightXAxis(), - 0.3 * getDriverController().getRightYAxis(), - true), - m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + // -0.3 * getDriverController().getLeftXAxis(), + // 0.3 * getDriverController().getLeftYAxis(), + // 0.3 * getDriverController().getRightXAxis(), + // 0.3 * getDriverController().getRightYAxis(), + // true), + // m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); } + + /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index b066f9c..f35fa57 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,6 +4,7 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -28,7 +29,9 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule[] modules; - // private RobotGyro gyro; + + + // private RobotGyro gyro3 private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); @@ -98,6 +101,16 @@ public class SwerveDrive extends SubsystemBase { setModuleStates(states); } + // ! experimental WPILib swerve drive example + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + // SwerveModuleState[] states = kinematics.toSwerveModuleStates( + // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) + // : new ChassisSpeeds(xSpeed, ySpeed, rot) + // ); + SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED)); + setModuleStates(states); + } + /** * Set each module of the swerve drive to the corresponding desired state. * @param desiredStates Array of module states to set. From e6b2d2f48603f7075bf696586ef2dc602c509b17 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 31 Jan 2023 19:24:12 -0700 Subject: [PATCH 34/40] FINAL OFFSETS ARE IN CODE --- src/main/java/frc4388/robot/Robot.java | 6 ++--- .../java/frc4388/robot/RobotContainer.java | 22 ++++++++++++++----- src/main/java/frc4388/robot/RobotMap.java | 14 ++++++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 6 +++-- .../robot/subsystems/SwerveModule.java | 10 +++++++-- 5 files changed, 42 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 31a45f5..a54b4b4 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -169,9 +169,9 @@ public class Robot extends TimedRobot { } private void drive(boolean fieldRelative) { - final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis(), 0.02) * Units.feetToMeters(Math.PI)); + final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis() * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis() * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis() * 0.3, 0.02) * Units.feetToMeters(Math.PI)); m_robotContainer.m_robotSwerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3180343..01da8cf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,11 @@ package frc4388.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -75,7 +80,6 @@ public class RobotContainer { // m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); } - /** * Use this method to define your button->command mappings. Buttons can be @@ -95,11 +99,14 @@ public class RobotContainer { // .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) // .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileTrue(new RunCommand(() -> m_robotSwerveDrive.rotateCANCodersToAngle(0), m_robotSwerveDrive)); + // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + // .whileTrue(new RunCommand(() -> m_robotSwerveDrive.rotateCANCodersToAngle(0), m_robotSwerveDrive)); - new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetCANCoders(0), m_robotSwerveDrive)); + // new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetCANCoders(0), m_robotSwerveDrive)); + + // new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + // .onTrue() //New interupt button new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -113,6 +120,11 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // no auto + // TrajectoryConfig trajConfig = new TrajectoryConfig(1, 1).setKinematics(m_robotSwerveDrive.getKinematics()); // TODO: make these AutoConstants + + // Trajectory traj = TrajectoryGenerator.generateTrajectory( + // new Pose2d(0, 0, new Rotation2d(0)), null, null, trajConfig); + return new InstantCommand(); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 040cf4b..0f5e1ae 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -127,10 +127,16 @@ public class RobotMap { // rightBackEncoder.configMagnetOffset(225.0); //180.0); //270.0);//267.01171875);//SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); // initialize SwerveModules - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -180.0);//, 270);//, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -267.0);//, 90);//, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -245.0);//, 90);//, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -42.0);//, 273);//, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder);//, -180.0);//, 270);//, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder);//, -267.0);//, 90);//, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder);//, -245.0);//, 90);//, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder);//, -42.0);//, 273);//, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + + // FINAL OFFSETS (I THINK) + // LEFT FRONT: -181.230469 + // RIGHT FRONT: -270.615234 + // LEFT BACK: -240.029297 + // RIGHT BACK: -40.869142 } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f35fa57..bcd0741 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -29,8 +29,6 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule[] modules; - - // private RobotGyro gyro3 private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); @@ -207,6 +205,10 @@ public class SwerveDrive extends SubsystemBase { } } + public SwerveDriveKinematics getKinematics() { + return this.kinematics; + } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index dff4429..95fdaa2 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; @@ -30,7 +31,7 @@ public class SwerveModule extends SubsystemBase { public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; /** Creates a new SwerveModule. */ - public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, /*CANCoder canCoder*/CANCoder encoder, double offset) { + public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder) {//, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; @@ -47,9 +48,11 @@ public class SwerveModule extends SubsystemBase { angleMotor.configAllSettings(angleConfig); CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); - canCoderConfig.magnetOffsetDegrees = offset; + // canCoderConfig.magnetOffsetDegrees = offset; encoder.configAllSettings(canCoderConfig); + // driveMotor.config_kP(0, 0.4); + // canCoderConfig.magnetOffsetDegrees = 270; } @@ -134,7 +137,10 @@ public class SwerveModule extends SubsystemBase { angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + double inchesPerSecond = Units.metersToInches(state.speedMetersPerSecond); + driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); + // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); } public void reset(double position) { From 129b53089ec15e74fe18e69551ad6d255bcf5138 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 2 Feb 2023 17:22:44 -0700 Subject: [PATCH 35/40] delt --- src/main/java/frc4388/robot/RobotMap.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 0f5e1ae..9679cb3 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -127,10 +127,11 @@ public class RobotMap { // rightBackEncoder.configMagnetOffset(225.0); //180.0); //270.0);//267.01171875);//SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); // initialize SwerveModules - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder);//, -180.0);//, 270);//, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder);//, -267.0);//, 90);//, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder);//, -245.0);//, 90);//, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder);//, -42.0);//, 273);//, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder); + // FINAL OFFSETS (I THINK) From 52ef32ec21c812d1efab2bb243da8b00e9cbfcc9 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 2 Feb 2023 17:55:52 -0700 Subject: [PATCH 36/40] drive done --- src/main/java/frc4388/robot/Robot.java | 13 --- .../java/frc4388/robot/RobotContainer.java | 8 ++ .../robot/commands/DriveWithInput.java | 79 +++++++++++++++++++ .../frc4388/robot/subsystems/SwerveDrive.java | 60 +++++++------- 4 files changed, 117 insertions(+), 43 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveWithInput.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index a54b4b4..5813025 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -64,10 +64,6 @@ public class Robot extends TimedRobot { } } - private final SlewRateLimiter xLimiter = new SlewRateLimiter(3); - private final SlewRateLimiter yLimiter = new SlewRateLimiter(3); - private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3); - // private MicroBot bot = new MicroBot(); /** @@ -165,15 +161,6 @@ public class Robot extends TimedRobot { // SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); // SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); // SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); - this.drive(false); - } - - private void drive(boolean fieldRelative) { - final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis() * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis() * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis() * 0.3, 0.02) * Units.feetToMeters(Math.PI)); - - m_robotContainer.m_robotSwerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 01da8cf..6eaf51f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.DriveWithInput; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -53,6 +54,12 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); + m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, + () -> getDriverController().getLeftXAxis(), + () -> getDriverController().getLeftYAxis(), + () -> getDriverController().getRightXAxis(), + false)); + /* Default Commands */ // drives the robot with a two-axis input from the driver controller // continually sends updates to the Blinkin LED controller to keep the lights on @@ -78,6 +85,7 @@ public class RobotContainer { // 0.3 * getDriverController().getRightYAxis(), // true), // m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + } diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java new file mode 100644 index 0000000..784536d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveWithInput.java @@ -0,0 +1,79 @@ +// 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. + +package frc4388.robot.commands; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveDrive; + +public class DriveWithInput extends CommandBase { + /** Creates a new DriveWithInput. */ + private final SwerveDrive swerve; + + private final Supplier xSpeed; + private final Supplier ySpeed; + private final Supplier rot; + private final boolean fieldRelative; + + private final SlewRateLimiter xLimiter, yLimiter, rotLimiter; + + + + + public DriveWithInput(SwerveDrive swerve, Supplier xSpeed, Supplier ySpeed, Supplier rot, boolean fieldRelative) { + // Use addRequirements() here to declare subsystem dependencies. + this.swerve = swerve; + this.xSpeed = xSpeed; + this.ySpeed = ySpeed; + this.rot = rot; + this.fieldRelative = fieldRelative; + + this.xLimiter = new SlewRateLimiter(3); + this.yLimiter = new SlewRateLimiter(3); + this.rotLimiter = new SlewRateLimiter(3); + + addRequirements(swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double x = xSpeed.get(); + double y = ySpeed.get(); + double r = rot.get(); + + x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(Math.PI)); + + swerve.drive(x, y, r, fieldRelative); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + System.out.println("----------------------------------------------------------------"); + System.out.println("DriveWithInput ended"); + System.out.println("Interrupted: " + interrupted); + System.out.println("----------------------------------------------------------------"); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index bcd0741..b1ca512 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -61,43 +61,43 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + // public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - Translation2d speed = new Translation2d(-xSpeed, ySpeed); - double mag = speed.getNorm(); - speed = speed.times(mag * speedAdjust); + // Translation2d speed = new Translation2d(-xSpeed, ySpeed); + // double mag = speed.getNorm(); + // speed = speed.times(mag * speedAdjust); - double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - // SwerveModuleState[] states = kinematics.toSwerveModuleStates( - // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) - // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) - //); + // double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + // double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + // // SwerveModuleState[] states = kinematics.toSwerveModuleStates( + // // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) + // // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) + // //); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); + // SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); - setModuleStates(states); - } + // setModuleStates(states); + // } - public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { - // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; - Translation2d speed = new Translation2d(-leftX, leftY); - speed = speed.times(speed.getNorm() * speedAdjust); - // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); - // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); - double xSpeedMetersPerSecond = -speed.getX(); - double ySpeedMetersPerSecond = speed.getY(); - // chassisSpeeds = fieldRelative - // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + // public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { + // // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; + // Translation2d speed = new Translation2d(-leftX, leftY); + // speed = speed.times(speed.getNorm() * speedAdjust); + // // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + // // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + // // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + // double xSpeedMetersPerSecond = -speed.getX(); + // double ySpeedMetersPerSecond = speed.getY(); + // // chassisSpeeds = fieldRelative + // // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + // // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + // // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); - ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + // ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); - setModuleStates(states); - } + // SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); + // setModuleStates(states); + // } // ! experimental WPILib swerve drive example public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { From 46f39c10958b5ce6ddd51906280df5a8f1296ee1 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 2 Feb 2023 19:10:08 -0700 Subject: [PATCH 37/40] cancoder offsets --- config/LeftBackCANCoder.json | 47 +++++++++++++++++++ config/LeftFrontCANCoder.json | 47 +++++++++++++++++++ config/RightBackCANCoder.json | 47 +++++++++++++++++++ config/RightFrontCANCoder.json | 47 +++++++++++++++++++ src/main/java/frc4388/robot/RobotMap.java | 11 ++--- .../robot/commands/DriveWithInput.java | 2 +- .../robot/subsystems/SwerveModule.java | 7 +-- 7 files changed, 197 insertions(+), 11 deletions(-) create mode 100644 config/LeftBackCANCoder.json create mode 100644 config/LeftFrontCANCoder.json create mode 100644 config/RightBackCANCoder.json create mode 100644 config/RightFrontCANCoder.json diff --git a/config/LeftBackCANCoder.json b/config/LeftBackCANCoder.json new file mode 100644 index 0000000..69ab1f3 --- /dev/null +++ b/config/LeftBackCANCoder.json @@ -0,0 +1,47 @@ +{ + "Configs": [ + { + "Name": "Integrated Sensor", + "Type": "IntegratedSensor", + "Description": "Configures how the sensor is processed.", + "Ordinal": 0, + "Values": { + "Absolute Sensor Range": 0, + "Magnet Offset (deg)": -240.0293, + "Sensor Direction": false, + "Sensor Initialization Strategy": 0 + } + }, + { + "Name": "Unit Coefficient And String", + "Type": "UnitCoeffGroup", + "Description": "Describes the units to report in API and Self-Test.", + "Ordinal": 0, + "Values": { + "Sensor Coefficient": 0.087890625, + "Sensor Time Base": 1, + "Unit String": "deg" + } + }, + { + "Name": "Custom Params", + "Type": "CustomParams", + "Description": "These are arbitrary values not used by the firmware.", + "Ordinal": 0, + "Values": { + "Custom Param 0": 0, + "Custom Param 1": 0 + } + }, + { + "Name": "Advanced Sensor and Meas", + "Type": "AdvancedSensorMeas", + "Description": "Group of configs relating to advanced sensor features and filters", + "Ordinal": 0, + "Values": { + "Velocity Period": 100, + "Velocity Window": 64 + } + } + ] +} \ No newline at end of file diff --git a/config/LeftFrontCANCoder.json b/config/LeftFrontCANCoder.json new file mode 100644 index 0000000..1a5edcd --- /dev/null +++ b/config/LeftFrontCANCoder.json @@ -0,0 +1,47 @@ +{ + "Configs": [ + { + "Name": "Integrated Sensor", + "Type": "IntegratedSensor", + "Description": "Configures how the sensor is processed.", + "Ordinal": 0, + "Values": { + "Absolute Sensor Range": 0, + "Magnet Offset (deg)": -181.230469, + "Sensor Direction": false, + "Sensor Initialization Strategy": 0 + } + }, + { + "Name": "Unit Coefficient And String", + "Type": "UnitCoeffGroup", + "Description": "Describes the units to report in API and Self-Test.", + "Ordinal": 0, + "Values": { + "Sensor Coefficient": 0.087890625, + "Sensor Time Base": 1, + "Unit String": "deg" + } + }, + { + "Name": "Custom Params", + "Type": "CustomParams", + "Description": "These are arbitrary values not used by the firmware.", + "Ordinal": 0, + "Values": { + "Custom Param 0": 0, + "Custom Param 1": 0 + } + }, + { + "Name": "Advanced Sensor and Meas", + "Type": "AdvancedSensorMeas", + "Description": "Group of configs relating to advanced sensor features and filters", + "Ordinal": 0, + "Values": { + "Velocity Period": 100, + "Velocity Window": 64 + } + } + ] +} \ No newline at end of file diff --git a/config/RightBackCANCoder.json b/config/RightBackCANCoder.json new file mode 100644 index 0000000..236a112 --- /dev/null +++ b/config/RightBackCANCoder.json @@ -0,0 +1,47 @@ +{ + "Configs": [ + { + "Name": "Integrated Sensor", + "Type": "IntegratedSensor", + "Description": "Configures how the sensor is processed.", + "Ordinal": 0, + "Values": { + "Absolute Sensor Range": 0, + "Magnet Offset (deg)": -40.86914, + "Sensor Direction": false, + "Sensor Initialization Strategy": 0 + } + }, + { + "Name": "Unit Coefficient And String", + "Type": "UnitCoeffGroup", + "Description": "Describes the units to report in API and Self-Test.", + "Ordinal": 0, + "Values": { + "Sensor Coefficient": 0.087890625, + "Sensor Time Base": 1, + "Unit String": "deg" + } + }, + { + "Name": "Custom Params", + "Type": "CustomParams", + "Description": "These are arbitrary values not used by the firmware.", + "Ordinal": 0, + "Values": { + "Custom Param 0": 0, + "Custom Param 1": 0 + } + }, + { + "Name": "Advanced Sensor and Meas", + "Type": "AdvancedSensorMeas", + "Description": "Group of configs relating to advanced sensor features and filters", + "Ordinal": 0, + "Values": { + "Velocity Period": 100, + "Velocity Window": 64 + } + } + ] +} \ No newline at end of file diff --git a/config/RightFrontCANCoder.json b/config/RightFrontCANCoder.json new file mode 100644 index 0000000..ec93eb5 --- /dev/null +++ b/config/RightFrontCANCoder.json @@ -0,0 +1,47 @@ +{ + "Configs": [ + { + "Name": "Integrated Sensor", + "Type": "IntegratedSensor", + "Description": "Configures how the sensor is processed.", + "Ordinal": 0, + "Values": { + "Absolute Sensor Range": 0, + "Magnet Offset (deg)": -270.615234, + "Sensor Direction": false, + "Sensor Initialization Strategy": 0 + } + }, + { + "Name": "Unit Coefficient And String", + "Type": "UnitCoeffGroup", + "Description": "Describes the units to report in API and Self-Test.", + "Ordinal": 0, + "Values": { + "Sensor Coefficient": 0.087890625, + "Sensor Time Base": 1, + "Unit String": "deg" + } + }, + { + "Name": "Custom Params", + "Type": "CustomParams", + "Description": "These are arbitrary values not used by the firmware.", + "Ordinal": 0, + "Values": { + "Custom Param 0": 0, + "Custom Param 1": 0 + } + }, + { + "Name": "Advanced Sensor and Meas", + "Type": "AdvancedSensorMeas", + "Description": "Group of configs relating to advanced sensor features and filters", + "Ordinal": 0, + "Values": { + "Velocity Period": 100, + "Velocity Window": 64 + } + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9679cb3..3341dd5 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -127,14 +127,11 @@ public class RobotMap { // rightBackEncoder.configMagnetOffset(225.0); //180.0); //270.0);//267.01171875);//SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); // initialize SwerveModules - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder); - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder); + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); - - - // FINAL OFFSETS (I THINK) // LEFT FRONT: -181.230469 // RIGHT FRONT: -270.615234 // LEFT BACK: -240.029297 diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java index 784536d..1fcaccc 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithInput.java +++ b/src/main/java/frc4388/robot/commands/DriveWithInput.java @@ -57,7 +57,7 @@ public class DriveWithInput extends CommandBase { x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(Math.PI)); + r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI)); swerve.drive(x, y, r, fieldRelative); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 95fdaa2..082991d 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -31,7 +31,7 @@ public class SwerveModule extends SubsystemBase { public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; /** Creates a new SwerveModule. */ - public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder) {//, double offset) { + public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; @@ -47,9 +47,10 @@ public class SwerveModule extends SubsystemBase { angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleConfig); - CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); + // CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); // canCoderConfig.magnetOffsetDegrees = offset; - encoder.configAllSettings(canCoderConfig); + // encoder.configAllSettings(canCoderConfig); + encoder.configMagnetOffset(offset); // driveMotor.config_kP(0, 0.4); From 07df881b2a5059e809aa265801493c7c928f97c3 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 2 Feb 2023 19:11:10 -0700 Subject: [PATCH 38/40] change --- .../java/frc4388/robot/RobotContainer.java | 3 +++ src/main/java/frc4388/robot/RobotMap.java | 4 ++-- .../frc4388/robot/commands/AutoBalance.java | 21 ++++++++++++------- 3 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6eaf51f..bb7afe4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.DriveWithInput; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; @@ -115,6 +116,8 @@ public class RobotContainer { // new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) // .onTrue() + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); //New interupt button new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9679cb3..d4dad2b 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -23,8 +23,8 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); - // public RobotGyro gyro = new RobotGyro(m_pigeon2); + private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + public RobotGyro gyro = new RobotGyro(m_pigeon2); public SwerveModule leftFront; diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index a8cff7d..c0c95f8 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -5,37 +5,42 @@ package frc4388.robot.commands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.Robot; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.RobotGyro; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutoBalance extends PelvicInflammatoryDisease { - Robot.MicroBot bot; + RobotGyro gyro; + SwerveDrive drive; /** Creates a new AutoBalanceTF2. */ - public AutoBalance(Robot.MicroBot bot) { + public AutoBalance(RobotGyro gyro, SwerveDrive drive) { super(.7, .1, 15, 0); - addRequirements(bot); - this.bot = bot; + addRequirements(drive); } @Override public double getError() { - return bot.gyro.getPitch(); + var pitch = gyro.getPitch(); + SmartDashboard.putNumber("pitch", pitch); + return pitch; } @Override public void runWithOutput(double output) { double out2 = MathUtil.clamp(output / 40, -.5, .5); - if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0; - bot.setOutput(out2); + if (Math.abs(gyro.getPitch()) < 3) out2 = 0; + drive.drive(out2, 0, 0, false); } @Override public void initialize() { super.initialize(); - this.bot.gyro.reset(); + this.gyro.reset(); } // Returns true when the command should end. From f9b89de6c5e1e80bc118401d391ae0bfd8577be3 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 2 Feb 2023 19:43:47 -0700 Subject: [PATCH 39/40] pid done --- src/main/java/frc4388/robot/Robot.java | 6 +++--- src/main/java/frc4388/robot/RobotContainer.java | 7 ++++--- src/main/java/frc4388/robot/commands/AutoBalance.java | 8 ++++++-- src/main/java/frc4388/robot/subsystems/SwerveModule.java | 2 +- 4 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 5813025..78c6890 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -158,9 +158,9 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - // SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); - // SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); - // SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); + // SmartDashboard.putNumber("yaw", m_robotContainer.m_robotMap.gyro.getAngle()); + SmartDashboard.putNumber("Robot.java Pitch", m_robotContainer.m_robotMap.gyro.getPitch()); + // SmartDashboard.putNumber("roll", m_robotContainer.m_robotMap.gyro.getRoll()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bb7afe4..2a2e90c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,7 @@ import frc4388.utility.controller.XboxController; */ public class RobotContainer { /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); + public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); @@ -99,8 +99,9 @@ public class RobotContainer { private void configureButtonBindings() { /* Driver Buttons */ - // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> gyroRef.reset())); + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset())); + // .onFalse() /* Operator Buttons */ // activates "Lit Mode" diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index c0c95f8..aeb4610 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -19,7 +19,11 @@ public class AutoBalance extends PelvicInflammatoryDisease { /** Creates a new AutoBalanceTF2. */ public AutoBalance(RobotGyro gyro, SwerveDrive drive) { - super(.7, .1, 15, 0); + super(1.0, 0, 0, 0); + + this.gyro = gyro; + this.drive = drive; + addRequirements(drive); } @@ -40,7 +44,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { @Override public void initialize() { super.initialize(); - this.gyro.reset(); + // this.gyro.reset(); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 082991d..e8dd497 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -140,7 +140,7 @@ public class SwerveModule extends SubsystemBase { double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); double inchesPerSecond = Units.metersToInches(state.speedMetersPerSecond); - driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); + driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); } From 784fd3f3b65ceb6d25a1b77b89a4e3025f01f4ea Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 2 Feb 2023 20:54:12 -0700 Subject: [PATCH 40/40] cleanup before master merge --- src/main/java/frc4388/robot/Robot.java | 54 ------------- .../java/frc4388/robot/RobotContainer.java | 57 +------------ src/main/java/frc4388/robot/RobotMap.java | 18 ----- .../frc4388/robot/commands/AutoBalance.java | 4 - .../robot/commands/DriveWithInput.java | 8 +- .../commands/PelvicInflammatoryDisease.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 80 +------------------ .../robot/subsystems/SwerveModule.java | 16 +--- .../java/frc4388/utility/RobotEncoder.java | 23 ------ 9 files changed, 9 insertions(+), 253 deletions(-) delete mode 100644 src/main/java/frc4388/utility/RobotEncoder.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 78c6890..8fc1ca3 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,21 +7,9 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.sensors.WPI_Pigeon2; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.commands.AutoBalance; -import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; /** @@ -37,34 +25,6 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - public static class MicroBot extends SubsystemBase { - public WPI_Pigeon2 pigeon = new WPI_Pigeon2(14); - public RobotGyro gyro = new RobotGyro(pigeon); - - public TalonSRX frontLeft = new TalonSRX(2); - public TalonSRX backLeft = new TalonSRX(3); - public TalonSRX backRight = new TalonSRX(5); - public TalonSRX frontRight = new TalonSRX(4); - - public MicroBot() { - frontRight.configFactoryDefault(); - frontLeft.configFactoryDefault(); - backLeft.configFactoryDefault(); - backRight.configFactoryDefault(); - - frontLeft.setInverted(true); - backLeft.setInverted(true); - } - - public void setOutput(double output) { - frontRight.set(ControlMode.PercentOutput, output); - frontLeft.set(ControlMode.PercentOutput, output); - backLeft.set(ControlMode.PercentOutput, output); - backRight.set(ControlMode.PercentOutput, output); - } - } - - // private MicroBot bot = new MicroBot(); /** * This function is run when the robot is first started up and should be @@ -75,8 +35,6 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - - // bot.setDefaultCommand(new AutoBalance(bot)); } /** @@ -118,13 +76,6 @@ public class Robot extends TimedRobot { public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } - */ - // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); @@ -149,8 +100,6 @@ public class Robot extends TimedRobot { m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); - - // m_robotContainer.gyroRef.reset(); } /** @@ -158,9 +107,6 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - // SmartDashboard.putNumber("yaw", m_robotContainer.m_robotMap.gyro.getAngle()); - SmartDashboard.putNumber("Robot.java Pitch", m_robotContainer.m_robotMap.gyro.getPitch()); - // SmartDashboard.putNumber("roll", m_robotContainer.m_robotMap.gyro.getRoll()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2a2e90c..031cd46 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,23 +7,14 @@ package frc4388.robot; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.DriveWithInput; -import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.LEDPatterns; -import frc4388.utility.RobotGyro; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -47,45 +38,22 @@ public class RobotContainer { private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - // public RobotGyro gyroRef = m_robotMap.gyro; - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); + /* Default Commands */ + m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, () -> getDriverController().getLeftXAxis(), () -> getDriverController().getLeftYAxis(), () -> getDriverController().getRightXAxis(), false)); - - /* Default Commands */ - // drives the robot with a two-axis input from the driver controller - // continually sends updates to the Blinkin LED controller to keep the lights on + // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-0.3*getDriverController().getLeftXAxis(), - // 0.3*getDriverController().getLeftYAxis(), - // -0.3*getDriverController().getRightXAxis(), false), m_robotSwerveDrive) - // ); - - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, - // -0.1, - // 0, false), m_robotSwerveDrive) - // ); - - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput( - // -0.3 * getDriverController().getLeftXAxis(), - // 0.3 * getDriverController().getLeftYAxis(), - // 0.3 * getDriverController().getRightXAxis(), - // 0.3 * getDriverController().getRightYAxis(), - // true), - // m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); } @@ -104,23 +72,11 @@ public class RobotContainer { // .onFalse() /* Operator Buttons */ - // activates "Lit Mode" - // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - // .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) - // .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); - // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - // .whileTrue(new RunCommand(() -> m_robotSwerveDrive.rotateCANCodersToAngle(0), m_robotSwerveDrive)); - - // new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetCANCoders(0), m_robotSwerveDrive)); - - // new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - // .onTrue() new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - //New interupt button + // interrupt button new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .onTrue(new InstantCommand()); } @@ -131,11 +87,6 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // no auto - // TrajectoryConfig trajConfig = new TrajectoryConfig(1, 1).setKinematics(m_robotSwerveDrive.getKinematics()); // TODO: make these AutoConstants - - // Trajectory traj = TrajectoryGenerator.generateTrajectory( - // new Pose2d(0, 0, new Rotation2d(0)), null, null, trajConfig); return new InstantCommand(); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index c33a95f..93a114d 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,11 +11,8 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; -import frc4388.utility.RobotEncoder; import frc4388.utility.RobotGyro; /** @@ -49,22 +46,18 @@ public class RobotMap { public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - //public final RobotEncoder leftFrontEncoder = new RobotEncoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID, 268.900); public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - //public final RobotEncoder rightFrontEncoder = new RobotEncoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID, 266.700); public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - //public final RobotEncoder leftBackEncoder = new RobotEncoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID, 268.285); public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); - //public final RobotEncoder rightBackEncoder = new RobotEncoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID, 86.965); void configureDriveMotors() { @@ -120,21 +113,10 @@ public class RobotMap { rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // config magnet offset - // leftFrontEncoder.configMagnetOffset(90.0); //180.0); //270);//271.58203125);//SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); - // rightFrontEncoder.configMagnetOffset(0.0); //180.0); //270);//271.58203125);//SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); - // leftBackEncoder.configMagnetOffset(23.99414); //0.0); //90.0);//92.98828125);//SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); - // rightBackEncoder.configMagnetOffset(225.0); //180.0); //270.0);//267.01171875);//SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); - // initialize SwerveModules this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); - - // LEFT FRONT: -181.230469 - // RIGHT FRONT: -270.615234 - // LEFT BACK: -240.029297 - // RIGHT BACK: -40.869142 } } diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index aeb4610..374de0a 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -6,13 +6,9 @@ package frc4388.robot.commands; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.Robot; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.RobotGyro; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutoBalance extends PelvicInflammatoryDisease { RobotGyro gyro; SwerveDrive drive; diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java index 1fcaccc..3b4d638 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithInput.java +++ b/src/main/java/frc4388/robot/commands/DriveWithInput.java @@ -4,19 +4,17 @@ package frc4388.robot.commands; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveDrive; public class DriveWithInput extends CommandBase { - /** Creates a new DriveWithInput. */ + private final SwerveDrive swerve; private final Supplier xSpeed; @@ -26,9 +24,7 @@ public class DriveWithInput extends CommandBase { private final SlewRateLimiter xLimiter, yLimiter, rotLimiter; - - - + /** Creates a new DriveWithInput. */ public DriveWithInput(SwerveDrive swerve, Supplier xSpeed, Supplier ySpeed, Supplier rot, boolean fieldRelative) { // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 1ab08d5..fb6a871 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -11,7 +11,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { protected Gains gains; private double output = 0; - /** Creates a new PelvicInflamitoryDisease. */ + /** Creates a new PelvicInflammatoryDisease. */ public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) { gains = new Gains(kp, ki, kd, kf, 0); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index b1ca512..d93f892 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,21 +4,13 @@ package frc4388.robot.subsystems; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { @@ -29,8 +21,6 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule[] modules; - // private RobotGyro gyro3 - private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); @@ -61,45 +51,7 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - // public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - - // Translation2d speed = new Translation2d(-xSpeed, ySpeed); - // double mag = speed.getNorm(); - // speed = speed.times(mag * speedAdjust); - - // double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - // double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - // // SwerveModuleState[] states = kinematics.toSwerveModuleStates( - // // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) - // // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) - // //); - - // SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); - - // setModuleStates(states); - // } - - // public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { - // // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; - // Translation2d speed = new Translation2d(-leftX, leftY); - // speed = speed.times(speed.getNorm() * speedAdjust); - // // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - // // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); - // // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); - // double xSpeedMetersPerSecond = -speed.getX(); - // double ySpeedMetersPerSecond = speed.getY(); - // // chassisSpeeds = fieldRelative - // // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - // // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - // // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); - - // ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); - - // SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); - // setModuleStates(states); - // } - - // ! experimental WPILib swerve drive example + // WPILib swerve drive example public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { // SwerveModuleState[] states = kinematics.toSwerveModuleStates( // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) @@ -179,32 +131,6 @@ public class SwerveDrive extends SubsystemBase { // ); // } - public void runAllDriveMotors(double output) { - this.leftFront.driveMotor.set(output); - this.rightFront.driveMotor.set(output); - this.leftBack.driveMotor.set(output); - this.rightBack.driveMotor.set(output); - } - - public void runAllSteerMotors(double output) { - this.leftFront.angleMotor.set(output); - this.rightFront.angleMotor.set(output); - this.leftBack.angleMotor.set(output); - this.rightBack.angleMotor.set(output); - } - - public void rotateCANCodersToAngle(double angle) { - for (SwerveModule module : this.modules) { - module.rotateToAngle(angle); - } - } - - public void resetCANCoders(double position) { - for (SwerveModule module : this.modules) { - module.reset(position); - } - } - public SwerveDriveKinematics getKinematics() { return this.kinematics; } @@ -213,10 +139,6 @@ public class SwerveDrive extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run // updateOdometry(); - SmartDashboard.putNumber("LeftFront CC", this.modules[0].getAngle().getDegrees()); - SmartDashboard.putNumber("RightFront CC", this.modules[1].getAngle().getDegrees()); - SmartDashboard.putNumber("LeftBack CC", this.modules[2].getAngle().getDegrees()); - SmartDashboard.putNumber("RightBack CC", this.modules[3].getAngle().getDegrees()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index e8dd497..4bbbefb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -10,22 +10,18 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.CANCoderConfiguration; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; -import frc4388.utility.RobotEncoder; public class SwerveModule extends SubsystemBase { public WPI_TalonFX driveMotor; public WPI_TalonFX angleMotor; - // private CANCoder canCoder; private CANCoder encoder; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; @@ -47,14 +43,7 @@ public class SwerveModule extends SubsystemBase { angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleConfig); - // CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); - // canCoderConfig.magnetOffsetDegrees = offset; - // encoder.configAllSettings(canCoderConfig); encoder.configMagnetOffset(offset); - - // driveMotor.config_kP(0, 0.4); - - // canCoderConfig.magnetOffsetDegrees = 270; } /** @@ -86,7 +75,7 @@ public class SwerveModule extends SubsystemBase { * @return the angle of a SwerveModule as a Rotation2d */ public Rotation2d getAngle() { - // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. + // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); } @@ -138,16 +127,13 @@ public class SwerveModule extends SubsystemBase { angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - double inchesPerSecond = Units.metersToInches(state.speedMetersPerSecond); driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); } public void reset(double position) { - // encoder.setPosition(position); encoder.setPositionToAbsolute(); - // canCoder.setPosition(1024); } public double getCurrent() { diff --git a/src/main/java/frc4388/utility/RobotEncoder.java b/src/main/java/frc4388/utility/RobotEncoder.java deleted file mode 100644 index 2e397ac..0000000 --- a/src/main/java/frc4388/utility/RobotEncoder.java +++ /dev/null @@ -1,23 +0,0 @@ -// 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. - -package frc4388.utility; - -import com.ctre.phoenix.sensors.CANCoder; - -/** Add your docs here. */ -public class RobotEncoder extends CANCoder { - private double offset; - - public RobotEncoder(int id, double offset) { - super(id); - - this.offset = offset; - } - - @Override - public double getAbsolutePosition() { - return super.getAbsolutePosition() - this.offset; - } -}