From 83d140ee86ff4fe7946b263485a29904180fb2a4 Mon Sep 17 00:00:00 2001 From: Tony Wu Date: Tue, 14 Jan 2025 17:20:48 -0800 Subject: [PATCH 01/44] BackAlgaeSubsystem finish + hypthetical contro sch --- src/main/java/frc/robot/Constants.java | 19 ++- .../robot/subsystems/BackAlgaeSubsystem.java | 119 ++++++++++++++++++ 2 files changed, 133 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 76a9153..a146059 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,12 +19,21 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; } + public static class BackAlgaeConstants{ + public static final int ROLLER_MOTOR_ID = 1;//Placeholder + public static final int PIVOT_MOTOR_ID = 2;//Placeholder + public static final double PIVOT_P = 0.01;//placeholder value + public static final double PIVOT_I = 0;//placeholder value + public static final double PIVOT_D = 0;//placeholder value - /** Constants for the swerve subsystem. */ - public static class SwerveConstants { - public static final int FL_DRIVE = 0; - public static final int FL_STEER = 1; - public static final double FL_OFFSET = 0; + public static final int SENSOR_ID = 0;//placeholder value + + } + /** Constants for the swerve subsystem. */ + public static class SwerveConstants { + public static final int FL_DRIVE = 0; + public static final int FL_STEER = 1; + public static final double FL_OFFSET = 0; public static final int FR_DRIVE = 2; public static final int FR_STEER = 3; diff --git a/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java new file mode 100644 index 0000000..7449bda --- /dev/null +++ b/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java @@ -0,0 +1,119 @@ +// 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 frc.robot.subsystems; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkBase.ControlType; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.BackAlgaeConstants; + +import edu.wpi.first.wpilibj.DigitalInput; + + +public class BackAlgaeSubsystem extends SubsystemBase { + static double speed; + static double angle; + private final SparkMax pivotMotor; + private final SparkMax rollerMotor; + + private RelativeEncoder encoder; + private final ClosedLoopConfig closedLoopConfig; + private SparkMaxConfig sparkMaxConfig; + private final SparkClosedLoopController steerPIDController; + private final EncoderConfig encoderConfig; + + private final DigitalInput algaeSensor; + + /** Creates a new ExampleSubsystem. */ + public BackAlgaeSubsystem() { + angle = 0; + pivotMotor = new SparkMax(BackAlgaeConstants.PIVOT_MOTOR_ID, MotorType.kBrushless); + rollerMotor = new SparkMax(BackAlgaeConstants.PIVOT_MOTOR_ID, MotorType.kBrushless); + + steerPIDController = pivotMotor.getClosedLoopController(); + + encoderConfig = new EncoderConfig(); + // encoderConfig.inverted(true); + + closedLoopConfig = new ClosedLoopConfig(); + closedLoopConfig.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .positionWrappingEnabled(true) + .positionWrappingMinInput(0) + .positionWrappingMaxInput(1); + + + sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig.apply(encoderConfig) + .apply(closedLoopConfig) + .inverted(true); + + pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + configurePID(BackAlgaeConstants.PIVOT_P, BackAlgaeConstants.PIVOT_I, BackAlgaeConstants.PIVOT_D); + + algaeSensor = new DigitalInput(BackAlgaeConstants.SENSOR_ID); + + } + + /** + * Example command factory method. + * + * @return a command + */ + + //sets the speed of the rollers + + + public void configurePID(double p, double i, double d){ + closedLoopConfig.pid(p, i, d); + sparkMaxConfig.apply(closedLoopConfig); + pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); +} + +public void setPosition(double targetRads) { + double targetDouble = (targetRads + Math.PI) / (2. * Math.PI); + System.out.println("target" + targetDouble); + System.out.println("current" + encoder.getPosition()); + steerPIDController.setReference(targetDouble, ControlType.kPosition); +} +public void driveRollers(double speed){ + rollerMotor.set(speed); +} +public double getMotorPosition() { + // Returns the position in rotations + return encoder.getPosition(); +} +public boolean SensorGet(){ + return algaeSensor.get(); +} + + /** + * An example method querying a boolean state of the subsystem (for example, a + * digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + + //sets the actuall speed of the rollers + @Override + public void periodic() { + // This method will be called once per scheduler run + } + +} From 9568ab3a59934f933799f9849a553d4b15b313a2 Mon Sep 17 00:00:00 2001 From: Tony Wu Date: Tue, 14 Jan 2025 17:21:09 -0800 Subject: [PATCH 02/44] BackAlgaeSubsystem done + hypothetical control sch --- src/main/java/frc/robot/Constants.java | 17 +++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 9 +++++++++ 2 files changed, 26 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a146059..8865b0b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,24 @@ public static class BackAlgaeConstants{ public static final double PIVOT_D = 0;//placeholder value public static final int SENSOR_ID = 0;//placeholder value + public static final double PIVOT_POSITION_1 = 0;//placeholder value + + public enum Positions{//each position corresponds to a tuned height + POS1(0.2), + POS2(0.4), + POS3(-0.3); + private final double pos; + // Constructor to set the double value + createPos(double pos) { + this.pos = pos; + } + + // Getter to retrieve the rate + public double getPos() { + return pos; + } + } } /** Constants for the swerve subsystem. */ public static class SwerveConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 813c685..e16f56f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,6 +58,15 @@ private void configureBindings() { * the robot is controlled along its own axes, otherwise controls apply to the field axes by default. If the * swerve aim button is held down, the robot will rotate automatically to always face a target, and only * translation will be manually controllable. */ + + //HYPOTHETICAL BACK ALGAE CODE + /* + * Bind a button to move pivot into intaking position, rollers start rolling automatically + * Rollers stop when you press the button again or when distance sensor detects ball + * + * Bind a button to move pivot into outtaking position + * Pressing that button twice spits the algae out + */ swerveSubsystem.setDefaultCommand( new RunCommand(() -> { swerveSubsystem.setDrivePowers( From beac2fc2496ecb9b72893cf31ec3c9456fcf8064 Mon Sep 17 00:00:00 2001 From: Tony Wu Date: Wed, 15 Jan 2025 11:21:54 -0800 Subject: [PATCH 03/44] goofy minor fixes --- src/main/java/frc/robot/Constants.java | 28 ++--- .../robot/commands/backAlgae/algaeIntake.java | 0 .../backAlgae/algaeSetPositionCommand.java | 0 .../robot/subsystems/BackAlgaeSubsystem.java | 2 +- .../frc/robot/subsystems/ClimbSubsysten.java | 112 ++++++++++++++++++ 5 files changed, 125 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc/robot/commands/backAlgae/algaeIntake.java create mode 100644 src/main/java/frc/robot/commands/backAlgae/algaeSetPositionCommand.java create mode 100644 src/main/java/frc/robot/subsystems/ClimbSubsysten.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8865b0b..44c31d0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,24 +28,20 @@ public static class BackAlgaeConstants{ public static final int SENSOR_ID = 0;//placeholder value public static final double PIVOT_POSITION_1 = 0;//placeholder value - - public enum Positions{//each position corresponds to a tuned height - POS1(0.2), - POS2(0.4), - POS3(-0.3); - private final double pos; + public static final double PIVOT_POSITION_2 = 0;//placeholder value + } + public static class ClimbConstants{ + public static final int TOP_MOTOR_ID = 1;//Placeholder + public static final int BOT_MOTOR_ID = 2;//Placeholder + public static final double CLIMB_P = 0.01;//placeholder value + public static final double CLIMB_I = 0;//placeholder value + public static final double CLIMB_D = 0;//placeholder value + public static final double CLIMB_POSITION_1 = 0;//placeholder value + public static final double CLIMB_POSITION_2 = 0;//placeholder value + } + - // Constructor to set the double value - createPos(double pos) { - this.pos = pos; - } - // Getter to retrieve the rate - public double getPos() { - return pos; - } - } - } /** Constants for the swerve subsystem. */ public static class SwerveConstants { public static final int FL_DRIVE = 0; diff --git a/src/main/java/frc/robot/commands/backAlgae/algaeIntake.java b/src/main/java/frc/robot/commands/backAlgae/algaeIntake.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/commands/backAlgae/algaeSetPositionCommand.java b/src/main/java/frc/robot/commands/backAlgae/algaeSetPositionCommand.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java index 7449bda..2c8fe87 100644 --- a/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java @@ -52,7 +52,7 @@ public BackAlgaeSubsystem() { encoderConfig = new EncoderConfig(); // encoderConfig.inverted(true); - closedLoopConfig = new ClosedLoopConfig(); + closedLoopConfig = new ClosedLoopConfig();//code copied from swerve, untested do NOT trust it closedLoopConfig.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingMinInput(0) diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsysten.java b/src/main/java/frc/robot/subsystems/ClimbSubsysten.java new file mode 100644 index 0000000..e57aa58 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimbSubsysten.java @@ -0,0 +1,112 @@ +// 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 frc.robot.subsystems; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkBase.ControlType; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Constants.ClimbConstants; + + +public class ClimbSubsysten extends SubsystemBase { + + private final SparkMax topMotor; + private final SparkMax botMotor; + + private RelativeEncoder encoder; + private final ClosedLoopConfig closedLoopConfig; + private SparkMaxConfig sparkMaxConfig; + private final SparkClosedLoopController steerPIDController; + private final EncoderConfig encoderConfig; + + /** Creates a new ExampleSubsystem. */ + public ClimbSubsysten() { + + topMotor = new SparkMax(ClimbConstants.TOP_MOTOR_ID, MotorType.kBrushless); + botMotor = new SparkMax(ClimbConstants.BOT_MOTOR_ID, MotorType.kBrushless); + botMotor.follow(topMotor); + steerPIDController = topMotor.getClosedLoopController(); + + encoderConfig = new EncoderConfig(); + // encoderConfig.inverted(true); + + closedLoopConfig = new ClosedLoopConfig();//code copied from swerve, untested do NOT trust it + closedLoopConfig.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .positionWrappingEnabled(true) + .positionWrappingMinInput(0) + .positionWrappingMaxInput(1); + + + sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig.apply(encoderConfig) + .apply(closedLoopConfig) + .inverted(true); + + topMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + configurePID(ClimbConstants.CLIMB_P, ClimbConstants.CLIMB_I, ClimbConstants.CLIMB_D); + + } + + /** + * Example command factory method. + * + * @return a command + */ + + //sets the speed of the rollers + + + public void configurePID(double p, double i, double d){ + closedLoopConfig.pid(p, i, d); + sparkMaxConfig.apply(closedLoopConfig); + topMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); +} + +public void setPosition(double targetRads) { + double targetDouble = (targetRads + Math.PI) / (2. * Math.PI); + System.out.println("target" + targetDouble); + System.out.println("current" + encoder.getPosition()); + steerPIDController.setReference(targetDouble, ControlType.kPosition); +} +public void driveRollers(double speed){ + topMotor.set(speed); +} +public double getMotorPosition() { + // Returns the position in rotations + return encoder.getPosition(); +} + + + /** + * An example method querying a boolean state of the subsystem (for example, a + * digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + + //sets the actuall speed of the rollers + @Override + public void periodic() { + // This method will be called once per scheduler run + } + +} From c95aaa1d18cff7427127136e29f2e59b6ee9ce2c Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Wed, 15 Jan 2025 12:20:58 -0800 Subject: [PATCH 04/44] climb command --- .../frc/robot/commands/ClimbRaiseCommand.java | 34 +++++++++++++++++++ .../robot/commands/backAlgae/algaeIntake.java | 0 .../backAlgae/algaeSetPositionCommand.java | 0 ...limbSubsysten.java => ClimbSubsystem.java} | 4 +-- 4 files changed, 36 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ClimbRaiseCommand.java delete mode 100644 src/main/java/frc/robot/commands/backAlgae/algaeIntake.java delete mode 100644 src/main/java/frc/robot/commands/backAlgae/algaeSetPositionCommand.java rename src/main/java/frc/robot/subsystems/{ClimbSubsysten.java => ClimbSubsystem.java} (97%) diff --git a/src/main/java/frc/robot/commands/ClimbRaiseCommand.java b/src/main/java/frc/robot/commands/ClimbRaiseCommand.java new file mode 100644 index 0000000..5277fdd --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimbRaiseCommand.java @@ -0,0 +1,34 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ClimbSubsystem; + +/** Lowers climb fully when off the chain. Should run at the start of auton to ensure that both arms are at their lowest + * positions so that the robot may fit underneath the stage. */ +public class ClimbRaiseCommand extends Command { + private static final double LOWERING_SPEED = -0.9; + private final ClimbSubsystem climbSubsystem; + private final int speed = 0; //placeholder + + /** Constructs a new {@link ClimbLowerCommand}. */ + public ClimbRaiseCommand(ClimbSubsystem climbSubsystem) { + this.climbSubsystem = climbSubsystem; + + this.addRequirements(climbSubsystem); + } + + @Override + public void initialize() { + climbSubsystem.driveRollers(speed); + } + + @Override + public boolean isFinished() { + return climbSubsystem.isLowered(); + } + + @Override + public void end(boolean interrupted) { + super.end(interrupted); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/backAlgae/algaeIntake.java b/src/main/java/frc/robot/commands/backAlgae/algaeIntake.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/commands/backAlgae/algaeSetPositionCommand.java b/src/main/java/frc/robot/commands/backAlgae/algaeSetPositionCommand.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsysten.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java similarity index 97% rename from src/main/java/frc/robot/subsystems/ClimbSubsysten.java rename to src/main/java/frc/robot/subsystems/ClimbSubsystem.java index e57aa58..9d7f1d4 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsysten.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -27,7 +27,7 @@ import frc.robot.Constants.ClimbConstants; -public class ClimbSubsysten extends SubsystemBase { +public class ClimbSubsystem extends SubsystemBase { private final SparkMax topMotor; private final SparkMax botMotor; @@ -39,7 +39,7 @@ public class ClimbSubsysten extends SubsystemBase { private final EncoderConfig encoderConfig; /** Creates a new ExampleSubsystem. */ - public ClimbSubsysten() { + public ClimbSubsystem() { topMotor = new SparkMax(ClimbConstants.TOP_MOTOR_ID, MotorType.kBrushless); botMotor = new SparkMax(ClimbConstants.BOT_MOTOR_ID, MotorType.kBrushless); From c8411b7fe98f8f9e6916afc38bb0297e4358b648 Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Thu, 16 Jan 2025 19:08:02 -0800 Subject: [PATCH 05/44] fixed sparkmax follow errors --- .../frc/robot/commands/ClimbRaiseCommand.java | 5 +- .../robot/subsystems/BackAlgaeSubsystem.java | 119 ------------------ .../frc/robot/subsystems/ClimbSubsystem.java | 51 ++++---- 3 files changed, 23 insertions(+), 152 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java diff --git a/src/main/java/frc/robot/commands/ClimbRaiseCommand.java b/src/main/java/frc/robot/commands/ClimbRaiseCommand.java index 5277fdd..ba81235 100644 --- a/src/main/java/frc/robot/commands/ClimbRaiseCommand.java +++ b/src/main/java/frc/robot/commands/ClimbRaiseCommand.java @@ -6,14 +6,13 @@ /** Lowers climb fully when off the chain. Should run at the start of auton to ensure that both arms are at their lowest * positions so that the robot may fit underneath the stage. */ public class ClimbRaiseCommand extends Command { - private static final double LOWERING_SPEED = -0.9; private final ClimbSubsystem climbSubsystem; private final int speed = 0; //placeholder + private final double endingMotorPosition = 0.0; /** Constructs a new {@link ClimbLowerCommand}. */ public ClimbRaiseCommand(ClimbSubsystem climbSubsystem) { this.climbSubsystem = climbSubsystem; - this.addRequirements(climbSubsystem); } @@ -24,7 +23,7 @@ public void initialize() { @Override public boolean isFinished() { - return climbSubsystem.isLowered(); + return climbSubsystem.getMotorPosition() == endingMotorPosition; } @Override diff --git a/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java deleted file mode 100644 index 2c8fe87..0000000 --- a/src/main/java/frc/robot/subsystems/BackAlgaeSubsystem.java +++ /dev/null @@ -1,119 +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 frc.robot.subsystems; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.config.EncoderConfig; -import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkAbsoluteEncoder; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkBase.ControlType; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.BackAlgaeConstants; - -import edu.wpi.first.wpilibj.DigitalInput; - - -public class BackAlgaeSubsystem extends SubsystemBase { - static double speed; - static double angle; - private final SparkMax pivotMotor; - private final SparkMax rollerMotor; - - private RelativeEncoder encoder; - private final ClosedLoopConfig closedLoopConfig; - private SparkMaxConfig sparkMaxConfig; - private final SparkClosedLoopController steerPIDController; - private final EncoderConfig encoderConfig; - - private final DigitalInput algaeSensor; - - /** Creates a new ExampleSubsystem. */ - public BackAlgaeSubsystem() { - angle = 0; - pivotMotor = new SparkMax(BackAlgaeConstants.PIVOT_MOTOR_ID, MotorType.kBrushless); - rollerMotor = new SparkMax(BackAlgaeConstants.PIVOT_MOTOR_ID, MotorType.kBrushless); - - steerPIDController = pivotMotor.getClosedLoopController(); - - encoderConfig = new EncoderConfig(); - // encoderConfig.inverted(true); - - closedLoopConfig = new ClosedLoopConfig();//code copied from swerve, untested do NOT trust it - closedLoopConfig.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) - .positionWrappingEnabled(true) - .positionWrappingMinInput(0) - .positionWrappingMaxInput(1); - - - sparkMaxConfig = new SparkMaxConfig(); - sparkMaxConfig.apply(encoderConfig) - .apply(closedLoopConfig) - .inverted(true); - - pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - configurePID(BackAlgaeConstants.PIVOT_P, BackAlgaeConstants.PIVOT_I, BackAlgaeConstants.PIVOT_D); - - algaeSensor = new DigitalInput(BackAlgaeConstants.SENSOR_ID); - - } - - /** - * Example command factory method. - * - * @return a command - */ - - //sets the speed of the rollers - - - public void configurePID(double p, double i, double d){ - closedLoopConfig.pid(p, i, d); - sparkMaxConfig.apply(closedLoopConfig); - pivotMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); -} - -public void setPosition(double targetRads) { - double targetDouble = (targetRads + Math.PI) / (2. * Math.PI); - System.out.println("target" + targetDouble); - System.out.println("current" + encoder.getPosition()); - steerPIDController.setReference(targetDouble, ControlType.kPosition); -} -public void driveRollers(double speed){ - rollerMotor.set(speed); -} -public double getMotorPosition() { - // Returns the position in rotations - return encoder.getPosition(); -} -public boolean SensorGet(){ - return algaeSensor.get(); -} - - /** - * An example method querying a boolean state of the subsystem (for example, a - * digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - - //sets the actuall speed of the rollers - @Override - public void periodic() { - // This method will be called once per scheduler run - } - -} diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 9d7f1d4..7551be1 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -3,27 +3,22 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.AbsoluteEncoderConfig; import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.EncoderConfig; -import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.MAXMotionConfig; import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkAbsoluteEncoder; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkBase.ControlType; - -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.config.MAXMotionConfig; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import frc.robot.Constants.ClimbConstants; @@ -32,48 +27,44 @@ public class ClimbSubsystem extends SubsystemBase { private final SparkMax topMotor; private final SparkMax botMotor; + private final int botMotorID; + private RelativeEncoder encoder; private final ClosedLoopConfig closedLoopConfig; private SparkMaxConfig sparkMaxConfig; private final SparkClosedLoopController steerPIDController; private final EncoderConfig encoderConfig; + private MAXMotionConfig maxMotionConfig; /** Creates a new ExampleSubsystem. */ public ClimbSubsystem() { topMotor = new SparkMax(ClimbConstants.TOP_MOTOR_ID, MotorType.kBrushless); botMotor = new SparkMax(ClimbConstants.BOT_MOTOR_ID, MotorType.kBrushless); - botMotor.follow(topMotor); + + botMotorID = 0; //have to change later + steerPIDController = topMotor.getClosedLoopController(); encoderConfig = new EncoderConfig(); - // encoderConfig.inverted(true); + encoderConfig.inverted(true); - closedLoopConfig = new ClosedLoopConfig();//code copied from swerve, untested do NOT trust it + closedLoopConfig = new ClosedLoopConfig(); //code copied from swerve, untested do NOT trust it closedLoopConfig.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingMinInput(0) .positionWrappingMaxInput(1); - sparkMaxConfig = new SparkMaxConfig(); sparkMaxConfig.apply(encoderConfig) .apply(closedLoopConfig) - .inverted(true); - + .inverted(true) + .follow(botMotorID); + topMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + botMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); configurePID(ClimbConstants.CLIMB_P, ClimbConstants.CLIMB_I, ClimbConstants.CLIMB_D); - } - - /** - * Example command factory method. - * - * @return a command - */ - - //sets the speed of the rollers - public void configurePID(double p, double i, double d){ closedLoopConfig.pid(p, i, d); From e36601c90b1ec393d5dd12f7de6db5790e6a896d Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Fri, 17 Jan 2025 12:18:28 -0800 Subject: [PATCH 06/44] fixed climb logic --- src/main/java/frc/robot/Constants.java | 1 + .../frc/robot/subsystems/ClimbSubsystem.java | 75 +++++++++-------- ...rLib.json => PathplannerLib-2025.2.1.json} | 8 +- ...enix6-25.1.0.json => Phoenix6-25.2.0.json} | 84 +++++++++++++------ ...Lib-2025.0.0.json => REVLib-2025.0.1.json} | 15 ++-- ...ca-2025.0.0.json => Studica-2025.0.1.json} | 14 ++-- 6 files changed, 114 insertions(+), 83 deletions(-) rename vendordeps/{PathplannerLib.json => PathplannerLib-2025.2.1.json} (87%) rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-25.2.0.json} (85%) rename vendordeps/{REVLib-2025.0.0.json => REVLib-2025.0.1.json} (86%) rename vendordeps/{Studica-2025.0.0.json => Studica-2025.0.1.json} (89%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 44c31d0..d6c51b2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,6 +38,7 @@ public static class ClimbConstants{ public static final double CLIMB_D = 0;//placeholder value public static final double CLIMB_POSITION_1 = 0;//placeholder value public static final double CLIMB_POSITION_2 = 0;//placeholder value + public static final double CLIMB_GEAR_RATIO = 0.0; //placeholder value } diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 7551be1..6298083 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -10,82 +10,84 @@ import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.AbsoluteEncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.EncoderConfig; -import com.revrobotics.spark.config.MAXMotionConfig; import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.MAXMotionConfig; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ClimbConstants; +import edu.wpi.first.math.util.Units; public class ClimbSubsystem extends SubsystemBase { private final SparkMax topMotor; private final SparkMax botMotor; - private final int botMotorID; - private RelativeEncoder encoder; private final ClosedLoopConfig closedLoopConfig; - private SparkMaxConfig sparkMaxConfig; - private final SparkClosedLoopController steerPIDController; + private final SparkMaxConfig sparkMaxConfig; + private final SparkMaxConfig sparkMaxConfigFollow; private final EncoderConfig encoderConfig; - private MAXMotionConfig maxMotionConfig; + private final SoftLimitConfig softLimitConfig; + + private final RelativeEncoder climbEncoder; + private final SparkClosedLoopController steerPIDController; /** Creates a new ExampleSubsystem. */ public ClimbSubsystem() { topMotor = new SparkMax(ClimbConstants.TOP_MOTOR_ID, MotorType.kBrushless); botMotor = new SparkMax(ClimbConstants.BOT_MOTOR_ID, MotorType.kBrushless); + climbEncoder = topMotor.getEncoder(); botMotorID = 0; //have to change later steerPIDController = topMotor.getClosedLoopController(); encoderConfig = new EncoderConfig(); - encoderConfig.inverted(true); + encoderConfig.positionConversionFactor(ClimbConstants.CLIMB_GEAR_RATIO); - closedLoopConfig = new ClosedLoopConfig(); //code copied from swerve, untested do NOT trust it - closedLoopConfig.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) - .positionWrappingEnabled(true) - .positionWrappingMinInput(0) - .positionWrappingMaxInput(1); + softLimitConfig = new SoftLimitConfig(); + softLimitConfig.forwardSoftLimitEnabled(true) + .forwardSoftLimit(Units.degreesToRadians(90)) //find out with mech for soft lim + .reverseSoftLimitEnabled(true) + .reverseSoftLimit(0); + + closedLoopConfig = new ClosedLoopConfig(); + closedLoopConfig.pid(ClimbConstants.CLIMB_P, ClimbConstants.CLIMB_I, ClimbConstants.CLIMB_D); sparkMaxConfig = new SparkMaxConfig(); sparkMaxConfig.apply(encoderConfig) - .apply(closedLoopConfig) - .inverted(true) - .follow(botMotorID); + .apply(softLimitConfig) + .apply(closedLoopConfig); + + sparkMaxConfigFollow = new SparkMaxConfig(); //to follow the motor + sparkMaxConfigFollow.follow(botMotorID) + .apply(encoderConfig) + .apply(softLimitConfig) + .apply(closedLoopConfig); topMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - botMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - configurePID(ClimbConstants.CLIMB_P, ClimbConstants.CLIMB_I, ClimbConstants.CLIMB_D); + botMotor.configure(sparkMaxConfigFollow, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - - public void configurePID(double p, double i, double d){ - closedLoopConfig.pid(p, i, d); - sparkMaxConfig.apply(closedLoopConfig); - topMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); -} -public void setPosition(double targetRads) { + public void setPosition(double targetRads) { double targetDouble = (targetRads + Math.PI) / (2. * Math.PI); System.out.println("target" + targetDouble); - System.out.println("current" + encoder.getPosition()); + System.out.println("current" + climbEncoder.getPosition()); steerPIDController.setReference(targetDouble, ControlType.kPosition); -} -public void driveRollers(double speed){ - topMotor.set(speed); -} -public double getMotorPosition() { - // Returns the position in rotations - return encoder.getPosition(); -} + } + public void driveRollers(double speed){ + topMotor.set(speed); + } + + public double getMotorPosition() { + // Returns the position in rotations + return climbEncoder.getPosition(); + } /** * An example method querying a boolean state of the subsystem (for example, a @@ -95,6 +97,7 @@ public double getMotorPosition() { */ //sets the actuall speed of the rollers + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-2025.2.1.json similarity index 87% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-2025.2.1.json index 396f92d..71e25f3 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-2025.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-2025.2.1.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.2.0.json similarity index 85% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-25.2.0.json index 473f6a8..38747fb 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-25.2.0.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-25.2.0.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.2.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +351,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib-2025.0.1.json similarity index 86% rename from vendordeps/REVLib-2025.0.0.json rename to vendordeps/REVLib-2025.0.1.json index cde6011..c998054 100644 --- a/vendordeps/REVLib-2025.0.0.json +++ b/vendordeps/REVLib-2025.0.1.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.0.json", + "fileName": "REVLib-2025.0.1.json", "name": "REVLib", - "version": "2025.0.0", + "version": "2025.0.1", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,19 +12,18 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.1.json similarity index 89% rename from vendordeps/Studica-2025.0.0.json rename to vendordeps/Studica-2025.0.1.json index ddb0e44..5010be0 100644 --- a/vendordeps/Studica-2025.0.0.json +++ b/vendordeps/Studica-2025.0.1.json @@ -1,13 +1,13 @@ { - "fileName": "Studica-2025.0.0.json", + "fileName": "Studica-2025.0.1.json", "name": "Studica", - "version": "2025.0.0", + "version": "2025.0.1", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2025", "mavenUrls": [ "https://dev.studica.com/maven/release/2025/" ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json", + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", "cppDependencies": [ { "artifactId": "Studica-cpp", @@ -24,7 +24,7 @@ "libName": "Studica", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.0.0" + "version": "2025.0.1" }, { "artifactId": "Studica-driver", @@ -41,14 +41,14 @@ "libName": "StudicaDriver", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.0.0" + "version": "2025.0.1" } ], "javaDependencies": [ { "artifactId": "Studica-java", "groupId": "com.studica.frc", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ @@ -65,7 +65,7 @@ "osxuniversal", "windowsx86-64" ], - "version": "2025.0.0" + "version": "2025.0.1" } ] } \ No newline at end of file From 46cb6ff63197573f0b11a1518761dac90bbac3e0 Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Fri, 17 Jan 2025 12:19:06 -0800 Subject: [PATCH 07/44] fixed climb logic --- .vscode/settings.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 6adedc3..e9f1f81 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,5 @@ "edu.wpi.first.math.**.struct.*", ], "wpilib.autoStartRioLog": true, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } From 1b2d8126dce93d224fed90344237d73664415cfb Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Fri, 17 Jan 2025 17:49:04 -0800 Subject: [PATCH 08/44] removed pid stuff --- src/main/java/frc/robot/subsystems/ClimbSubsystem.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 6298083..24ad7bd 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -4,10 +4,8 @@ package frc.robot.subsystems; import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SoftLimitConfig; @@ -33,7 +31,7 @@ public class ClimbSubsystem extends SubsystemBase { private final SoftLimitConfig softLimitConfig; private final RelativeEncoder climbEncoder; - private final SparkClosedLoopController steerPIDController; + //private final SparkClosedLoopController steerPIDController; /** Creates a new ExampleSubsystem. */ public ClimbSubsystem() { @@ -44,7 +42,7 @@ public ClimbSubsystem() { botMotorID = 0; //have to change later - steerPIDController = topMotor.getClosedLoopController(); + //steerPIDController = topMotor.getClosedLoopController(); encoderConfig = new EncoderConfig(); encoderConfig.positionConversionFactor(ClimbConstants.CLIMB_GEAR_RATIO); @@ -56,7 +54,7 @@ public ClimbSubsystem() { .reverseSoftLimit(0); closedLoopConfig = new ClosedLoopConfig(); - closedLoopConfig.pid(ClimbConstants.CLIMB_P, ClimbConstants.CLIMB_I, ClimbConstants.CLIMB_D); + //closedLoopConfig.pid(ClimbConstants.CLIMB_P, ClimbConstants.CLIMB_I, ClimbConstants.CLIMB_D); sparkMaxConfig = new SparkMaxConfig(); sparkMaxConfig.apply(encoderConfig) @@ -77,7 +75,6 @@ public void setPosition(double targetRads) { double targetDouble = (targetRads + Math.PI) / (2. * Math.PI); System.out.println("target" + targetDouble); System.out.println("current" + climbEncoder.getPosition()); - steerPIDController.setReference(targetDouble, ControlType.kPosition); } public void driveRollers(double speed){ From dcf071e99567e2c42119d6ee73eb37dc7eed44c7 Mon Sep 17 00:00:00 2001 From: Jesse Date: Fri, 17 Jan 2025 16:03:05 -0800 Subject: [PATCH 09/44] change max speed back to 6000 --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d6c51b2..976eb41 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,7 +69,7 @@ public static class SwerveConstants { public static final double DRIVE_GEAR_REDUCTION = 9. * 20. / 26.; public static final double DRIVE_WHEEL_CIRCUMFERENCE = Units.inchesToMeters(4 * Math.PI); - public static final double MAX_VEL = 4200. / 6.923 / 60. * 2. * 2. * Math.PI * .0254; // Kraken speed / gear ratio / reduced to per second * circumference * convert to meters + public static final double MAX_VEL = 6000. / 6.923 / 60. * 2. * 2. * Math.PI * .0254; // Kraken speed / gear ratio / reduced to per second * circumference * convert to meters public static final double MAX_OMEGA = MAX_VEL / FL_POS.getNorm(); public static final double DRIVE_P = 0.32; // temporary value From 5155c238fe190a1dc0f1205eb7aa7c0d8e63a0f5 Mon Sep 17 00:00:00 2001 From: Jesse Date: Sun, 19 Jan 2025 16:46:11 -0800 Subject: [PATCH 10/44] Fix PS5 controller can't rotate after power cycle issue --- src/main/java/frc/robot/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e16f56f..28abd55 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,6 +102,7 @@ public Command getAutonomousCommand() { * 0 */ private void constructDriveController(){ + DriverStation.refreshData(); if(DriverStation.getJoystickName(0).equals("Controller (Xbox One For Windows)")) { driveController = new XboxDriveController(); } From b5a9800af2e9b7a15d51839fcd28183b8207ce6a Mon Sep 17 00:00:00 2001 From: Jesse Date: Mon, 20 Jan 2025 13:19:01 -0800 Subject: [PATCH 11/44] make PID arrays to allow different PIDs across modules --- src/main/java/frc/robot/Constants.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 976eb41..423116d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -72,16 +72,16 @@ public static class SwerveConstants { public static final double MAX_VEL = 6000. / 6.923 / 60. * 2. * 2. * Math.PI * .0254; // Kraken speed / gear ratio / reduced to per second * circumference * convert to meters public static final double MAX_OMEGA = MAX_VEL / FL_POS.getNorm(); - public static final double DRIVE_P = 0.32; // temporary value - public static final double DRIVE_I = 0; - public static final double DRIVE_D = 0; - public static final double DRIVE_S = 0.1499; - public static final double DRIVE_V = 0.112; + public static final double[] DRIVE_P = new double[] {0.32, 0.32, 0.32, 0.32}; + public static final double[] DRIVE_I = new double[] {0, 0, 0, 0}; + public static final double[] DRIVE_D = new double[] {0, 0, 0, 0}; + public static final double[] DRIVE_S = new double[] {0.1499, 0.1499, 0.1499, 0.1499}; + public static final double[] DRIVE_V = new double[] {0.112, 0.112, 0.112, 0.112}; - public static final double STEER_P = 5.8; - public static final double STEER_I = 0; - public static final double STEER_D = 0; - public static final double STEER_FF = 0; + public static final double[] STEER_P = new double[] {5.4, 5.4, 5.4, 5.4}; + public static final double[] STEER_I = new double[] {0, 0, 0, 0}; + public static final double[] STEER_D = new double[] {0, 0, 0, 0}; + public static final double[] STEER_FF = new double[] {0.036, 0.024, 0.0182, 0.05}; } public static class LoggingConstants{ From daa8872c6b9292da2e6fcfc81b0a834f24792191 Mon Sep 17 00:00:00 2001 From: Jesse Date: Mon, 20 Jan 2025 13:19:12 -0800 Subject: [PATCH 12/44] add debug for drive and steer --- .../robot/subsystems/swerve/SwerveModule.java | 75 ++++++++++++++++++- 1 file changed, 73 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index c84d354..88b3211 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -3,14 +3,24 @@ 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.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableInstance; import static frc.robot.Constants.SwerveConstants.*; +import java.util.EnumSet; + public class SwerveModule { public final KrakenDriveMotor driveMotor; public final NeoSteerMotor steerMotor; + private int drivePort; + private int steerPort; + + private int driveIndex; + private int steerIndex; + private double offsetRads = 0; /** Constructs a Swerve Module. @@ -22,11 +32,28 @@ public class SwerveModule { public SwerveModule(int drivePort, int steerPort, double offsetRads) { + this.drivePort = drivePort; + this.steerPort = steerPort; + + driveIndex = drivePort / 2; + steerIndex = (steerPort -1) / 2; + steerMotor = new NeoSteerMotor(steerPort); - steerMotor.configurePID(STEER_P, STEER_I, STEER_D, STEER_FF); + steerMotor.configurePID( + STEER_P[steerIndex], + STEER_I[steerIndex], + STEER_D[steerIndex], + STEER_FF[steerIndex] + ); driveMotor = new KrakenDriveMotor(drivePort); - driveMotor.configPID(DRIVE_P, DRIVE_I, DRIVE_D, DRIVE_S, DRIVE_V); + driveMotor.configPID( + DRIVE_P[driveIndex], + DRIVE_I[driveIndex], + DRIVE_D[driveIndex], + DRIVE_S[driveIndex], + DRIVE_V[driveIndex] + ); this.offsetRads = offsetRads; } @@ -68,6 +95,7 @@ public SwerveModulePosition getPosition() { getWrappedAngle() ); } + /** * Gets the state of the swerve module (drive velo in m/s + angle 0-1 ) * @return state of the module (velo is m/s and angle is double from 0 to 1) @@ -115,4 +143,47 @@ public double getDistanceDriven() { public double getDriveVelocity() { return driveMotor.getVelocity(); } + + public void steerDebug(){ + NetworkTableInstance.getDefault().getTable("steerDebug") + .getEntry(steerPort + "PIDF") + .setDoubleArray( + new double[] { + STEER_P[steerIndex], + STEER_I[steerIndex], + STEER_D[steerIndex], + STEER_FF[steerIndex] + } + ); + NetworkTableInstance.getDefault().getTable("steerDebug").addListener( + steerPort + "PIDF", + EnumSet.of(NetworkTableEvent.Kind.kValueAll), + (table, key, event) -> { + double[] pidf = event.valueData.value.getDoubleArray(); + steerMotor.configurePID(pidf[0], pidf[1], pidf[2], pidf[3]); + } + ); + } + + public void driveDebug(){ + NetworkTableInstance.getDefault().getTable("driveDebug") + .getEntry(drivePort + "PIDSV") + .setDoubleArray( + new double[] { + DRIVE_P[driveIndex], + DRIVE_I[driveIndex], + DRIVE_D[driveIndex], + DRIVE_S[driveIndex], + DRIVE_V[driveIndex] + } + ); + NetworkTableInstance.getDefault().getTable("driveDebug").addListener( + drivePort + "PIDSV", + EnumSet.of(NetworkTableEvent.Kind.kValueAll), + (table, key, event) -> { + double[] pidsv = event.valueData.value.getDoubleArray(); + driveMotor.configPID(pidsv[0], pidsv[1], pidsv[2], pidsv[3], pidsv[4]); + } + ); + } } From 75d80153de03ea6f834ebd4747d233b12920b93a Mon Sep 17 00:00:00 2001 From: Jesse Chu Date: Mon, 20 Jan 2025 13:59:52 -0800 Subject: [PATCH 13/44] remove shuffleboard tab since using advantage scope --- .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 9142556..434ff9d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -12,9 +12,6 @@ import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static frc.robot.Constants.SwerveConstants.*; @@ -49,9 +46,6 @@ public class SwerveSubsystem extends SubsystemBase { private NetworkTable swerveTable; private StructArrayPublisher swerveStatesPublisher; - private final Field2d fieldVisual = new Field2d(); - private final ShuffleboardTab tab = Shuffleboard.getTab("Swerve"); - private StructPublisher estimatedPosePublisher; public SwerveSubsystem() { @@ -222,8 +216,6 @@ private void initNT(){ "estimatedPose", Pose2d.struct ).publish(); - - tab.add("Field", fieldVisual); } /** @@ -232,7 +224,6 @@ private void initNT(){ private void publishStats(){ swerveStatesPublisher.set(getModuleStates()); estimatedPosePublisher.set(estimatedPose); - fieldVisual.setRobotPose(estimatedPose); frontLeftModule.driveMotor.publishStats(); frontRightModule.driveMotor.publishStats(); backLeftModule.driveMotor.publishStats(); From ef71d47c3c4f39a9d77d995cbc264d11baeb06ec Mon Sep 17 00:00:00 2001 From: Jesse Chu Date: Mon, 20 Jan 2025 14:26:24 -0800 Subject: [PATCH 14/44] added constants to toggle debug for drive and steer --- src/main/java/frc/robot/Constants.java | 4 ++ .../subsystems/swerve/NeoSteerMotor.java | 34 +++++++++++- .../robot/subsystems/swerve/SwerveModule.java | 12 ++++- .../subsystems/swerve/SwerveSubsystem.java | 54 ++++++++++++++++--- 4 files changed, 95 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 423116d..45ad9d0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -82,6 +82,10 @@ public static class SwerveConstants { public static final double[] STEER_I = new double[] {0, 0, 0, 0}; public static final double[] STEER_D = new double[] {0, 0, 0, 0}; public static final double[] STEER_FF = new double[] {0.036, 0.024, 0.0182, 0.05}; + + public static final boolean DRIVE_DEBUG = false; + public static final boolean STEER_DEBUG = false; + public static final boolean STATE_DEBUG = false; } public static class LoggingConstants{ diff --git a/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java b/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java index fe5d3f4..21702f8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java @@ -7,6 +7,13 @@ import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; + +import static frc.robot.Constants.LoggingConstants.SWERVE_TABLE; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; @@ -17,8 +24,8 @@ public class NeoSteerMotor { private final SparkMax motor; - private final SparkAbsoluteEncoder steerEncoder; + private SparkBaseConfig sparkMaxConfig; private AbsoluteEncoderConfig encoderConfig; private ClosedLoopConfig closedLoopConfig; @@ -26,6 +33,12 @@ public class NeoSteerMotor { private SparkClosedLoopController steerPIDController; + private NetworkTableInstance ntInstance; + private NetworkTable swerveStatsTable; + private DoublePublisher neoPositionPublisher; + private DoublePublisher neoSetPositionPublisher; + private double targetDouble = 0; + /** * A Neo steer motor for swerve steering * @param canId the motor's CAN ID @@ -95,4 +108,23 @@ public void setPosition(double targetRads) { public double getPosition(){ return steerEncoder.getPosition(); } + + /** + * Initializes NetworkTables + * @param canId + */ + public void initNT (int canId){ + ntInstance = NetworkTableInstance.getDefault(); + swerveStatsTable = ntInstance.getTable(SWERVE_TABLE); + neoPositionPublisher = swerveStatsTable.getDoubleTopic(canId + "neoPosition").publish(); + neoSetPositionPublisher = swerveStatsTable.getDoubleTopic(canId + "neoSetPosition").publish(); + } + + /** + * Publishes Neo stats to NT + */ + public void publishStats(){ + neoPositionPublisher.set(getPosition()); + neoSetPositionPublisher.set(targetDouble); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 88b3211..aa87046 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -12,8 +12,8 @@ public class SwerveModule { - public final KrakenDriveMotor driveMotor; - public final NeoSteerMotor steerMotor; + private final KrakenDriveMotor driveMotor; + private final NeoSteerMotor steerMotor; private int drivePort; private int steerPort; @@ -144,6 +144,14 @@ public double getDriveVelocity() { return driveMotor.getVelocity(); } + public void publishDriveStats(){ + driveMotor.publishStats(); + } + + public void publishSteerStats(){ + steerMotor.publishStats(); + } + public void steerDebug(){ NetworkTableInstance.getDefault().getTable("steerDebug") .getEntry(steerPort + "PIDF") diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 434ff9d..5c474aa 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -39,7 +39,6 @@ public class SwerveSubsystem extends SubsystemBase { private final SwerveDrivePoseEstimator poseEstimator; private Rotation2d driverHeadingOffset = new Rotation2d(); - private final AHRS ahrs; private NetworkTableInstance ntInstance; @@ -66,6 +65,13 @@ public SwerveSubsystem() { buildAuton(); initNT(); + + if(DRIVE_DEBUG){ + enableDriveDebug(); + } + if(STEER_DEBUG){ + enableSteerDebug(); + } } @Override @@ -222,14 +228,50 @@ private void initNT(){ * publishes swerve stats to NT */ private void publishStats(){ - swerveStatesPublisher.set(getModuleStates()); estimatedPosePublisher.set(estimatedPose); - frontLeftModule.driveMotor.publishStats(); - frontRightModule.driveMotor.publishStats(); - backLeftModule.driveMotor.publishStats(); - backRightModule.driveMotor.publishStats(); + + if(STATE_DEBUG || DRIVE_DEBUG || STEER_DEBUG){ + swerveStatesPublisher.set(getModuleStates()); + } + + if(DRIVE_DEBUG){ + frontLeftModule.publishDriveStats(); + frontRightModule.publishDriveStats(); + backLeftModule.publishDriveStats(); + backRightModule.publishDriveStats(); + } + + if(STEER_DEBUG){ + frontLeftModule.publishSteerStats(); + frontRightModule.publishSteerStats(); + backLeftModule.publishSteerStats(); + backRightModule.publishSteerStats(); + } } + /** + * Enables drive debug + */ + private void enableDriveDebug(){ + frontLeftModule.driveDebug(); + frontRightModule.driveDebug(); + backLeftModule.driveDebug(); + backRightModule.driveDebug(); + } + + /** + * Enables steer debug + */ + private void enableSteerDebug(){ + frontLeftModule.steerDebug(); + frontRightModule.steerDebug(); + backLeftModule.steerDebug(); + backRightModule.steerDebug(); + } + + /** + * Builds the auton builder + */ private void buildAuton(){ RobotConfig config = null; try { From 5585d6a860bcad3fd86e32bd37a5c218c0ce5303 Mon Sep 17 00:00:00 2001 From: Jesse Chu Date: Mon, 20 Jan 2025 14:35:51 -0800 Subject: [PATCH 15/44] initializes nt --- src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java b/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java index 21702f8..a2c2aac 100644 --- a/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java @@ -76,6 +76,7 @@ public NeoSteerMotor(int canId) { // motor.enableVoltageCompensation(12.0); // motor.burnFlash(); + initNT(canId); } /** From fba320cfb16398ba6556bb6a301a9257dc0abee9 Mon Sep 17 00:00:00 2001 From: Jesse Chu Date: Tue, 21 Jan 2025 15:09:44 -0800 Subject: [PATCH 16/44] sync, timestamp seems weird --- src/main/java/frc/robot/RobotContainer.java | 14 ++--- .../PhoenixLoggingSubsystem.java | 16 ++++++ .../subsystems/swerve/KrakenDriveMotor.java | 53 +++++++++++++++++- .../subsystems/swerve/NeoSteerMotor.java | 54 ++++++++++++++++++- .../robot/subsystems/swerve/SwerveModule.java | 5 ++ .../subsystems/swerve/SwerveSubsystem.java | 20 ++++++- src/main/java/frc/robot/util/GRTUtil.java | 14 +++++ 7 files changed, 166 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/util/GRTUtil.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 28abd55..b018e86 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,13 +33,16 @@ public class RobotContainer { private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); - private final FieldManagementSubsystem fieldManagementSubsystem = new FieldManagementSubsystem(); - private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = new PhoenixLoggingSubsystem(fieldManagementSubsystem); + private final FieldManagementSubsystem fieldManagementSubsystem = + new FieldManagementSubsystem(); + + private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = + new PhoenixLoggingSubsystem(fieldManagementSubsystem); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { constructDriveController(); - startLog(); + // startLog(); configureBindings(); } @@ -119,8 +122,7 @@ else if(DriverStation.getJoystickName(0).equals("DualSense Wireless Controller") * Starts datalog at /media/sda1/robotLogs */ private void startLog(){ - DataLogManager.start("/media/sda1/robotLogs"); - DriverStation.startDataLog(DataLogManager.getLog()); - + DataLogManager.start(); + // DriverStation.startDataLog(DataLogManager.getLog()); } } diff --git a/src/main/java/frc/robot/subsystems/PhoenixLoggingSubsystem/PhoenixLoggingSubsystem.java b/src/main/java/frc/robot/subsystems/PhoenixLoggingSubsystem/PhoenixLoggingSubsystem.java index 7d4fc84..cc9a890 100644 --- a/src/main/java/frc/robot/subsystems/PhoenixLoggingSubsystem/PhoenixLoggingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PhoenixLoggingSubsystem/PhoenixLoggingSubsystem.java @@ -1,10 +1,15 @@ package frc.robot.subsystems.PhoenixLoggingSubsystem; +import javax.xml.crypto.Data; + import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.FieldManagementSubsystem.FieldManagementSubsystem; import frc.robot.subsystems.FieldManagementSubsystem.MatchStatus; +import frc.robot.subsystems.FieldManagementSubsystem.RobotStatus; public class PhoenixLoggingSubsystem extends SubsystemBase{ @@ -23,9 +28,20 @@ public void periodic(){ || currentMatchStatus == MatchStatus.TELEOP && !isLogging){ SignalLogger.start(); + DataLogManager.start(); + // DriverStation.startDataLog(DataLogManager.getLog()); + isLogging = true; } else if(currentMatchStatus == MatchStatus.ENDED){ SignalLogger.stop(); + DataLogManager.stop(); + isLogging = false; + } + else if(fieldManagementSubsystem.getRobotStatus() == RobotStatus.DISABLED){ + DataLogManager.stop(); + SignalLogger.stop(); + isLogging = false; } + // System.out.println("Log Dir: " + DataLogManager.getLogDir()); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java index c9bb245..3c4337c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java @@ -20,6 +20,9 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import frc.robot.util.GRTUtil; public class KrakenDriveMotor { @@ -43,6 +46,13 @@ public class KrakenDriveMotor { private StatusSignal supplyCurrentSignal; private StatusSignal statorCurrentSignal; //torqueCurrent is Pro + private DoubleLogEntry veloErrorLogEntry; + private DoubleLogEntry veloLogEntry; + private DoubleLogEntry targetVeloEntry; + private DoubleLogEntry appliedVoltsLogEntry; + private DoubleLogEntry supplyCurrLogEntry; + private DoubleLogEntry statorCurrLogEntry; + private DoubleLogEntry temperatureLogEntry; /** A kraken drive motor for swerve. * @@ -64,6 +74,7 @@ public KrakenDriveMotor(int canId) { initNT(canId); initSignals(); + initLogs(canId); } /** @@ -97,6 +108,19 @@ private void initSignals(){ motor.optimizeBusUtilization(0, 1.0); } + /** + * Initializes log entries + * @param canId drive motor's CAN ID + */ + private void initLogs(int canId){ + veloErrorLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "veloError"); + veloLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "velo"); + targetVeloEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "targetVelo"); + appliedVoltsLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "appliedVolts"); + supplyCurrLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "supplyCurrent"); + statorCurrLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "statorCurrent"); + temperatureLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "temperature"); + } /** * Set motor velo to target velo * @param metersPerSec target velo in m/s @@ -162,4 +186,31 @@ public void publishStats(){ statorCurrentPublisher.set(motor.getStatorCurrent().getValueAsDouble()); } -} + public void logStats(){ + veloErrorLogEntry.append( + motor.getClosedLoopError().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + veloLogEntry.append( + motor.getVelocity().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + targetVeloEntry.append(targetRps, GRTUtil.getFPGATime()); + + appliedVoltsLogEntry.append( + motor.getMotorVoltage().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + supplyCurrLogEntry.append( + motor.getSupplyCurrent().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + statorCurrLogEntry.append( + motor.getStatorCurrent().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + temperatureLogEntry.append( + motor.getDeviceTemp().getValueAsDouble(), GRTUtil.getFPGATime() + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java b/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java index a2c2aac..5e21a71 100644 --- a/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/NeoSteerMotor.java @@ -13,6 +13,9 @@ import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import frc.robot.util.GRTUtil; import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkClosedLoopController; @@ -37,6 +40,14 @@ public class NeoSteerMotor { private NetworkTable swerveStatsTable; private DoublePublisher neoPositionPublisher; private DoublePublisher neoSetPositionPublisher; + + private DoubleLogEntry positionLogEntry; + private DoubleLogEntry targetPositionLogEntry; + private DoubleLogEntry busVoltageLogEntry; + private DoubleLogEntry outputCurrtLogEntry; + private DoubleLogEntry temperatureLogEntry; + private DoubleLogEntry appliedOutputLogEntry; //pplied output duty cycle. + private double targetDouble = 0; /** @@ -77,6 +88,7 @@ public NeoSteerMotor(int canId) { // motor.burnFlash(); initNT(canId); + initLogs(canId); } /** @@ -98,7 +110,7 @@ public void configurePID(double p, double i, double d, double ff){ * @param targetRads target position in radiants */ public void setPosition(double targetRads) { - double targetDouble = (targetRads + Math.PI) / (2. * Math.PI); + targetDouble = (targetRads + Math.PI) / (2. * Math.PI); steerPIDController.setReference(targetDouble, ControlType.kPosition); } @@ -114,12 +126,32 @@ public double getPosition(){ * Initializes NetworkTables * @param canId */ - public void initNT (int canId){ + private void initNT (int canId){ ntInstance = NetworkTableInstance.getDefault(); swerveStatsTable = ntInstance.getTable(SWERVE_TABLE); neoPositionPublisher = swerveStatsTable.getDoubleTopic(canId + "neoPosition").publish(); neoSetPositionPublisher = swerveStatsTable.getDoubleTopic(canId + "neoSetPosition").publish(); } + + private void initLogs(int canId){ + positionLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "position"); + + targetPositionLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "targetPosition"); + + busVoltageLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "busVoltage"); + + outputCurrtLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "outputCurrent"); + + temperatureLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "temperature"); + + appliedOutputLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "appliedOutput"); + } /** * Publishes Neo stats to NT @@ -128,4 +160,22 @@ public void publishStats(){ neoPositionPublisher.set(getPosition()); neoSetPositionPublisher.set(targetDouble); } + + public void logStats(){ + positionLogEntry.append(getPosition(), GRTUtil.getFPGATime()); + targetPositionLogEntry.append(targetDouble, GRTUtil.getFPGATime()); + busVoltageLogEntry.append(motor.getBusVoltage(), GRTUtil.getFPGATime()); + + outputCurrtLogEntry.append( + motor.getOutputCurrent(), GRTUtil.getFPGATime() + ); + + temperatureLogEntry.append( + motor.getMotorTemperature(), GRTUtil.getFPGATime() + ); + + appliedOutputLogEntry.append( + motor.getAppliedOutput(), GRTUtil.getFPGATime() + ); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index aa87046..27d50e0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -152,6 +152,11 @@ public void publishSteerStats(){ steerMotor.publishStats(); } + public void logStats(){ + driveMotor.logStats(); + steerMotor.logStats(); + } + public void steerDebug(){ NetworkTableInstance.getDefault().getTable("steerDebug") .getEntry(steerPort + "PIDF") diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 5c474aa..30e6d56 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -11,8 +11,11 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.util.datalog.StructLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.GRTUtil; import static frc.robot.Constants.SwerveConstants.*; import static frc.robot.Constants.LoggingConstants.*; @@ -46,6 +49,12 @@ public class SwerveSubsystem extends SubsystemBase { private StructArrayPublisher swerveStatesPublisher; private StructPublisher estimatedPosePublisher; + private StructLogEntry estimatedPoseLogEntry = + StructLogEntry.create( + DataLogManager.getLog(), + "estimatedPose", + Pose2d.struct + ); public SwerveSubsystem() { ahrs = new AHRS(NavXComType.kMXP_SPI); @@ -86,8 +95,10 @@ public void periodic() { gyroAngle, getModulePositions() ); - + + estimatedPoseLogEntry.append(estimatedPose, GRTUtil.getFPGATime()); publishStats(); + logStats(); } /** @@ -249,6 +260,13 @@ private void publishStats(){ } } + private void logStats(){ + frontLeftModule.logStats(); + frontRightModule.logStats(); + backLeftModule.logStats(); + backRightModule.logStats(); + } + /** * Enables drive debug */ diff --git a/src/main/java/frc/robot/util/GRTUtil.java b/src/main/java/frc/robot/util/GRTUtil.java new file mode 100644 index 0000000..53d4ad7 --- /dev/null +++ b/src/main/java/frc/robot/util/GRTUtil.java @@ -0,0 +1,14 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.RobotController; + +public class GRTUtil { + /** + * Get the current time in seconds + * @return seconds + */ + public static long getFPGATime(){ + return (long) (RobotController.getFPGATime()); + } +} + From 16a288ae09c64b908cd0a54379999ca713b09803 Mon Sep 17 00:00:00 2001 From: Jesse Date: Tue, 21 Jan 2025 17:02:59 -0800 Subject: [PATCH 17/44] Add position as part of the kraken log --- .../java/frc/robot/subsystems/swerve/KrakenDriveMotor.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java index 3c4337c..c7de691 100644 --- a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java @@ -46,6 +46,7 @@ public class KrakenDriveMotor { private StatusSignal supplyCurrentSignal; private StatusSignal statorCurrentSignal; //torqueCurrent is Pro + private DoubleLogEntry positionLogEntry; private DoubleLogEntry veloErrorLogEntry; private DoubleLogEntry veloLogEntry; private DoubleLogEntry targetVeloEntry; @@ -113,6 +114,7 @@ private void initSignals(){ * @param canId drive motor's CAN ID */ private void initLogs(int canId){ + positionLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "position"); veloErrorLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "veloError"); veloLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "velo"); targetVeloEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "targetVelo"); @@ -187,6 +189,10 @@ public void publishStats(){ } public void logStats(){ + positionLogEntry.append( + motor.getPosition().getValueAsDouble(), GRTUtil.getFPGATime() + ); + veloErrorLogEntry.append( motor.getClosedLoopError().getValueAsDouble(), GRTUtil.getFPGATime() ); From 588ba4547e1b3da188d6a129c9fdff401429a7f7 Mon Sep 17 00:00:00 2001 From: Jesse Date: Tue, 21 Jan 2025 17:03:28 -0800 Subject: [PATCH 18/44] Only start log once and don't use phoenix signal logs --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b018e86..10039ad 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,13 +36,13 @@ public class RobotContainer { private final FieldManagementSubsystem fieldManagementSubsystem = new FieldManagementSubsystem(); - private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = - new PhoenixLoggingSubsystem(fieldManagementSubsystem); + // private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = + // new PhoenixLoggingSubsystem(fieldManagementSubsystem); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { constructDriveController(); - // startLog(); + startLog(); configureBindings(); } @@ -123,6 +123,6 @@ else if(DriverStation.getJoystickName(0).equals("DualSense Wireless Controller") */ private void startLog(){ DataLogManager.start(); - // DriverStation.startDataLog(DataLogManager.getLog()); + DriverStation.startDataLog(DataLogManager.getLog()); } } From dc8c2c1c48bc40e3b8f44910d2c8f514c6096821 Mon Sep 17 00:00:00 2001 From: Jesse Date: Tue, 21 Jan 2025 17:03:40 -0800 Subject: [PATCH 19/44] remove phoenix signal logs --- .../PhoenixLoggingSubsystem.java | 41 ++++++++++--------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/PhoenixLoggingSubsystem/PhoenixLoggingSubsystem.java b/src/main/java/frc/robot/subsystems/PhoenixLoggingSubsystem/PhoenixLoggingSubsystem.java index cc9a890..1657961 100644 --- a/src/main/java/frc/robot/subsystems/PhoenixLoggingSubsystem/PhoenixLoggingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PhoenixLoggingSubsystem/PhoenixLoggingSubsystem.java @@ -18,30 +18,31 @@ public class PhoenixLoggingSubsystem extends SubsystemBase{ public PhoenixLoggingSubsystem(FieldManagementSubsystem fieldManagementSubsystem){ this.fieldManagementSubsystem = fieldManagementSubsystem; - SignalLogger.setPath("/media/sda1/ctre-logs/"); + SignalLogger.setPath("/u/ctre-logs/"); + SignalLogger.start(); } @Override public void periodic(){ - MatchStatus currentMatchStatus = fieldManagementSubsystem.getMatchStatus(); - if(currentMatchStatus == MatchStatus.AUTON - || currentMatchStatus == MatchStatus.TELEOP - && !isLogging){ - SignalLogger.start(); - DataLogManager.start(); - // DriverStation.startDataLog(DataLogManager.getLog()); - isLogging = true; - } - else if(currentMatchStatus == MatchStatus.ENDED){ - SignalLogger.stop(); - DataLogManager.stop(); - isLogging = false; - } - else if(fieldManagementSubsystem.getRobotStatus() == RobotStatus.DISABLED){ - DataLogManager.stop(); - SignalLogger.stop(); - isLogging = false; - } + // MatchStatus currentMatchStatus = fieldManagementSubsystem.getMatchStatus(); + // if(currentMatchStatus == MatchStatus.AUTON + // || currentMatchStatus == MatchStatus.TELEOP + // && !isLogging){ + // SignalLogger.start(); + // DataLogManager.start(); + // // DriverStation.startDataLog(DataLogManager.getLog()); + // isLogging = true; + // } + // else if(currentMatchStatus == MatchStatus.ENDED){ + // SignalLogger.stop(); + // DataLogManager.stop(); + // isLogging = false; + // } + // else if(fieldManagementSubsystem.getRobotStatus() == RobotStatus.DISABLED){ + // DataLogManager.stop(); + // SignalLogger.stop(); + // isLogging = false; + // } // System.out.println("Log Dir: " + DataLogManager.getLogDir()); } } From aa5a78376820cecfead1f61532754246c0b01060 Mon Sep 17 00:00:00 2001 From: Jesse Date: Mon, 27 Jan 2025 19:59:18 -0800 Subject: [PATCH 20/44] Untested class --- .../frc/robot/util/Motors/LoggedSparkMax.java | 141 ++++++++++++++++++ .../util/Motors/LoggedSparkMaxConfig.java | 39 +++++ 2 files changed, 180 insertions(+) create mode 100644 src/main/java/frc/robot/util/Motors/LoggedSparkMax.java create mode 100644 src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java new file mode 100644 index 0000000..5caa7e7 --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -0,0 +1,141 @@ +package frc.robot.util.Motors; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import frc.robot.util.GRTUtil; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; + +public class LoggedSparkMax { + + private final SparkMax motor; + private final SparkMaxConfig sparkMaxConfig; + private final ClosedLoopConfig closedLoopConfig; + private final RelativeEncoder encoder; + private SparkClosedLoopController closedLoopController; + private double targetPosition; + + private NetworkTableInstance ntInstance; + private NetworkTable motorStatsTable; + private DoublePublisher neoPositionPublisher; + private DoublePublisher neoSetPositionPublisher; + + private DoubleLogEntry positionLogEntry; + private DoubleLogEntry targetPositionLogEntry; + private DoubleLogEntry busVoltageLogEntry; + private DoubleLogEntry outputCurrtLogEntry; + private DoubleLogEntry temperatureLogEntry; + private DoubleLogEntry appliedOutputLogEntry; + + public LoggedSparkMax(LoggedSparkMaxConfig config) { + motor = new SparkMax(config.getCanId(), MotorType.kBrushless); + sparkMaxConfig = config.getSparkMaxConfig(); + closedLoopConfig = config.getClosedLoopConfig(); + motor.configure(sparkMaxConfig, + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ); + closedLoopController = motor.getClosedLoopController(); + encoder = motor.getEncoder(); + + initLogs(config.getCanId()); + intiNT(config.getCanId()); + } + + public void configurePIDF(double p, double i, double d, double f) { + closedLoopConfig.pidf(p, i, d, f); + sparkMaxConfig.apply(closedLoopConfig); + motor.configure(sparkMaxConfig, + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ); + closedLoopController = motor.getClosedLoopController(); + } + + public void setPosition(double position){ + targetPosition = position; + closedLoopController.setReference(position, ControlType.kPosition); + } + + public double getPosition(){ + return encoder.getPosition(); + } + + /** + * Initializes NetworkTables + * @param canId motor's CAN ID + */ + private void intiNT(int canId) { + ntInstance = NetworkTableInstance.getDefault(); + motorStatsTable = ntInstance.getTable("MotorStats"); + neoPositionPublisher = motorStatsTable.getDoubleTopic( + canId + "neoPosition" + ).publish(); + neoSetPositionPublisher = motorStatsTable.getDoubleTopic( + canId + "neoSetPosition" + ).publish(); + } + + /** + * Initializes logs + * @param canId motor's CAN ID + */ + private void initLogs(int canId){ + positionLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "position"); + + targetPositionLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "targetPosition"); + + busVoltageLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "busVoltage"); + + outputCurrtLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "outputCurrent"); + + temperatureLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "temperature"); + + appliedOutputLogEntry = + new DoubleLogEntry(DataLogManager.getLog(), canId + "appliedOutput"); + } + + /** + * Publishes Neo stats to NT + */ + public void publishStats(){ + neoPositionPublisher.set(getPosition()); + neoSetPositionPublisher.set(targetPosition); + } + + /** + * Logs Neo stats into log file + */ + public void logStats(){ + positionLogEntry.append(getPosition(), GRTUtil.getFPGATime()); + targetPositionLogEntry.append(targetPosition, GRTUtil.getFPGATime()); + busVoltageLogEntry.append(motor.getBusVoltage(), GRTUtil.getFPGATime()); + + outputCurrtLogEntry.append( + motor.getOutputCurrent(), GRTUtil.getFPGATime() + ); + + temperatureLogEntry.append( + motor.getMotorTemperature(), GRTUtil.getFPGATime() + ); + + appliedOutputLogEntry.append( + motor.getAppliedOutput(), GRTUtil.getFPGATime() + ); + } +} diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java new file mode 100644 index 0000000..461638e --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java @@ -0,0 +1,39 @@ +package frc.robot.util.Motors; + +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class LoggedSparkMaxConfig { + private final int canId; + private final ClosedLoopConfig closedLoopConfig; + private final SparkMaxConfig sparkMaxConfig; + private final EncoderConfig encoderConfig; + + public LoggedSparkMaxConfig( + int canId, ClosedLoopConfig closedLoopConfig, EncoderConfig encoderConfig){ + this.canId = canId; + this.closedLoopConfig = closedLoopConfig; + this.encoderConfig = encoderConfig; + this.sparkMaxConfig = new SparkMaxConfig(); + this.sparkMaxConfig.apply(closedLoopConfig); + this.sparkMaxConfig.apply(encoderConfig); + } + + public int getCanId() { + return canId; + } + + public ClosedLoopConfig getClosedLoopConfig() { + return closedLoopConfig; + } + + public SparkMaxConfig getSparkMaxConfig() { + return sparkMaxConfig; + } + + public EncoderConfig getEncoderConfig() { + return encoderConfig; + } + +} From ee5999482860d260142391e4a7404f23446ddd2f Mon Sep 17 00:00:00 2001 From: Jesse Date: Mon, 27 Jan 2025 20:28:38 -0800 Subject: [PATCH 21/44] Untested Talon Class --- src/main/java/frc/robot/Constants.java | 2 + .../subsystems/swerve/KrakenDriveMotor.java | 1 + .../frc/robot/util/Motors/LoggedTalon.java | 170 ++++++++++++++++++ 3 files changed, 173 insertions(+) create mode 100644 src/main/java/frc/robot/util/Motors/LoggedTalon.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 45ad9d0..69b80bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -90,5 +90,7 @@ public static class SwerveConstants { public static class LoggingConstants{ public static final String SWERVE_TABLE = "SwerveStats"; + public static final String REV_TABLE = "MotorStats"; + public static final String CTRE_TABLE = "MotorStats"; } } diff --git a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java index c7de691..879156b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java @@ -123,6 +123,7 @@ private void initLogs(int canId){ statorCurrLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "statorCurrent"); temperatureLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "temperature"); } + /** * Set motor velo to target velo * @param metersPerSec target velo in m/s diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java new file mode 100644 index 0000000..8be8020 --- /dev/null +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -0,0 +1,170 @@ +package frc.robot.util.Motors; + +import static frc.robot.Constants.LoggingConstants.CTRE_TABLE; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import frc.robot.util.GRTUtil; + +public class LoggedTalon{ + + private final TalonFX motor; + + private NetworkTableInstance ntInstance; + private NetworkTable swerveStatsTable; + private DoublePublisher veloErrorPublisher; + private DoublePublisher veloPublisher; + private DoublePublisher appliedVlotsPublisher; + private DoublePublisher supplyCurrentPublisher; + private DoublePublisher statorCurrentPublisher; + + private DoubleLogEntry positionLogEntry; + private DoubleLogEntry veloErrorLogEntry; + private DoubleLogEntry veloLogEntry; + private DoubleLogEntry appliedVoltsLogEntry; + private DoubleLogEntry supplyCurrLogEntry; + private DoubleLogEntry statorCurrLogEntry; + private DoubleLogEntry temperatureLogEntry; + + public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ + motor = new TalonFX(canId, "can"); + for (int i = 0; i < 4; i++) { + boolean error = motor.getConfigurator().apply(talonConfig, 0.1) == StatusCode.OK; + if (!error) break; + } + initNT(canId); + initLogs(canId); + } + + /** + * initializes network table and entries + * @param canId motor's CAN ID + */ + private void initNT(int canId){ + ntInstance = NetworkTableInstance.getDefault(); + swerveStatsTable = ntInstance.getTable(CTRE_TABLE); + veloErrorPublisher = swerveStatsTable.getDoubleTopic( + canId + "veloError" + ).publish(); + veloPublisher = swerveStatsTable.getDoubleTopic(canId + "velo").publish(); + appliedVlotsPublisher = swerveStatsTable.getDoubleTopic( + canId + "appliedVolts" + ).publish(); + supplyCurrentPublisher = swerveStatsTable.getDoubleTopic( + canId + "supplyCurrent" + ).publish(); + statorCurrentPublisher = swerveStatsTable.getDoubleTopic( + canId + "statorCurrent" + ).publish(); + } + + /** + * Initializes log entries + * @param canId drive motor's CAN ID + */ + private void initLogs(int canId){ + positionLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "position" + ); + veloErrorLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "veloError" + ); + veloLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "velo" + ); + appliedVoltsLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "appliedVolts" + ); + supplyCurrLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "supplyCurrent" + ); + statorCurrLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "statorCurrent" + ); + temperatureLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "temperature" + ); + } + + /** + * Configures drive motor's PIDSV + * @param p kP + * @param i kI + * @param d kD + * @param s kS for static friction + * @param v kV Voltage feed forward + */ + public void configPID(double p, double i, double d, double s, double v) { + Slot0Configs slot0Configs = new Slot0Configs(); + + slot0Configs.kP = p; + slot0Configs.kI = i; + slot0Configs.kD = d; + slot0Configs.kS = s; + slot0Configs.kV = v; + + motor.getConfigurator().apply(slot0Configs); + } + + public double getPosition(){ + return motor.getPosition().getValueAsDouble(); + } + + public double getVelocity(){ + return motor.getVelocity().getValueAsDouble(); + } + + /** + * Publishes motor stats to NT for logging + */ + public void publishStats(){ + veloErrorPublisher.set(motor.getClosedLoopError().getValueAsDouble()); + veloPublisher.set(motor.getVelocity().getValueAsDouble()); + appliedVlotsPublisher.set(motor.getMotorVoltage().getValueAsDouble()); + supplyCurrentPublisher.set(motor.getSupplyCurrent().getValueAsDouble()); + statorCurrentPublisher.set(motor.getStatorCurrent().getValueAsDouble()); + } + + public void logStats(){ + positionLogEntry.append( + motor.getPosition().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + veloErrorLogEntry.append( + motor.getClosedLoopError().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + veloLogEntry.append( + motor.getVelocity().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + appliedVoltsLogEntry.append( + motor.getMotorVoltage().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + supplyCurrLogEntry.append( + motor.getSupplyCurrent().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + statorCurrLogEntry.append( + motor.getStatorCurrent().getValueAsDouble(), GRTUtil.getFPGATime() + ); + + temperatureLogEntry.append( + motor.getDeviceTemp().getValueAsDouble(), GRTUtil.getFPGATime() + ); + } + + public void setPosition(double position){ + motor.setControl(new PositionVoltage(position)); + } +} From 612f9140dd68933cc6e710ba299437e7ba0ef9fa Mon Sep 17 00:00:00 2001 From: Jesse Date: Tue, 28 Jan 2025 19:15:57 -0800 Subject: [PATCH 22/44] Added methods to accomodate usages in existing branches --- .../frc/robot/util/Motors/LoggedSparkMax.java | 59 +++++++- .../frc/robot/util/Motors/LoggedTalon.java | 130 ++++++++++++------ 2 files changed, 142 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java index 5caa7e7..4119a4f 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -12,11 +12,13 @@ import frc.robot.util.GRTUtil; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; public class LoggedSparkMax { @@ -25,7 +27,7 @@ public class LoggedSparkMax { private final ClosedLoopConfig closedLoopConfig; private final RelativeEncoder encoder; private SparkClosedLoopController closedLoopController; - private double targetPosition; + private double targetReference; private NetworkTableInstance ntInstance; private NetworkTable motorStatsTable; @@ -53,6 +55,13 @@ public LoggedSparkMax(LoggedSparkMaxConfig config) { intiNT(config.getCanId()); } + /** + * Configures the PIDF values for the motor + * @param p kP value + * @param i kI value + * @param d kD value + * @param f kF value + */ public void configurePIDF(double p, double i, double d, double f) { closedLoopConfig.pidf(p, i, d, f); sparkMaxConfig.apply(closedLoopConfig); @@ -62,11 +71,47 @@ public void configurePIDF(double p, double i, double d, double f) { closedLoopController = motor.getClosedLoopController(); } - public void setPosition(double position){ - targetPosition = position; - closedLoopController.setReference(position, ControlType.kPosition); + /** + * Sets the target reference for the motor + * @param value value of the reference + * @param controlType type of control (position, velocity, etc.) + */ + public void setReference(double value, ControlType controlType){ + targetReference = value; + closedLoopController.setReference(value, controlType); } + /** + * Sets the target reference for the motor + * @param value value of the reference + * @param controlType type of control (position, velocity, etc.) + * @param closedLoopSlot closed loop slot + * @param arbFF arbitrary feed forward + * @param arbFFUnits arbitrary feed forward units + */ + public void setReference(double value, ControlType controlType, + ClosedLoopSlot closedLoopSlot, double arbFF, + ArbFFUnits arbFFUnits + ){ + targetReference = value; + closedLoopController.setReference(value, ControlType.kPosition, + closedLoopSlot, arbFF, arbFFUnits + ); + } + + /** + * Sets the motor's speed to a specific value + * @param value value of the speed from -1.0 to 1.0 + */ + public void set(double value){ + motor.set(value); + } + + /** + * Gets the position of the motor in rotations after taking the position + * conversion factor into account + * @return position of the motor in rotations + */ public double getPosition(){ return encoder.getPosition(); } @@ -115,7 +160,7 @@ private void initLogs(int canId){ */ public void publishStats(){ neoPositionPublisher.set(getPosition()); - neoSetPositionPublisher.set(targetPosition); + neoSetPositionPublisher.set(targetReference); } /** @@ -123,7 +168,7 @@ public void publishStats(){ */ public void logStats(){ positionLogEntry.append(getPosition(), GRTUtil.getFPGATime()); - targetPositionLogEntry.append(targetPosition, GRTUtil.getFPGATime()); + targetPositionLogEntry.append(targetReference, GRTUtil.getFPGATime()); busVoltageLogEntry.append(motor.getBusVoltage(), GRTUtil.getFPGATime()); outputCurrtLogEntry.append( diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index 8be8020..166f500 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.networktables.DoublePublisher; @@ -20,20 +21,28 @@ public class LoggedTalon{ private final TalonFX motor; private NetworkTableInstance ntInstance; - private NetworkTable swerveStatsTable; - private DoublePublisher veloErrorPublisher; + private NetworkTable motorStatsTable; + + private DoublePublisher positionPublisher; private DoublePublisher veloPublisher; private DoublePublisher appliedVlotsPublisher; private DoublePublisher supplyCurrentPublisher; private DoublePublisher statorCurrentPublisher; + private DoublePublisher temperaturePublisher; + private DoublePublisher targetPositionPublisher; + private DoublePublisher targetVelocityPublisher; private DoubleLogEntry positionLogEntry; - private DoubleLogEntry veloErrorLogEntry; private DoubleLogEntry veloLogEntry; private DoubleLogEntry appliedVoltsLogEntry; private DoubleLogEntry supplyCurrLogEntry; private DoubleLogEntry statorCurrLogEntry; private DoubleLogEntry temperatureLogEntry; + private DoubleLogEntry targetPositionLogEntry; + private DoubleLogEntry targetVelocityLogEntry; + + private double targetPosition; + private double targetVelocity; public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ motor = new TalonFX(canId, "can"); @@ -45,26 +54,73 @@ public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ initLogs(canId); } + /** + * Using PIDF to set the motor's position + * @param position position reference + */ + public void setPosition(double position){ + targetPosition = position; + motor.setControl(new PositionVoltage(position)); + } + + /** + * Using PIDF to set the motor's velocity + * @param velocity$ velocity reference + */ + public void setVelocity(double velocity){ + targetVelocity = velocity; + motor.setControl(new VelocityVoltage(velocity)); + } + + /** + * Configures drive motor's PIDSV + * @param p kP + * @param i kI + * @param d kD + * @param s kS for static friction + * @param v kV Voltage feed forward + */ + public void configPID(double p, double i, double d, double s, double v) { + Slot0Configs slot0Configs = new Slot0Configs(); + + slot0Configs.kP = p; + slot0Configs.kI = i; + slot0Configs.kD = d; + slot0Configs.kS = s; + slot0Configs.kV = v; + + motor.getConfigurator().apply(slot0Configs); + } + /** * initializes network table and entries * @param canId motor's CAN ID */ private void initNT(int canId){ ntInstance = NetworkTableInstance.getDefault(); - swerveStatsTable = ntInstance.getTable(CTRE_TABLE); - veloErrorPublisher = swerveStatsTable.getDoubleTopic( - canId + "veloError" + motorStatsTable = ntInstance.getTable(CTRE_TABLE); + positionPublisher = motorStatsTable.getDoubleTopic( + canId + "position" ).publish(); - veloPublisher = swerveStatsTable.getDoubleTopic(canId + "velo").publish(); - appliedVlotsPublisher = swerveStatsTable.getDoubleTopic( + veloPublisher = motorStatsTable.getDoubleTopic(canId + "velo").publish(); + appliedVlotsPublisher = motorStatsTable.getDoubleTopic( canId + "appliedVolts" ).publish(); - supplyCurrentPublisher = swerveStatsTable.getDoubleTopic( + supplyCurrentPublisher = motorStatsTable.getDoubleTopic( canId + "supplyCurrent" ).publish(); - statorCurrentPublisher = swerveStatsTable.getDoubleTopic( + statorCurrentPublisher = motorStatsTable.getDoubleTopic( canId + "statorCurrent" ).publish(); + temperaturePublisher = motorStatsTable.getDoubleTopic( + canId + "temperature" + ).publish(); + targetPositionPublisher = motorStatsTable.getDoubleTopic( + canId + "targetPosition" + ).publish(); + targetVelocityPublisher = motorStatsTable.getDoubleTopic( + canId + "targetVelocity" + ).publish(); } /** @@ -75,9 +131,6 @@ private void initLogs(int canId){ positionLogEntry = new DoubleLogEntry( DataLogManager.getLog(), canId + "position" ); - veloErrorLogEntry = new DoubleLogEntry( - DataLogManager.getLog(), canId + "veloError" - ); veloLogEntry = new DoubleLogEntry( DataLogManager.getLog(), canId + "velo" ); @@ -93,32 +146,28 @@ private void initLogs(int canId){ temperatureLogEntry = new DoubleLogEntry( DataLogManager.getLog(), canId + "temperature" ); + targetPositionLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "targetPosition" + ); + targetVelocityLogEntry = new DoubleLogEntry( + DataLogManager.getLog(), canId + "targetVelocity" + ); } /** - * Configures drive motor's PIDSV - * @param p kP - * @param i kI - * @param d kD - * @param s kS for static friction - * @param v kV Voltage feed forward + * Gets motor's position in rotations after taking the + * SensorToMechanismRatio into account + * @return position of the motor in rotations */ - public void configPID(double p, double i, double d, double s, double v) { - Slot0Configs slot0Configs = new Slot0Configs(); - - slot0Configs.kP = p; - slot0Configs.kI = i; - slot0Configs.kD = d; - slot0Configs.kS = s; - slot0Configs.kV = v; - - motor.getConfigurator().apply(slot0Configs); - } - public double getPosition(){ return motor.getPosition().getValueAsDouble(); } + /** + * Gets motor's velocity in RPS after taking the SensorToMechanismRatio into + * account + * @return velocity of the motor in RPS + */ public double getVelocity(){ return motor.getVelocity().getValueAsDouble(); } @@ -127,22 +176,24 @@ public double getVelocity(){ * Publishes motor stats to NT for logging */ public void publishStats(){ - veloErrorPublisher.set(motor.getClosedLoopError().getValueAsDouble()); + positionPublisher.set(motor.getPosition().getValueAsDouble()); veloPublisher.set(motor.getVelocity().getValueAsDouble()); appliedVlotsPublisher.set(motor.getMotorVoltage().getValueAsDouble()); supplyCurrentPublisher.set(motor.getSupplyCurrent().getValueAsDouble()); statorCurrentPublisher.set(motor.getStatorCurrent().getValueAsDouble()); + temperaturePublisher.set(motor.getDeviceTemp().getValueAsDouble()); + targetPositionPublisher.set(targetPosition); + targetVelocityPublisher.set(targetVelocity); } + /** + * Loggs motor stats into log file + */ public void logStats(){ positionLogEntry.append( motor.getPosition().getValueAsDouble(), GRTUtil.getFPGATime() ); - veloErrorLogEntry.append( - motor.getClosedLoopError().getValueAsDouble(), GRTUtil.getFPGATime() - ); - veloLogEntry.append( motor.getVelocity().getValueAsDouble(), GRTUtil.getFPGATime() ); @@ -162,9 +213,8 @@ public void logStats(){ temperatureLogEntry.append( motor.getDeviceTemp().getValueAsDouble(), GRTUtil.getFPGATime() ); - } - - public void setPosition(double position){ - motor.setControl(new PositionVoltage(position)); + + targetPositionLogEntry.append(targetPosition, GRTUtil.getFPGATime()); + targetVelocityLogEntry.append(targetVelocity, GRTUtil.getFPGATime()); } } From 681abdd3cbad93363d5746d36a2e6dd94e70eca0 Mon Sep 17 00:00:00 2001 From: Jesse Date: Tue, 28 Jan 2025 20:47:56 -0800 Subject: [PATCH 23/44] Enable PIDF tuning through NT + Logging more things for spark max. --- src/main/java/frc/robot/Constants.java | 12 +- .../subsystems/swerve/SwerveSubsystem.java | 1 + .../frc/robot/util/Motors/LoggedSparkMax.java | 117 ++++++++++++++---- 3 files changed, 99 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 69b80bb..5648985 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -82,10 +82,6 @@ public static class SwerveConstants { public static final double[] STEER_I = new double[] {0, 0, 0, 0}; public static final double[] STEER_D = new double[] {0, 0, 0, 0}; public static final double[] STEER_FF = new double[] {0.036, 0.024, 0.0182, 0.05}; - - public static final boolean DRIVE_DEBUG = false; - public static final boolean STEER_DEBUG = false; - public static final boolean STATE_DEBUG = false; } public static class LoggingConstants{ @@ -93,4 +89,12 @@ public static class LoggingConstants{ public static final String REV_TABLE = "MotorStats"; public static final String CTRE_TABLE = "MotorStats"; } + + public static class DebugConstants{ + public static final boolean STATE_DEBUG = false; + public static final boolean DRIVE_DEBUG = false; + public static final boolean STEER_DEBUG = false; + public static final boolean REV_DEBUG = false; + public static final boolean CTRE_DEBUG = false; + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 30e6d56..54371d6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -19,6 +19,7 @@ import static frc.robot.Constants.SwerveConstants.*; import static frc.robot.Constants.LoggingConstants.*; +import static frc.robot.Constants.DebugConstants.*; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java index 4119a4f..218b428 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMax.java @@ -6,11 +6,17 @@ import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import frc.robot.util.GRTUtil; +import static frc.robot.Constants.LoggingConstants.REV_TABLE; +import static frc.robot.Constants.DebugConstants.REV_DEBUG; + +import java.util.EnumSet; + import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.ClosedLoopSlot; @@ -22,6 +28,7 @@ public class LoggedSparkMax { + private final int canId; private final SparkMax motor; private final SparkMaxConfig sparkMaxConfig; private final ClosedLoopConfig closedLoopConfig; @@ -31,18 +38,25 @@ public class LoggedSparkMax { private NetworkTableInstance ntInstance; private NetworkTable motorStatsTable; - private DoublePublisher neoPositionPublisher; - private DoublePublisher neoSetPositionPublisher; + private DoublePublisher positionPublisher; + private DoublePublisher velocityPublisher; + private DoublePublisher busVoltagePublisher; + private DoublePublisher outputCurrentPublisher; + private DoublePublisher appliedOutputPublisher; + private DoublePublisher temperaturePublisher; + private DoublePublisher targetReferencePublisher; private DoubleLogEntry positionLogEntry; - private DoubleLogEntry targetPositionLogEntry; + private DoubleLogEntry velocityLogEntry; private DoubleLogEntry busVoltageLogEntry; private DoubleLogEntry outputCurrtLogEntry; - private DoubleLogEntry temperatureLogEntry; private DoubleLogEntry appliedOutputLogEntry; + private DoubleLogEntry temperatureLogEntry; + private DoubleLogEntry targetReferenceLogEntry; public LoggedSparkMax(LoggedSparkMaxConfig config) { - motor = new SparkMax(config.getCanId(), MotorType.kBrushless); + canId = config.getCanId(); + motor = new SparkMax(canId, MotorType.kBrushless); sparkMaxConfig = config.getSparkMaxConfig(); closedLoopConfig = config.getClosedLoopConfig(); motor.configure(sparkMaxConfig, @@ -51,8 +65,11 @@ public LoggedSparkMax(LoggedSparkMaxConfig config) { closedLoopController = motor.getClosedLoopController(); encoder = motor.getEncoder(); - initLogs(config.getCanId()); - intiNT(config.getCanId()); + initLogs(); + intiNT(); + if(REV_DEBUG){ + enableDebug(); + } } /** @@ -116,18 +133,41 @@ public double getPosition(){ return encoder.getPosition(); } + /** + * Gets the velocity of the motor in RPM after taking the velocity + * conversion factor into account + */ + public double getVelocity(){ + return encoder.getVelocity(); + } + /** * Initializes NetworkTables * @param canId motor's CAN ID */ - private void intiNT(int canId) { + private void intiNT() { ntInstance = NetworkTableInstance.getDefault(); - motorStatsTable = ntInstance.getTable("MotorStats"); - neoPositionPublisher = motorStatsTable.getDoubleTopic( - canId + "neoPosition" + motorStatsTable = ntInstance.getTable(REV_TABLE); + positionPublisher = motorStatsTable.getDoubleTopic( + canId + "Position" ).publish(); - neoSetPositionPublisher = motorStatsTable.getDoubleTopic( - canId + "neoSetPosition" + velocityPublisher = motorStatsTable.getDoubleTopic( + canId + "Velocity" + ).publish(); + busVoltagePublisher = motorStatsTable.getDoubleTopic( + canId + "BusVoltage" + ).publish(); + outputCurrentPublisher = motorStatsTable.getDoubleTopic( + canId + "OutputCurrent" + ).publish(); + appliedOutputPublisher = motorStatsTable.getDoubleTopic( + canId + "AppliedOutput" + ).publish(); + temperaturePublisher = motorStatsTable.getDoubleTopic( + canId + "Temperature" + ).publish(); + targetReferencePublisher = motorStatsTable.getDoubleTopic( + canId + "TargetReference" ).publish(); } @@ -135,12 +175,12 @@ private void intiNT(int canId) { * Initializes logs * @param canId motor's CAN ID */ - private void initLogs(int canId){ + private void initLogs(){ positionLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "position"); - targetPositionLogEntry = - new DoubleLogEntry(DataLogManager.getLog(), canId + "targetPosition"); + targetReferenceLogEntry= + new DoubleLogEntry(DataLogManager.getLog(), canId + "targetReference"); busVoltageLogEntry = new DoubleLogEntry(DataLogManager.getLog(), canId + "busVoltage"); @@ -156,31 +196,54 @@ private void initLogs(int canId){ } /** - * Publishes Neo stats to NT + * Allows for PIDF tuning in NT + */ + private void enableDebug(){ + motorStatsTable.getDoubleArrayTopic(canId + "PIDF").publish().set( + new double[] {0., 0., 0., 0.} + ); + motorStatsTable.addListener(canId + "PIDF", + EnumSet.of(NetworkTableEvent.Kind.kValueAll), + (table, key, event) -> { + configurePIDF( + event.valueData.value.getDoubleArray()[0], + event.valueData.value.getDoubleArray()[1], + event.valueData.value.getDoubleArray()[2], + event.valueData.value.getDoubleArray()[3] + ); + } + ); + } + + /** + * Publishes stats to NT */ public void publishStats(){ - neoPositionPublisher.set(getPosition()); - neoSetPositionPublisher.set(targetReference); + positionPublisher.set(getPosition()); + velocityPublisher.set(getVelocity()); + busVoltagePublisher.set(motor.getBusVoltage()); + outputCurrentPublisher.set(motor.getOutputCurrent()); + appliedOutputPublisher.set(motor.getAppliedOutput()); + temperaturePublisher.set(motor.getMotorTemperature()); + targetReferencePublisher.set(targetReference); } /** - * Logs Neo stats into log file + * Logs stats into log file */ public void logStats(){ positionLogEntry.append(getPosition(), GRTUtil.getFPGATime()); - targetPositionLogEntry.append(targetReference, GRTUtil.getFPGATime()); + velocityLogEntry.append(getVelocity(), GRTUtil.getFPGATime()); busVoltageLogEntry.append(motor.getBusVoltage(), GRTUtil.getFPGATime()); - outputCurrtLogEntry.append( motor.getOutputCurrent(), GRTUtil.getFPGATime() ); - - temperatureLogEntry.append( - motor.getMotorTemperature(), GRTUtil.getFPGATime() - ); - appliedOutputLogEntry.append( motor.getAppliedOutput(), GRTUtil.getFPGATime() ); + temperatureLogEntry.append( + motor.getMotorTemperature(), GRTUtil.getFPGATime() + ); + targetReferenceLogEntry.append(targetReference, GRTUtil.getFPGATime()); } } From 560def0116dbe554a036c48fa59c4358d269d6f3 Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 10:22:16 -0800 Subject: [PATCH 24/44] Allows changing talon pid through NT --- .../frc/robot/util/Motors/LoggedTalon.java | 45 ++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index 166f500..a816b15 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -1,6 +1,9 @@ package frc.robot.util.Motors; import static frc.robot.Constants.LoggingConstants.CTRE_TABLE; +import static frc.robot.Constants.DebugConstants.CTRE_DEBUG; + +import java.util.EnumSet; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.Slot0Configs; @@ -11,6 +14,7 @@ import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; @@ -20,6 +24,9 @@ public class LoggedTalon{ private final TalonFX motor; + private final int canId; + private double[] pidsvg = new double[6]; + private NetworkTableInstance ntInstance; private NetworkTable motorStatsTable; @@ -50,8 +57,20 @@ public LoggedTalon(int canId, TalonFXConfiguration talonConfig){ boolean error = motor.getConfigurator().apply(talonConfig, 0.1) == StatusCode.OK; if (!error) break; } + this.canId = canId; + pidsvg = new double[] { + talonConfig.Slot0.kP, + talonConfig.Slot0.kI, + talonConfig.Slot0.kD, + talonConfig.Slot0.kS, + talonConfig.Slot0.kV, + talonConfig.Slot0.kG + }; initNT(canId); initLogs(canId); + if(CTRE_DEBUG){ + enableDebug(); + } } /** @@ -80,7 +99,9 @@ public void setVelocity(double velocity){ * @param s kS for static friction * @param v kV Voltage feed forward */ - public void configPID(double p, double i, double d, double s, double v) { + public void configurePIDSVG(double p, double i, double d, double s, double v, + double g + ) { Slot0Configs slot0Configs = new Slot0Configs(); slot0Configs.kP = p; @@ -88,6 +109,7 @@ public void configPID(double p, double i, double d, double s, double v) { slot0Configs.kD = d; slot0Configs.kS = s; slot0Configs.kV = v; + slot0Configs.kG = g; motor.getConfigurator().apply(slot0Configs); } @@ -154,6 +176,27 @@ private void initLogs(int canId){ ); } + /** + * Allows changing PID through NT + */ + private void enableDebug(){ + motorStatsTable.getDoubleArrayTopic(canId + "PIDSVG").publish().set( + pidsvg + ); + motorStatsTable.addListener(canId + "PIDSVG", + EnumSet.of(NetworkTableEvent.Kind.kValueAll), + (table, key, event) -> { + configurePIDSVG( + event.valueData.value.getDoubleArray()[0], + event.valueData.value.getDoubleArray()[1], + event.valueData.value.getDoubleArray()[2], + event.valueData.value.getDoubleArray()[3], + event.valueData.value.getDoubleArray()[4], + event.valueData.value.getDoubleArray()[5] + ); + } + ); + } /** * Gets motor's position in rotations after taking the * SensorToMechanismRatio into account From 88dd0abb6c3575bd67dc8ff7a9a5add694eead6c Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 15:59:11 -0800 Subject: [PATCH 25/44] fixed typo --- src/main/java/frc/robot/util/Motors/LoggedTalon.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedTalon.java b/src/main/java/frc/robot/util/Motors/LoggedTalon.java index a816b15..b81d353 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedTalon.java +++ b/src/main/java/frc/robot/util/Motors/LoggedTalon.java @@ -84,7 +84,7 @@ public void setPosition(double position){ /** * Using PIDF to set the motor's velocity - * @param velocity$ velocity reference + * @param velocity velocity reference */ public void setVelocity(double velocity){ targetVelocity = velocity; From 1870d901c72f7f52e0608b0b95deb55834b89e39 Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 17:21:19 -0800 Subject: [PATCH 26/44] Added comments & allowed one spark max to follow another --- .../util/Motors/LoggedSparkMaxConfig.java | 34 ++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java index 461638e..7d0cd21 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java @@ -20,20 +20,52 @@ public LoggedSparkMaxConfig( this.sparkMaxConfig.apply(encoderConfig); } + /** + * Follow another motor + * @param canId the CAN ID of the motor to follow + */ + public void follow(int canId){ + this.sparkMaxConfig.follow(canId); + } + + /** + * Follow another motor + * @param canId the CAN ID of the motor to follow + * @param invert whether to invert the motor + */ + public void follow(int canId, boolean invert){ + this.sparkMaxConfig.follow(canId, invert); + } + + /** + * Get the CAN ID of the motor + * @return the CAN ID of the motor + */ public int getCanId() { return canId; } + /** + * Get the closed loop configuration + * @return the closed loop configuration + */ public ClosedLoopConfig getClosedLoopConfig() { return closedLoopConfig; } + /** + * Get the Spark Max configuration + * @return the Spark Max configuration + */ public SparkMaxConfig getSparkMaxConfig() { return sparkMaxConfig; } + /** + * Get the encoder configuration + * @return the encoder configuration + */ public EncoderConfig getEncoderConfig() { return encoderConfig; } - } From ccd95ced20f4007121c042dd7d3c8d1ae552ec6d Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 17:28:17 -0800 Subject: [PATCH 27/44] Using optional int to represent the can ID to follow --- .../util/Motors/LoggedSparkMaxConfig.java | 25 ++++++------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java index 7d0cd21..fab826b 100644 --- a/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java +++ b/src/main/java/frc/robot/util/Motors/LoggedSparkMaxConfig.java @@ -1,5 +1,7 @@ package frc.robot.util.Motors; +import java.util.OptionalInt; + import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkMaxConfig; @@ -11,30 +13,19 @@ public class LoggedSparkMaxConfig { private final EncoderConfig encoderConfig; public LoggedSparkMaxConfig( - int canId, ClosedLoopConfig closedLoopConfig, EncoderConfig encoderConfig){ + int canId, ClosedLoopConfig closedLoopConfig, + EncoderConfig encoderConfig, OptionalInt followCanId + ){ this.canId = canId; this.closedLoopConfig = closedLoopConfig; this.encoderConfig = encoderConfig; this.sparkMaxConfig = new SparkMaxConfig(); this.sparkMaxConfig.apply(closedLoopConfig); this.sparkMaxConfig.apply(encoderConfig); - } - - /** - * Follow another motor - * @param canId the CAN ID of the motor to follow - */ - public void follow(int canId){ - this.sparkMaxConfig.follow(canId); - } - /** - * Follow another motor - * @param canId the CAN ID of the motor to follow - * @param invert whether to invert the motor - */ - public void follow(int canId, boolean invert){ - this.sparkMaxConfig.follow(canId, invert); + if(followCanId.isPresent()){ + this.sparkMaxConfig.follow(followCanId.getAsInt()); + } } /** From aa0ced71ec20bd4f76b4a35d6231f18236bcc580 Mon Sep 17 00:00:00 2001 From: Jesse Date: Wed, 29 Jan 2025 19:11:19 -0800 Subject: [PATCH 28/44] Redo climb through logged motors. Use trigget to set speed instead of PID --- src/main/java/frc/robot/Constants.java | 71 +++++++---- src/main/java/frc/robot/RobotContainer.java | 5 - .../frc/robot/commands/ClimbRaiseCommand.java | 33 ------ .../frc/robot/subsystems/ClimbSubsystem.java | 111 +++++------------- 4 files changed, 75 insertions(+), 145 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ClimbRaiseCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5648985..00bcef8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,8 +4,15 @@ package frc.robot; +import java.util.OptionalInt; + +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import frc.robot.util.Motors.LoggedSparkMaxConfig; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -19,35 +26,47 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; } - public static class BackAlgaeConstants{ - public static final int ROLLER_MOTOR_ID = 1;//Placeholder - public static final int PIVOT_MOTOR_ID = 2;//Placeholder - public static final double PIVOT_P = 0.01;//placeholder value - public static final double PIVOT_I = 0;//placeholder value - public static final double PIVOT_D = 0;//placeholder value - - public static final int SENSOR_ID = 0;//placeholder value - public static final double PIVOT_POSITION_1 = 0;//placeholder value - public static final double PIVOT_POSITION_2 = 0;//placeholder value - } - public static class ClimbConstants{ - public static final int TOP_MOTOR_ID = 1;//Placeholder - public static final int BOT_MOTOR_ID = 2;//Placeholder - public static final double CLIMB_P = 0.01;//placeholder value - public static final double CLIMB_I = 0;//placeholder value - public static final double CLIMB_D = 0;//placeholder value - public static final double CLIMB_POSITION_1 = 0;//placeholder value - public static final double CLIMB_POSITION_2 = 0;//placeholder value - public static final double CLIMB_GEAR_RATIO = 0.0; //placeholder value - } + public static class BackAlgaeConstants{ + public static final int ROLLER_MOTOR_ID = 1;//Placeholder + public static final int PIVOT_MOTOR_ID = 2;//Placeholder + public static final double PIVOT_P = 0.01;//placeholder value + public static final double PIVOT_I = 0;//placeholder value + public static final double PIVOT_D = 0;//placeholder value + public static final int SENSOR_ID = 0;//placeholder value + public static final double PIVOT_POSITION_1 = 0;//placeholder value + public static final double PIVOT_POSITION_2 = 0;//placeholder value + } + + public static class ClimbConstants{ + public static final int TOP_MOTOR_ID = 9;//Placeholder + public static final int BOT_MOTOR_ID = 10;//Placeholder + public static final LoggedSparkMaxConfig TOP_MOTOR_CONFIG = + new LoggedSparkMaxConfig( + TOP_MOTOR_ID, + new ClosedLoopConfig() + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .outputRange(0, 1), + new EncoderConfig(), + null + ); + public static final LoggedSparkMaxConfig BOT_MOTOR_CONFIG = + new LoggedSparkMaxConfig( + BOT_MOTOR_ID, + new ClosedLoopConfig() + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .outputRange(0, 1), + new EncoderConfig(), + OptionalInt.of(TOP_MOTOR_ID) + ); + } - /** Constants for the swerve subsystem. */ - public static class SwerveConstants { - public static final int FL_DRIVE = 0; - public static final int FL_STEER = 1; - public static final double FL_OFFSET = 0; + /** Constants for the swerve subsystem. */ + public static class SwerveConstants { + public static final int FL_DRIVE = 0; + public static final int FL_STEER = 1; + public static final double FL_OFFSET = 0; public static final int FR_DRIVE = 2; public static final int FR_STEER = 3; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 10039ad..7507e7a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,8 +17,6 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.FieldManagementSubsystem.FieldManagementSubsystem; -import frc.robot.subsystems.PhoenixLoggingSubsystem.PhoenixLoggingSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; /** @@ -33,9 +31,6 @@ public class RobotContainer { private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); - private final FieldManagementSubsystem fieldManagementSubsystem = - new FieldManagementSubsystem(); - // private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = // new PhoenixLoggingSubsystem(fieldManagementSubsystem); diff --git a/src/main/java/frc/robot/commands/ClimbRaiseCommand.java b/src/main/java/frc/robot/commands/ClimbRaiseCommand.java deleted file mode 100644 index ba81235..0000000 --- a/src/main/java/frc/robot/commands/ClimbRaiseCommand.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ClimbSubsystem; - -/** Lowers climb fully when off the chain. Should run at the start of auton to ensure that both arms are at their lowest - * positions so that the robot may fit underneath the stage. */ -public class ClimbRaiseCommand extends Command { - private final ClimbSubsystem climbSubsystem; - private final int speed = 0; //placeholder - private final double endingMotorPosition = 0.0; - - /** Constructs a new {@link ClimbLowerCommand}. */ - public ClimbRaiseCommand(ClimbSubsystem climbSubsystem) { - this.climbSubsystem = climbSubsystem; - this.addRequirements(climbSubsystem); - } - - @Override - public void initialize() { - climbSubsystem.driveRollers(speed); - } - - @Override - public boolean isFinished() { - return climbSubsystem.getMotorPosition() == endingMotorPosition; - } - - @Override - public void end(boolean interrupted) { - super.end(interrupted); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 24ad7bd..34dc58c 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -3,101 +3,50 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SoftLimitConfig; -import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.EncoderConfig; -import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ClimbConstants; - -import edu.wpi.first.math.util.Units; +import static frc.robot.Constants.ClimbConstants.TOP_MOTOR_CONFIG; +import static frc.robot.Constants.ClimbConstants.BOT_MOTOR_CONFIG; +import static frc.robot.Constants.DebugConstants.REV_DEBUG; +import frc.robot.util.Motors.LoggedSparkMax; public class ClimbSubsystem extends SubsystemBase { - private final SparkMax topMotor; - private final SparkMax botMotor; - private final int botMotorID; - - private final ClosedLoopConfig closedLoopConfig; - private final SparkMaxConfig sparkMaxConfig; - private final SparkMaxConfig sparkMaxConfigFollow; - private final EncoderConfig encoderConfig; - private final SoftLimitConfig softLimitConfig; - - private final RelativeEncoder climbEncoder; - //private final SparkClosedLoopController steerPIDController; + private final LoggedSparkMax topMotor; + private final LoggedSparkMax botMotor; + private double targetSpeed; /** Creates a new ExampleSubsystem. */ public ClimbSubsystem() { - - topMotor = new SparkMax(ClimbConstants.TOP_MOTOR_ID, MotorType.kBrushless); - botMotor = new SparkMax(ClimbConstants.BOT_MOTOR_ID, MotorType.kBrushless); - climbEncoder = topMotor.getEncoder(); - - botMotorID = 0; //have to change later - - //steerPIDController = topMotor.getClosedLoopController(); - - encoderConfig = new EncoderConfig(); - encoderConfig.positionConversionFactor(ClimbConstants.CLIMB_GEAR_RATIO); - - softLimitConfig = new SoftLimitConfig(); - softLimitConfig.forwardSoftLimitEnabled(true) - .forwardSoftLimit(Units.degreesToRadians(90)) //find out with mech for soft lim - .reverseSoftLimitEnabled(true) - .reverseSoftLimit(0); - - closedLoopConfig = new ClosedLoopConfig(); - //closedLoopConfig.pid(ClimbConstants.CLIMB_P, ClimbConstants.CLIMB_I, ClimbConstants.CLIMB_D); - - sparkMaxConfig = new SparkMaxConfig(); - sparkMaxConfig.apply(encoderConfig) - .apply(softLimitConfig) - .apply(closedLoopConfig); - - sparkMaxConfigFollow = new SparkMaxConfig(); //to follow the motor - sparkMaxConfigFollow.follow(botMotorID) - .apply(encoderConfig) - .apply(softLimitConfig) - .apply(closedLoopConfig); - - topMotor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - botMotor.configure(sparkMaxConfigFollow, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } - - public void setPosition(double targetRads) { - double targetDouble = (targetRads + Math.PI) / (2. * Math.PI); - System.out.println("target" + targetDouble); - System.out.println("current" + climbEncoder.getPosition()); + topMotor = new LoggedSparkMax(TOP_MOTOR_CONFIG); + botMotor = new LoggedSparkMax(BOT_MOTOR_CONFIG); } - public void driveRollers(double speed){ - topMotor.set(speed); + @Override + public void periodic() { + topMotor.set(targetSpeed); + topMotor.logStats(); + botMotor.logStats(); + if(REV_DEBUG){ + topMotor.publishStats(); + botMotor.publishStats(); + } } - public double getMotorPosition() { - // Returns the position in rotations - return climbEncoder.getPosition(); + /** + * Gets the position of the top motor + * @return position of the top motor in rotations after taking the position + * conversion factor into account + */ + public double getPosition() { + return topMotor.getPosition(); } /** - * An example method querying a boolean state of the subsystem (for example, a - * digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. + * Sets the speed of the motors + * @param speed target speed from -1.0 to 1.0 */ - - //sets the actuall speed of the rollers - - @Override - public void periodic() { - // This method will be called once per scheduler run + public void setSpeed(double speed){ + targetSpeed = speed; } - -} +} \ No newline at end of file From dd9781912ab68fb952f991116e82cd310e3efb8e Mon Sep 17 00:00:00 2001 From: Jesse Date: Sun, 2 Feb 2025 13:08:15 -0800 Subject: [PATCH 29/44] Init targetSpeed to 0 --- src/main/java/frc/robot/subsystems/ClimbSubsystem.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 34dc58c..07a8dbb 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -14,7 +14,7 @@ public class ClimbSubsystem extends SubsystemBase { private final LoggedSparkMax topMotor; private final LoggedSparkMax botMotor; - private double targetSpeed; + private double targetSpeed = 0; /** Creates a new ExampleSubsystem. */ public ClimbSubsystem() { @@ -25,11 +25,11 @@ public ClimbSubsystem() { @Override public void periodic() { topMotor.set(targetSpeed); - topMotor.logStats(); - botMotor.logStats(); + // topMotor.logStats(); + // botMotor.logStats(); if(REV_DEBUG){ - topMotor.publishStats(); - botMotor.publishStats(); + // topMotor.publishStats(); + // botMotor.publishStats(); } } From 0ca186a3a4d5bc7adfd55727ab692c0a9e7eea84 Mon Sep 17 00:00:00 2001 From: Jesse Date: Sun, 2 Feb 2025 13:56:26 -0800 Subject: [PATCH 30/44] use OptionalInt.empty() instead of null --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 00bcef8..37a8f73 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,7 +49,7 @@ public static class ClimbConstants{ .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .outputRange(0, 1), new EncoderConfig(), - null + OptionalInt.empty() ); public static final LoggedSparkMaxConfig BOT_MOTOR_CONFIG = new LoggedSparkMaxConfig( From 9992e9b8ea9302114ea22acc7cf042f119eb1268 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Wed, 5 Feb 2025 11:30:36 -0800 Subject: [PATCH 31/44] add binding for climb --- src/main/java/frc/robot/RobotContainer.java | 12 ++++++++++++ .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 7 +++++++ 2 files changed, 19 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7507e7a..ef78708 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import frc.robot.Constants.ClimbConstants; import frc.robot.controllers.BaseDriveController; import frc.robot.controllers.DualJoystickDriveController; import frc.robot.controllers.PS5DriveController; @@ -14,9 +15,12 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; 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.CommandPS5Controller; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.ClimbSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; /** @@ -28,8 +32,10 @@ public class RobotContainer { private BaseDriveController driveController; + private CommandPS5Controller mechcontroller = new CommandPS5Controller(1); private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); + private final ClimbSubsystem climbSubsystem = new ClimbSubsystem(); // private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = // new PhoenixLoggingSubsystem(fieldManagementSubsystem); @@ -84,6 +90,12 @@ private void configureBindings() { }, swerveSubsystem ); + + climbSubsystem.setDefaultCommand( + new InstantCommand(() -> { + climbSubsystem.setSpeed(mechcontroller.getL2Axis()); + }, climbSubsystem) + ); } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 54371d6..33bf3d5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; @@ -56,6 +57,7 @@ public class SwerveSubsystem extends SubsystemBase { "estimatedPose", Pose2d.struct ); + private DoublePublisher gyroHeadingPublisher; public SwerveSubsystem() { ahrs = new AHRS(NavXComType.kMXP_SPI); @@ -234,6 +236,10 @@ private void initNT(){ "estimatedPose", Pose2d.struct ).publish(); + + gyroHeadingPublisher = swerveTable + .getDoubleTopic("GyroHeading") + .publish(); } /** @@ -241,6 +247,7 @@ private void initNT(){ */ private void publishStats(){ estimatedPosePublisher.set(estimatedPose); + gyroHeadingPublisher.set(ahrs.getAngle()); if(STATE_DEBUG || DRIVE_DEBUG || STEER_DEBUG){ swerveStatesPublisher.set(getModuleStates()); From 25412745ad28c32a9fba71b177d2833b449ab6de Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Wed, 5 Feb 2025 11:40:07 -0800 Subject: [PATCH 32/44] rumble in robot container --- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ef78708..48c3f5f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PS5Controller; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -94,10 +95,26 @@ private void configureBindings() { climbSubsystem.setDefaultCommand( new InstantCommand(() -> { climbSubsystem.setSpeed(mechcontroller.getL2Axis()); + + setRum(mechcontroller.getL2Axis()); + }, climbSubsystem) ); } + + + /* + * Sets the rumble of the controller + * @param value the value of the rumble + */ + public void setRum(double value) { + mechcontroller.getHID().setRumble(PS5Controller.RumbleType.kLeftRumble, value); + mechcontroller.getHID().setRumble(PS5Controller.RumbleType.kRightRumble, value); + } + + + /** * Use this to pass the autonomous command to the main {@link Robot} class. * From 0877d12c3150d666bdf8508b0436c37b6afbd020 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Wed, 5 Feb 2025 12:23:33 -0800 Subject: [PATCH 33/44] using kraken-climb using option and create button --- src/main/java/frc/robot/RobotContainer.java | 32 ++++++++++++++++----- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 48c3f5f..e5fdeb1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,17 +33,26 @@ public class RobotContainer { private BaseDriveController driveController; - private CommandPS5Controller mechcontroller = new CommandPS5Controller(1); + private CommandPS5Controller mechController = new CommandPS5Controller(1); private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); private final ClimbSubsystem climbSubsystem = new ClimbSubsystem(); + private final Trigger createTrigger; + private final Trigger optionTrigger; + private final Trigger xbutton; + // private final PhoenixLoggingSubsystem phoenixLoggingSubsystem = // new PhoenixLoggingSubsystem(fieldManagementSubsystem); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { constructDriveController(); + + createTrigger = new Trigger(mechController.create()); + optionTrigger = new Trigger(mechController.options()); + xbutton = new Trigger(mechController.cross()); + startLog(); configureBindings(); } @@ -92,12 +101,21 @@ private void configureBindings() { swerveSubsystem ); - climbSubsystem.setDefaultCommand( - new InstantCommand(() -> { - climbSubsystem.setSpeed(mechcontroller.getL2Axis()); + // climbSubsystem.setDefaultCommand( + // new InstantCommand(() -> { + // climbSubsystem.setSpeed(mechcontroller.getL2Axis()); + // setRum(mechcontroller.getL2Axis()); - setRum(mechcontroller.getL2Axis()); + // }, climbSubsystem) + // ); + createTrigger.and(optionTrigger).whileTrue( + new RunCommand(() -> { + climbSubsystem.setSpeed(-0.2); + }, climbSubsystem) + ).onFalse( + new RunCommand(() -> { + climbSubsystem.setSpeed(0); }, climbSubsystem) ); } @@ -109,8 +127,8 @@ private void configureBindings() { * @param value the value of the rumble */ public void setRum(double value) { - mechcontroller.getHID().setRumble(PS5Controller.RumbleType.kLeftRumble, value); - mechcontroller.getHID().setRumble(PS5Controller.RumbleType.kRightRumble, value); + mechController.getHID().setRumble(PS5Controller.RumbleType.kLeftRumble, value); + mechController.getHID().setRumble(PS5Controller.RumbleType.kRightRumble, value); } From eb5866c2bceeccf07b99be2f406755384ae634f8 Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Wed, 5 Feb 2025 14:42:54 -0800 Subject: [PATCH 34/44] fix rumble --- src/main/java/frc/robot/RobotContainer.java | 2 ++ src/main/java/frc/robot/subsystems/ClimbSubsystem.java | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e5fdeb1..1dfb579 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -112,10 +112,12 @@ private void configureBindings() { createTrigger.and(optionTrigger).whileTrue( new RunCommand(() -> { climbSubsystem.setSpeed(-0.2); + setRum(climbSubsystem.getVelocity()); }, climbSubsystem) ).onFalse( new RunCommand(() -> { climbSubsystem.setSpeed(0); + setRum(climbSubsystem.getVelocity()); }, climbSubsystem) ); } diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 07a8dbb..acc932b 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -42,6 +42,15 @@ public double getPosition() { return topMotor.getPosition(); } + /** + * Gets the velocity of the top motor + * @return velocity of the top motor in RPM after taking the velocity + * conversion factor into account + */ + public double getVelocity() { + return topMotor.getVelocity(); + } + /** * Sets the speed of the motors * @param speed target speed from -1.0 to 1.0 From 44d3644ef996f15c3102cf1a67b5e9e0f4911590 Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Wed, 5 Feb 2025 15:34:51 -0800 Subject: [PATCH 35/44] increase speed --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1dfb579..d162cd5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -111,7 +111,7 @@ private void configureBindings() { createTrigger.and(optionTrigger).whileTrue( new RunCommand(() -> { - climbSubsystem.setSpeed(-0.2); + climbSubsystem.setSpeed(-0.5); setRum(climbSubsystem.getVelocity()); }, climbSubsystem) ).onFalse( From 15217082021f9647b24e31fa053c191b9f543608 Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Wed, 5 Feb 2025 16:43:06 -0800 Subject: [PATCH 36/44] qucik rumble fix --- src/main/java/frc/robot/RobotContainer.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d162cd5..9341cb8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.PS5Controller; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -112,12 +113,12 @@ private void configureBindings() { createTrigger.and(optionTrigger).whileTrue( new RunCommand(() -> { climbSubsystem.setSpeed(-0.5); - setRum(climbSubsystem.getVelocity()); + setRum(0.5); }, climbSubsystem) ).onFalse( new RunCommand(() -> { climbSubsystem.setSpeed(0); - setRum(climbSubsystem.getVelocity()); + setRum(0); }, climbSubsystem) ); } @@ -129,8 +130,8 @@ private void configureBindings() { * @param value the value of the rumble */ public void setRum(double value) { - mechController.getHID().setRumble(PS5Controller.RumbleType.kLeftRumble, value); - mechController.getHID().setRumble(PS5Controller.RumbleType.kRightRumble, value); + mechController.setRumble(GenericHID.RumbleType.kLeftRumble, value); // Strong vibration + mechController.setRumble(GenericHID.RumbleType.kRightRumble, value); } From c2c7328012c1052678fc029747119e2196cc7abb Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Wed, 5 Feb 2025 17:16:55 -0800 Subject: [PATCH 37/44] small changes --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9341cb8..07b2754 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,6 +35,7 @@ public class RobotContainer { private BaseDriveController driveController; private CommandPS5Controller mechController = new CommandPS5Controller(1); + private final GenericHID driverController = new GenericHID(0); private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); private final ClimbSubsystem climbSubsystem = new ClimbSubsystem(); @@ -134,6 +135,15 @@ public void setRum(double value) { mechController.setRumble(GenericHID.RumbleType.kRightRumble, value); } + public void setRumble(GenericHID.RumbleType type, double value) { + System.out.println("Setting rumble: " + type + " to value: " + value); + driverController.setRumble(type, value); + } + + public void testRumble() { + setRumble(GenericHID.RumbleType.kLeftRumble, 1.0); + setRumble(GenericHID.RumbleType.kRightRumble, 1.0); + } /** From 98a38b6683ff3d7fb343a9b24199af8b69df3600 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Wed, 5 Feb 2025 17:38:50 -0800 Subject: [PATCH 38/44] flip navx --- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 33bf3d5..4588bc8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -15,6 +15,7 @@ import edu.wpi.first.util.datalog.StructLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.GRTUtil; @@ -61,6 +62,7 @@ public class SwerveSubsystem extends SubsystemBase { public SwerveSubsystem() { ahrs = new AHRS(NavXComType.kMXP_SPI); + ahrs.configureVelocity(false, false, false, true); frontLeftModule = new SwerveModule(FL_DRIVE, FL_STEER, FL_OFFSET); frontRightModule = new SwerveModule(FR_DRIVE, FR_STEER, FR_OFFSET); @@ -181,7 +183,7 @@ public void resetDriverHeading() { /** Gets the gyro heading.*/ private Rotation2d getGyroHeading() { - return Rotation2d.fromDegrees(ahrs.getAngle()); // Might need to flip depending on the robot setup + return Rotation2d.fromDegrees(-ahrs.getAngle()); // Works for Navx 2, flip for navx1 } /** From befc29d3d93e0c677fd5b3952ad978f7885432e9 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Thu, 6 Feb 2025 16:31:06 -0800 Subject: [PATCH 39/44] climb to not use logged sparkmax for more configs tmp --- .vscode/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 3 +- .../frc/robot/subsystems/ClimbSubsystem.java | 56 +++++++++++++++++-- 3 files changed, 53 insertions(+), 8 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index e9f1f81..64b765c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,5 @@ "edu.wpi.first.math.**.struct.*", ], "wpilib.autoStartRioLog": true, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07b2754..dc7522c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -113,7 +113,7 @@ private void configureBindings() { createTrigger.and(optionTrigger).whileTrue( new RunCommand(() -> { - climbSubsystem.setSpeed(-0.5); + climbSubsystem.setSpeed(0.3); setRum(0.5); }, climbSubsystem) ).onFalse( @@ -122,6 +122,7 @@ private void configureBindings() { setRum(0); }, climbSubsystem) ); + } diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index acc932b..eaf5f1e 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -4,22 +4,66 @@ package frc.robot.subsystems; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static frc.robot.Constants.ClimbConstants.TOP_MOTOR_CONFIG; +import static frc.robot.Constants.ClimbConstants.TOP_MOTOR_ID; import static frc.robot.Constants.ClimbConstants.BOT_MOTOR_CONFIG; +import static frc.robot.Constants.ClimbConstants.BOT_MOTOR_ID; import static frc.robot.Constants.DebugConstants.REV_DEBUG; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; + import frc.robot.util.Motors.LoggedSparkMax; public class ClimbSubsystem extends SubsystemBase { - private final LoggedSparkMax topMotor; - private final LoggedSparkMax botMotor; + private final SparkMax topMotor; + private final SparkMax botMotor; private double targetSpeed = 0; + private ClosedLoopConfig closedLoopConfig; + private EncoderConfig encoderConfig; + + private RelativeEncoder encoder; + private SparkMaxConfig topSparkMaxConfig, botSparkMaxConfig; + /** Creates a new ExampleSubsystem. */ public ClimbSubsystem() { - topMotor = new LoggedSparkMax(TOP_MOTOR_CONFIG); - botMotor = new LoggedSparkMax(BOT_MOTOR_CONFIG); + topMotor = new SparkMax(TOP_MOTOR_ID, MotorType.kBrushless); + botMotor = new SparkMax(BOT_MOTOR_ID, MotorType.kBrushless); + + closedLoopConfig = new ClosedLoopConfig() + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .outputRange(0, 1); + encoderConfig = new EncoderConfig(); + encoder = topMotor.getEncoder(); + + topSparkMaxConfig = new SparkMaxConfig(); + topSparkMaxConfig.apply(closedLoopConfig) + .inverted(true); + + botSparkMaxConfig = new SparkMaxConfig(); + botSparkMaxConfig.apply(closedLoopConfig) + .inverted(true) + .follow(TOP_MOTOR_ID); + + topMotor.configure(topSparkMaxConfig, + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ); + botMotor.configure(botSparkMaxConfig, + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ); + } @Override @@ -39,7 +83,7 @@ public void periodic() { * conversion factor into account */ public double getPosition() { - return topMotor.getPosition(); + return encoder.getPosition(); } /** @@ -48,7 +92,7 @@ public double getPosition() { * conversion factor into account */ public double getVelocity() { - return topMotor.getVelocity(); + return encoder.getVelocity(); } /** From faa1a9843fa38295de29669eb376d54b1ffe02a9 Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Fri, 7 Feb 2025 11:25:02 -0800 Subject: [PATCH 40/44] added reverse --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc7522c..276758a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,17 +123,25 @@ private void configureBindings() { }, climbSubsystem) ); + xbutton.whileTrue( + new RunCommand(() -> { + climbSubsystem.setSpeed(-0.3); + setRum(0.5); + }, climbSubsystem) + ).onFalse( + new RunCommand(() -> { + climbSubsystem.setSpeed(0); + setRum(0); + }, climbSubsystem) + ); } - - /* * Sets the rumble of the controller * @param value the value of the rumble */ public void setRum(double value) { - mechController.setRumble(GenericHID.RumbleType.kLeftRumble, value); // Strong vibration - mechController.setRumble(GenericHID.RumbleType.kRightRumble, value); + mechController.getHID().setOutputs((int) value); } public void setRumble(GenericHID.RumbleType type, double value) { From 9b7df20ed287ce7b42a1a31331db54ede5a2d397 Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Fri, 7 Feb 2025 14:21:57 -0800 Subject: [PATCH 41/44] quick changes for rumble :D --- src/main/java/frc/robot/RobotContainer.java | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 276758a..1b90e41 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -141,17 +141,10 @@ private void configureBindings() { * @param value the value of the rumble */ public void setRum(double value) { - mechController.getHID().setOutputs((int) value); - } - - public void setRumble(GenericHID.RumbleType type, double value) { - System.out.println("Setting rumble: " + type + " to value: " + value); - driverController.setRumble(type, value); - } - public void testRumble() { - setRumble(GenericHID.RumbleType.kLeftRumble, 1.0); - setRumble(GenericHID.RumbleType.kRightRumble, 1.0); + mechController.getHID().setRumble(GenericHID.RumbleType.kLeftRumble, value); + mechController.getHID().setRumble(GenericHID.RumbleType.kRightRumble, value); + } From 04b79193999eb3f2b28120723efa4aef10b9b0da Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Sat, 8 Feb 2025 09:03:55 -0800 Subject: [PATCH 42/44] softlimit added, it is 53, but need to double check again --- src/main/java/frc/robot/RobotContainer.java | 26 +++++++------------ .../frc/robot/subsystems/ClimbSubsystem.java | 21 ++++++++++++++- 2 files changed, 30 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b90e41..7c5fcc6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,7 +35,6 @@ public class RobotContainer { private BaseDriveController driveController; private CommandPS5Controller mechController = new CommandPS5Controller(1); - private final GenericHID driverController = new GenericHID(0); private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); private final ClimbSubsystem climbSubsystem = new ClimbSubsystem(); @@ -55,6 +54,8 @@ public RobotContainer() { optionTrigger = new Trigger(mechController.options()); xbutton = new Trigger(mechController.cross()); + climbSubsystem.resetEncoderPos(); + startLog(); configureBindings(); } @@ -113,39 +114,32 @@ private void configureBindings() { createTrigger.and(optionTrigger).whileTrue( new RunCommand(() -> { - climbSubsystem.setSpeed(0.3); - setRum(0.5); + climbSubsystem.setSpeed(0.2); + System.out.println(climbSubsystem.getPosition()); + }, climbSubsystem) ).onFalse( new RunCommand(() -> { climbSubsystem.setSpeed(0); - setRum(0); + }, climbSubsystem) ); xbutton.whileTrue( new RunCommand(() -> { - climbSubsystem.setSpeed(-0.3); - setRum(0.5); + climbSubsystem.setSpeed(-0.2); + System.out.println(climbSubsystem.getPosition()); + }, climbSubsystem) ).onFalse( new RunCommand(() -> { climbSubsystem.setSpeed(0); - setRum(0); + }, climbSubsystem) ); } - /* - * Sets the rumble of the controller - * @param value the value of the rumble - */ - public void setRum(double value) { - mechController.getHID().setRumble(GenericHID.RumbleType.kLeftRumble, value); - mechController.getHID().setRumble(GenericHID.RumbleType.kRightRumble, value); - - } /** diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index eaf5f1e..04c9438 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -18,6 +18,7 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -33,6 +34,7 @@ public class ClimbSubsystem extends SubsystemBase { private ClosedLoopConfig closedLoopConfig; private EncoderConfig encoderConfig; + private SoftLimitConfig softLimitConfig; private RelativeEncoder encoder; private SparkMaxConfig topSparkMaxConfig, botSparkMaxConfig; @@ -41,6 +43,7 @@ public class ClimbSubsystem extends SubsystemBase { public ClimbSubsystem() { topMotor = new SparkMax(TOP_MOTOR_ID, MotorType.kBrushless); botMotor = new SparkMax(BOT_MOTOR_ID, MotorType.kBrushless); + closedLoopConfig = new ClosedLoopConfig() .feedbackSensor(FeedbackSensor.kPrimaryEncoder) @@ -48,13 +51,22 @@ public ClimbSubsystem() { encoderConfig = new EncoderConfig(); encoder = topMotor.getEncoder(); + softLimitConfig = new SoftLimitConfig(); + softLimitConfig.forwardSoftLimitEnabled(true) + .forwardSoftLimit(53) + .reverseSoftLimitEnabled(true) + .reverseSoftLimit(0); + + topSparkMaxConfig = new SparkMaxConfig(); topSparkMaxConfig.apply(closedLoopConfig) + .apply(softLimitConfig) .inverted(true); botSparkMaxConfig = new SparkMaxConfig(); botSparkMaxConfig.apply(closedLoopConfig) .inverted(true) + .apply(softLimitConfig) .follow(TOP_MOTOR_ID); topMotor.configure(topSparkMaxConfig, @@ -77,6 +89,14 @@ public void periodic() { } } + /** + * Set encoder position to 0 + */ + public void resetEncoderPos(){ + encoder.setPosition(0); + } + + /** * Gets the position of the top motor * @return position of the top motor in rotations after taking the position @@ -85,7 +105,6 @@ public void periodic() { public double getPosition() { return encoder.getPosition(); } - /** * Gets the velocity of the top motor * @return velocity of the top motor in RPM after taking the velocity From 5e735da5aec4b71cbec0900402a8a48ddf6d11a0 Mon Sep 17 00:00:00 2001 From: swaswa999 Date: Sun, 9 Feb 2025 17:42:19 -0800 Subject: [PATCH 43/44] fixed soft limit, forks great now --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/subsystems/ClimbSubsystem.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7c5fcc6..1b6f4e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -114,7 +114,7 @@ private void configureBindings() { createTrigger.and(optionTrigger).whileTrue( new RunCommand(() -> { - climbSubsystem.setSpeed(0.2); + climbSubsystem.setSpeed(0.3); System.out.println(climbSubsystem.getPosition()); }, climbSubsystem) @@ -127,7 +127,7 @@ private void configureBindings() { xbutton.whileTrue( new RunCommand(() -> { - climbSubsystem.setSpeed(-0.2); + climbSubsystem.setSpeed(-0.3); System.out.println(climbSubsystem.getPosition()); }, climbSubsystem) diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 04c9438..8927bc6 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -53,7 +53,7 @@ public ClimbSubsystem() { softLimitConfig = new SoftLimitConfig(); softLimitConfig.forwardSoftLimitEnabled(true) - .forwardSoftLimit(53) + .forwardSoftLimit(67) .reverseSoftLimitEnabled(true) .reverseSoftLimit(0); From 29ad2d47d94e8f20be204465350e34c555f36ea5 Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Wed, 19 Feb 2025 12:12:02 -0800 Subject: [PATCH 44/44] formatted climb subsystem file --- .vscode/settings.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 64b765c..6aae32f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "wpilib.autoStartRioLog": true, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable", + "java.checkstyle.configuration": "/google_checks.xml" }