From 3965f7fe462884f74dd6b0699b85822d40e7d50f Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Tue, 7 Jan 2025 18:05:45 -0500 Subject: [PATCH 001/110] elevator io done and elevator other stuff skelton --- .../robot/subsystems/elevator/Elevator.java | 49 +++++++++++++++++++ .../robot/subsystems/elevator/ElevatorIO.java | 30 ++++++++++++ .../subsystems/elevator/ElevatorIOInputs.java | 5 ++ .../subsystems/elevator/ElevatorIOReal.java | 14 ++++++ .../subsystems/elevator/ElevatorIOSim.java | 10 ++++ 5 files changed, 108 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/elevator/Elevator.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputs.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..d5b32da --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems.elevator; + +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.motorcontrol.PWMTalonSRX; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class Elevator extends SubsystemBase { + + + private ElevatorIO io; + private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + + +private TalonFX motor = new TalonFX(5); //change motor later +public void up(){ + motor.set(1); +} +public void down(){ + motor.set(-1); +} +public void stop(){ + motor.set(0); +} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..15802b1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + @AutoLog + + public static class ElevatorIOInputs { + public double ElevatorInputVolts = 0.0; + public double ElevatorAppliedVolts = 0.0; + public double ElevatorPositionRads = 0.0; + public double ElevatorVelocityRads = 0.0; + public double ElevatorCurrent = 0.0; + + public double ElevatorMotorVelocityRadPerSec = 0.0; + public double ElevatorMotorPositionRads = 0.0; + } + + public default void updateInputs(ElevatorIOInputs inputs) {} + + public default void moveElevator(double speed) {} + + public default void setVoltage(double volts) {} + + // True is brake, false is coast + public default void setIdleMode(boolean mode) {} + + public default void resetEncoders() {} + } + diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputs.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputs.java new file mode 100644 index 0000000..008b940 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputs.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.elevator; + +public interface ElevatorIOInputs { + +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java new file mode 100644 index 0000000..f5c483c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.elevator; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; + +public class ElevatorIOReal implements ElevatorIOInputs { +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java new file mode 100644 index 0000000..3fb9c45 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; + +public class ElevatorIOSim implements ElevatorIO { +} \ No newline at end of file From 0c8ebaaa37d5ea82a6266a005fe61c643d449a16 Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Tue, 7 Jan 2025 18:11:24 -0500 Subject: [PATCH 002/110] intake started (Kathy + Kayla) mostly last years code since we don't know what intake looks like this year. --- src/main/java/frc/robot/Constants.java | 26 ++++++++ src/main/java/frc/robot/RobotContainer.java | 16 +++++ .../frc/robot/subsystems/intake/Intake.java | 58 +++++++++++++++++ .../subsystems/intake/IntakeConstants.java | 11 ++++ .../frc/robot/subsystems/intake/IntakeIO.java | 18 ++++++ .../robot/subsystems/intake/IntakeIOReal.java | 63 +++++++++++++++++++ .../robot/subsystems/intake/IntakeIOSim.java | 25 ++++++++ 7 files changed, 217 insertions(+) create mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..1bfebdf --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,26 @@ +package frc.robot; + +public final class Constants { + public static final class IntakeSetpoints{ + //placeholders + public static final double intake = -0.85; + public static final double reverse = 0.7; + } + + public static final Mode currentMode = Mode.fromState(); + public static enum Mode { + REAL, + + SIM, + + REPLAY; + + static Mode fromState() { + if (Robot.isReal()) { + return REAL; + } else { + return SIM; + } + } + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..196ed21 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,10 +6,26 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeIOReal; +import frc.robot.subsystems.intake.IntakeIOSim; public class RobotContainer { + private final Intake intake; + public RobotContainer() { configureBindings(); + switch (Constants.currentMode) { + case REAL: + intake = Intake.initialize(new IntakeIOReal()); + break; + case SIM: + intake = Intake.initialize(new IntakeIOSim()); + break; + default: + intake = Intake.initialize(new IntakeIOSim()); + + } } private void configureBindings() {} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..b783b47 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeSetpoints; +import frc.robot.subsystems.intake.IntakeIO.IntakeIOInputs; +import org.littletonrobotics.junction.Logger; + +public class Intake extends SubsystemBase { + private IntakeIO io; + private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + private double intakeSpeed = 0.0; + + private static Intake instance; + + public static Intake getInstance(){ + return instance; + } + + public static Intake initialize(IntakeIO io){ + if(instance == null){ + instance = new Intake(io); + } + return instance; + } + + private Intake(IntakeIO io) { + this.io = io; + io.updateInputs(inputs); + } + + public void setIntakeSpeed(double speed){ + intakeSpeed = speed; + } + + // public boolean isIntaking() { + // return inputs.currentAmps > IntakeConstants.intakeCurrent && inputs.angularVelocityRPM > 2400 && !Indexer.getInstance().isStoring(); + // } + + public Command intake() { + return startEnd(() -> setIntakeSpeed(IntakeSetpoints.intake), () -> setIntakeSpeed(0)); + } + + public Command reverse() { + return startEnd(() -> setIntakeSpeed(IntakeSetpoints.reverse), () -> setIntakeSpeed(0)); + } + + @Override + public void periodic(){ + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + io.setMotorSpeed(intakeSpeed); + Logger.recordOutput("Intake/Intake speed", intakeSpeed); + } +} + + + diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..e4b8d64 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.intake; + +public final class IntakeConstants { + //PLACEHOLDERS + public static final int canID = 0; + + public static final double simGearRatio = 0; + public static final double overrideSpeed = -0.0; + + public static final double intakeCurrent = 0; +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..8307716 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public double voltage = 0.0; + public double angularPositionRot = 0.0; + public double angularVelocityRPM = 0.0; + public double currentAmps = 0.0; + } + public default void updateInputs(IntakeIOInputs inputs) { + } + + public default void setMotorSpeed(double speed) { + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java new file mode 100644 index 0000000..0a3277d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -0,0 +1,63 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + + +public class IntakeIOReal implements IntakeIO { + private final TalonFX intakeMotor = new TalonFX(IntakeConstants.canID); + + private TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); + + + // private final StatusSignal current = intakeMotor.getStatorCurrent(); + // private final StatusSignal voltage = intakeMotor.getMotorVoltage(); + // private final StatusSignal velocity = intakeMotor.getVelocity(); + + public IntakeIOReal() { + // intakeMotor.setInverted(true); + + talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; + // talonFXConfig.CurrentLimits.StatorCurrentLimit = CurrentLimits.intakeKraken; + + // talonFXConfig. + + talonFXConfig.Audio.BeepOnBoot = true; + + talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + + intakeMotor.clearStickyFaults(); + + // BaseStatusSignal.setUpdateFrequencyForAll(50, velocity, voltage, current); + + // intakeMotor.optimizeBusUtilization(1.0); + + StatusCode response = intakeMotor.getConfigurator().apply(talonFXConfig); + if (!response.isOK()) { + System.out.println( + "Talon ID " + + intakeMotor.getDeviceID() + + " failed config with error " + + response.toString()); + } + } + + // public void updateInputs(IntakeIOInputs inputs) { + // BaseStatusSignal.refreshAll(velocity, voltage, current); + + // inputs.angularVelocityRPM = velocity.getValueAsDouble() * 60; + // // inputs.angularPositionRot = intakeMotor.getPosition().getValueAsDouble(); + // inputs.currentAmps = current.getValueAsDouble(); + // inputs.voltage = voltage.getValueAsDouble(); + + @Override + public void setMotorSpeed(double speed) { + intakeMotor.set(-speed); + } + +} + diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..f7b3e08 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.intake; + + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class IntakeIOSim implements IntakeIO { + // not done + private final DCMotorSim motor = new DCMotorSim(null, null, null); + + @Override + public void updateInputs(IntakeIOInputs inputs) { + motor.update(0.02); + + inputs.angularPositionRot = motor.getAngularPositionRotations(); + inputs.angularVelocityRPM = motor.getAngularVelocityRPM(); + inputs.currentAmps = motor.getCurrentDrawAmps(); + } + + @Override + public void setMotorSpeed(double speed) { + motor.setInputVoltage(MathUtil.clamp(12 * speed, -12, 12)); + } +} \ No newline at end of file From dc2083f958ab0b9c0d5073c7fea74d397fc74626 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Wed, 8 Jan 2025 15:59:07 -0500 Subject: [PATCH 003/110] Drive started --- src/main/java/frc/robot/Drive/Drive.java | 0 src/main/java/frc/robot/Drive/DriveConstants.java | 0 src/main/java/frc/robot/Drive/GyroIO.java | 0 src/main/java/frc/robot/Drive/GyroIOPigeon.java | 0 src/main/java/frc/robot/Drive/Module.java | 0 src/main/java/frc/robot/Drive/ModuleIO.java | 0 src/main/java/frc/robot/Drive/ModuleIOSim.java | 0 src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java | 0 src/main/java/frc/robot/Drive/ModuleIOTalonFX.java | 0 9 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/main/java/frc/robot/Drive/Drive.java create mode 100644 src/main/java/frc/robot/Drive/DriveConstants.java create mode 100644 src/main/java/frc/robot/Drive/GyroIO.java create mode 100644 src/main/java/frc/robot/Drive/GyroIOPigeon.java create mode 100644 src/main/java/frc/robot/Drive/Module.java create mode 100644 src/main/java/frc/robot/Drive/ModuleIO.java create mode 100644 src/main/java/frc/robot/Drive/ModuleIOSim.java create mode 100644 src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java create mode 100644 src/main/java/frc/robot/Drive/ModuleIOTalonFX.java diff --git a/src/main/java/frc/robot/Drive/Drive.java b/src/main/java/frc/robot/Drive/Drive.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/Drive/DriveConstants.java b/src/main/java/frc/robot/Drive/DriveConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/Drive/GyroIO.java b/src/main/java/frc/robot/Drive/GyroIO.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/Drive/GyroIOPigeon.java b/src/main/java/frc/robot/Drive/GyroIOPigeon.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/Drive/Module.java b/src/main/java/frc/robot/Drive/Module.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/Drive/ModuleIO.java b/src/main/java/frc/robot/Drive/ModuleIO.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/Drive/ModuleIOSim.java b/src/main/java/frc/robot/Drive/ModuleIOSim.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java b/src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java new file mode 100644 index 0000000..e69de29 From d5b224331811bd721b9d7da750879b157ad8efe8 Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Thu, 9 Jan 2025 18:21:22 -0500 Subject: [PATCH 004/110] more elevator stuff --- .../robot/subsystems/elevator/Elevator.java | 29 ++++++++++--------- .../robot/subsystems/elevator/ElevatorIO.java | 1 - .../subsystems/elevator/ElevatorIOReal.java | 5 +++- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index d5b32da..aa8dc9f 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.geometry.Rotation2d; @@ -19,31 +20,31 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.wpilibj.AnalogPotentiometer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.PWMTalonSRX; +import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PIDSubsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.math.controller.PIDController; -public class Elevator extends SubsystemBase { +public class Elevator extends SubsystemBase { + public ElevatorFeedforward feedforward = new ElevatorFeedforward(1, 2, 3); + private ElevatorIO io; + private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); - private ElevatorIO io; - private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + public void tankDriveWithFeedforward(double leftVelocity, double rightVelocity) { + leftMotor.setVoltage(feedforward.calculate(leftVelocity)); + rightMotor.setVoltage(feedforward.calculate(rightVelocity)); + } - -private TalonFX motor = new TalonFX(5); //change motor later -public void up(){ - motor.set(1); -} -public void down(){ - motor.set(-1); -} -public void stop(){ - motor.set(0); } -} \ No newline at end of file + + + diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 15802b1..726cb29 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -4,7 +4,6 @@ public interface ElevatorIO { @AutoLog - public static class ElevatorIOInputs { public double ElevatorInputVolts = 0.0; public double ElevatorAppliedVolts = 0.0; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index f5c483c..d5a096a 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -10,5 +10,8 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; -public class ElevatorIOReal implements ElevatorIOInputs { +public class ElevatorIOReal implements ElevatorIO { + + private TalonFX topElevatorMotor = new TalonFX(0); + private TalonFX bottomElevatorMotor = new TalonFX(1); } \ No newline at end of file From 3325be12163d37c943072201c2b5a764a07b63c3 Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Mon, 13 Jan 2025 15:42:44 -0500 Subject: [PATCH 005/110] some intake code changes (trying to get sim to work) --- simgui-ds.json | 92 +++++++++++++++++++ .../frc/robot/subsystems/intake/Intake.java | 9 +- .../subsystems/intake/IntakeConstants.java | 1 + .../frc/robot/subsystems/intake/IntakeIO.java | 1 - .../robot/subsystems/intake/IntakeIOReal.java | 31 ++++--- .../robot/subsystems/intake/IntakeIOSim.java | 4 +- 6 files changed, 117 insertions(+), 21 deletions(-) create mode 100644 simgui-ds.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index b783b47..0ef08e3 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -5,10 +5,11 @@ import frc.robot.Constants.IntakeSetpoints; import frc.robot.subsystems.intake.IntakeIO.IntakeIOInputs; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; public class Intake extends SubsystemBase { private IntakeIO io; - private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + private IntakeIOInputs inputs = new IntakeIOInputs(); private double intakeSpeed = 0.0; private static Intake instance; @@ -33,10 +34,6 @@ public void setIntakeSpeed(double speed){ intakeSpeed = speed; } - // public boolean isIntaking() { - // return inputs.currentAmps > IntakeConstants.intakeCurrent && inputs.angularVelocityRPM > 2400 && !Indexer.getInstance().isStoring(); - // } - public Command intake() { return startEnd(() -> setIntakeSpeed(IntakeSetpoints.intake), () -> setIntakeSpeed(0)); } @@ -48,7 +45,7 @@ public Command reverse() { @Override public void periodic(){ io.updateInputs(inputs); - Logger.processInputs("Intake", inputs); + Logger.processInputs("Intake", (LoggableInputs) inputs); io.setMotorSpeed(intakeSpeed); Logger.recordOutput("Intake/Intake speed", intakeSpeed); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index e4b8d64..92ee792 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -8,4 +8,5 @@ public final class IntakeConstants { public static final double overrideSpeed = -0.0; public static final double intakeCurrent = 0; + public static final double currentLimit = 30; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 8307716..c251860 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -3,7 +3,6 @@ import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { - @AutoLog public static class IntakeIOInputs { public double voltage = 0.0; public double angularPositionRot = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index 0a3277d..dcacee2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -7,6 +7,10 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + public class IntakeIOReal implements IntakeIO { private final TalonFX intakeMotor = new TalonFX(IntakeConstants.canID); @@ -14,15 +18,15 @@ public class IntakeIOReal implements IntakeIO { private TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - // private final StatusSignal current = intakeMotor.getStatorCurrent(); - // private final StatusSignal voltage = intakeMotor.getMotorVoltage(); - // private final StatusSignal velocity = intakeMotor.getVelocity(); + private final StatusSignal current = intakeMotor.getStatorCurrent(); + private final StatusSignal voltage = intakeMotor.getMotorVoltage(); + private final StatusSignal velocity = intakeMotor.getVelocity(); public IntakeIOReal() { - // intakeMotor.setInverted(true); + intakeMotor.setInverted(true); talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; - // talonFXConfig.CurrentLimits.StatorCurrentLimit = CurrentLimits.intakeKraken; + talonFXConfig.CurrentLimits.StatorCurrentLimit = IntakeConstants.currentLimit; // talonFXConfig. @@ -32,9 +36,9 @@ public IntakeIOReal() { intakeMotor.clearStickyFaults(); - // BaseStatusSignal.setUpdateFrequencyForAll(50, velocity, voltage, current); + BaseStatusSignal.setUpdateFrequencyForAll(50, velocity, voltage, current); - // intakeMotor.optimizeBusUtilization(1.0); + intakeMotor.optimizeBusUtilization(1.0); StatusCode response = intakeMotor.getConfigurator().apply(talonFXConfig); if (!response.isOK()) { @@ -46,13 +50,14 @@ public IntakeIOReal() { } } - // public void updateInputs(IntakeIOInputs inputs) { - // BaseStatusSignal.refreshAll(velocity, voltage, current); + public void updateInputs(IntakeIOInputs inputs) { + BaseStatusSignal.refreshAll(velocity, voltage, current); - // inputs.angularVelocityRPM = velocity.getValueAsDouble() * 60; - // // inputs.angularPositionRot = intakeMotor.getPosition().getValueAsDouble(); - // inputs.currentAmps = current.getValueAsDouble(); - // inputs.voltage = voltage.getValueAsDouble(); + inputs.angularVelocityRPM = velocity.getValueAsDouble() * 60; + inputs.angularPositionRot = intakeMotor.getPosition().getValueAsDouble(); + inputs.currentAmps = current.getValueAsDouble(); + inputs.voltage = voltage.getValueAsDouble(); + } @Override public void setMotorSpeed(double speed) { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index f7b3e08..d03a77d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -2,12 +2,14 @@ import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.math.system.plant.LinearSystemId; public class IntakeIOSim implements IntakeIO { // not done - private final DCMotorSim motor = new DCMotorSim(null, null, null); + private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1), null, null); @Override public void updateInputs(IntakeIOInputs inputs) { From 7140073db2c2c0fb9b0e40bd936ca23e4663496e Mon Sep 17 00:00:00 2001 From: ColinH0 Date: Mon, 13 Jan 2025 17:13:33 -0500 Subject: [PATCH 006/110] tried to fix sim --- simgui-ds.json | 6 ++++++ src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 9 +++++++-- src/main/java/frc/robot/subsystems/intake/Intake.java | 5 +++-- src/main/java/frc/robot/subsystems/intake/IntakeIO.java | 3 +++ .../java/frc/robot/subsystems/intake/IntakeIOSim.java | 2 +- 7 files changed, 22 insertions(+), 6 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1bfebdf..c305837 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,7 +7,7 @@ public static final class IntakeSetpoints{ public static final double reverse = 0.7; } - public static final Mode currentMode = Mode.fromState(); + public static final Mode currentMode = Mode.SIM; public static enum Mode { REAL, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0f8efc1..d1846ea 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,6 +48,7 @@ public Robot() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + System.out.println("Hello World"); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 196ed21..331569d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.intake.Intake; @@ -14,10 +16,11 @@ public class RobotContainer { private final Intake intake; public RobotContainer() { + System.out.println("Hello World"); configureBindings(); switch (Constants.currentMode) { case REAL: - intake = Intake.initialize(new IntakeIOReal()); + intake = Intake.initialize(new IntakeIOSim()); break; case SIM: intake = Intake.initialize(new IntakeIOSim()); @@ -28,7 +31,9 @@ public RobotContainer() { } } - private void configureBindings() {} + private void configureBindings() { + + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 0ef08e3..344f1b4 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.intake; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeSetpoints; import frc.robot.subsystems.intake.IntakeIO.IntakeIOInputs; @@ -9,7 +10,7 @@ public class Intake extends SubsystemBase { private IntakeIO io; - private IntakeIOInputs inputs = new IntakeIOInputs(); + private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); private double intakeSpeed = 0.0; private static Intake instance; @@ -45,7 +46,7 @@ public Command reverse() { @Override public void periodic(){ io.updateInputs(inputs); - Logger.processInputs("Intake", (LoggableInputs) inputs); + Logger.processInputs("Intake", inputs); io.setMotorSpeed(intakeSpeed); Logger.recordOutput("Intake/Intake speed", intakeSpeed); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index c251860..bfa08ae 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -1,8 +1,11 @@ package frc.robot.subsystems.intake; +import javax.annotation.processing.SupportedOptions; + import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { + @AutoLog public static class IntakeIOInputs { public double voltage = 0.0; public double angularPositionRot = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index d03a77d..ea9883a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -9,7 +9,7 @@ public class IntakeIOSim implements IntakeIO { // not done - private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1), null, null); + private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1), null, 0.2); @Override public void updateInputs(IntakeIOInputs inputs) { From aab2de8acafbbfda3004cd8f89b67697a9e34287 Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Mon, 13 Jan 2025 17:39:48 -0500 Subject: [PATCH 007/110] update elevator code --- .../robot/subsystems/elevator/Elevator.java | 70 +++++++++++++++++-- .../elevator/ElevatorConstants.java | 5 ++ .../subsystems/elevator/ElevatorIOReal.java | 4 +- 3 files changed, 71 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index aa8dc9f..9981deb 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -3,10 +3,12 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.hardware.TalonFX; +import com.google.flatbuffers.Constants; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; @@ -33,17 +35,73 @@ public class Elevator extends SubsystemBase { - public ElevatorFeedforward feedforward = new ElevatorFeedforward(1, 2, 3); private ElevatorIO io; - private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + private ElevatorIOInputs inputs = new ElevatorIOInputs(); + + @AutoLogOutput + private double leftMovingSpeed = 0.0; + + @AutoLogOutput + private double rightMovingSpeed = 0.0; + + private double maxDistance = ElevatorConstants.maxDistance; + + private static Elevator instance; - public void tankDriveWithFeedforward(double leftVelocity, double rightVelocity) { - leftMotor.setVoltage(feedforward.calculate(leftVelocity)); - rightMotor.setVoltage(feedforward.calculate(rightVelocity)); + public static Elevator getInstance(){ + return instance; + } + + public static Elevator initialize(ElevatorIO io){ + if(instance == null){ + instance = new Elevator(io); + } + return instance; + } + + private Elevator(ElevatorIO elevatorIO){ + io = elevatorIO; + io.updateInputs(inputs); + } + + public void setMovingSpeedRPM(double leftSpeed, double rightSpeed){ + leftMovingSpeed = leftSpeed; + rightMovingSpeed = rightSpeed; + } + + private ElevatorFeedforward feedFoward = new ElevatorFeedforward(rightMovingSpeed, maxDistance, leftMovingSpeed); + + private PIDController controller = new PIDController( + ElevatorConstants.kP, + ElevatorConstants.kI, + ElevatorConstants.kD, + ElevatorConstants.constraints); + + public void setGoal(double goal){ + + } + + private void setElevatorMode(Elevator mode){ + + } + + public double getTruePosition() { + return getPosition(); + } + + public double getVelocity() { + return inputs.elevatorMotorVelocityRadPerSec; + } + + public void reset() { + controller.reset(getPosition()); + io.resetEncoders(); + this.setGoal(getPosition()); + } + } -} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..7fe6e91 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.elevator; + +public final class ElevatorConstants{ + public static final double maxDistance = 0.7112; +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index d5a096a..4d83631 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -12,6 +12,6 @@ public class ElevatorIOReal implements ElevatorIO { - private TalonFX topElevatorMotor = new TalonFX(0); - private TalonFX bottomElevatorMotor = new TalonFX(1); + private TalonFX leftElevatorMotor = new TalonFX(0); + private TalonFX rightElevatorMotor = new TalonFX(1); } \ No newline at end of file From 3637bd98c16c39c1e79aa5cde4efa247e1c10194 Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Mon, 13 Jan 2025 17:41:39 -0500 Subject: [PATCH 008/110] elevator changes --- .../java/frc/robot/subsystems/elevator/Elevator.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 9981deb..000d3a9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -61,7 +61,7 @@ public static Elevator initialize(ElevatorIO io){ private Elevator(ElevatorIO elevatorIO){ io = elevatorIO; - io.updateInputs(inputs); + io.updateElevatorIOInputs(inputs); } public void setMovingSpeedRPM(double leftSpeed, double rightSpeed){ @@ -89,12 +89,15 @@ public double getTruePosition() { return getPosition(); } + public double getPosition(){ + + } + public double getVelocity() { - return inputs.elevatorMotorVelocityRadPerSec; } public void reset() { - controller.reset(getPosition()); + controller.reset(); io.resetEncoders(); this.setGoal(getPosition()); } From a0738ffb49611c2a5037d44f0ec1c1bf01aaf006 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Mon, 13 Jan 2025 17:43:02 -0500 Subject: [PATCH 009/110] fixed intake motor sim argument issues --- .vscode/settings.json | 3 ++- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java | 2 +- 4 files changed, 3 insertions(+), 4 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index dccbc7c..0df8b28 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d1846ea..0f8efc1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,7 +48,6 @@ public Robot() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - System.out.println("Hello World"); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 331569d..07ecae6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,6 @@ public class RobotContainer { private final Intake intake; public RobotContainer() { - System.out.println("Hello World"); configureBindings(); switch (Constants.currentMode) { case REAL: diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index ea9883a..831557e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -9,7 +9,7 @@ public class IntakeIOSim implements IntakeIO { // not done - private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1), null, 0.2); + private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1), DCMotor.getKrakenX60(1), 0.0, 0.0); @Override public void updateInputs(IntakeIOInputs inputs) { From 3255874cefea7264136263fce69e299e43068ce6 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Mon, 13 Jan 2025 17:47:17 -0500 Subject: [PATCH 010/110] start an arm subsystem --- .../java/frc/robot/subsystems/arm/Arm.java | 5 ++ .../robot/subsystems/arm/ArmConstants.java | 5 ++ .../java/frc/robot/subsystems/arm/ArmIO.java | 19 ++++++ .../frc/robot/subsystems/arm/ArmIOReal.java | 68 +++++++++++++++++++ .../frc/robot/subsystems/arm/ArmIOSim.java | 5 ++ .../robot/subsystems/arm/CurrentLimits.java | 5 ++ 6 files changed, 107 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/arm/Arm.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmConstants.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmIO.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/arm/CurrentLimits.java diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..49857b2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.arm; + +public class Arm { + +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java new file mode 100644 index 0000000..a79523d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.arm; + +public class ArmConstants { + public static int canID = 10; +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java new file mode 100644 index 0000000..c84a0cb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.arm; + +import org.littletonrobotics.junction.AutoLog; + +public interface ArmIO { + @AutoLog + public static class ArmIOInputs{ + public double voltage = 0.0; + public double angularPosition = 0.0; + public double angularVelocity = 0.0; + public double current = 0.0; + } + public default void setVoltage(double voltage) { + + } + public default void setVelocity(double velocity){ + + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java new file mode 100644 index 0000000..f189900 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems.arm; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.CurrentLimits; +import frc.robot.util.KrakenLogger; + +public class ArmIOReal implements ArmIO { + private final TalonFX armMotor = new TalonFX(ArmConstants.canID); + + private TalonFXConfiguration talonnFXConfig = new TalonFXConfiguration(); + + + private final StatusSignal current = armMotor.getStatorCurrent(); + private final StatusSignal voltage = armMotor.getMotorVoltage(); + private final StatusSignal velocity = armMotor.getVelocity(); + + public ArmIOReal(){ + TalonFXConfiguration talonFXConfig; + talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; + Object talonFX; + talonFX.Config.CurrentLimits.StatorCurrentLimit = CurrentLimits.armKraken; + + talonFXConfig.Audio.BeepOnBoot = true; + + talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.coast; + + armMotor.clearStickyFaults(); + + BaseStatusSignal.setUpdateFrequencyForAll (50,velocity, voltage, current ); + + armMotor.optimizeBusUtilization(1.0); + + StatusCode response = armMotor.getConfigurator().apply(talonFXConfig); + if(!response.isOK()){ + System.out.println( + "Talon ID" + + armMotor.getDeviceID() + + "failed config with error" + + response.toString()); + } + } + + public void updateInputs(ArmIOInputs inputs) { + BaseStatusSignal.refreshAll(velocity, voltage, current); + + inputs.angularVelocity = velocity.getValueAsDouble()*60; + inputs.current = current.getValueAsDouble(); + inputs.voltage = voltage.getValueAsDouble(); + } + + @Override + public void setVelocity(double velocity) { + armMotor.set(-velocity); + } + + public void setVoltage(double voltage) { + armMotor.set(-voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java new file mode 100644 index 0000000..043f05a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.arm; + +public class ArmIOSim { + +} diff --git a/src/main/java/frc/robot/subsystems/arm/CurrentLimits.java b/src/main/java/frc/robot/subsystems/arm/CurrentLimits.java new file mode 100644 index 0000000..1dd5d3a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/CurrentLimits.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.arm; + +public class CurrentLimits { + +} From cd10bde857e5878fc8525a23359333eb1b0546f5 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Mon, 13 Jan 2025 18:19:44 -0500 Subject: [PATCH 011/110] Swerve code written --- src/main/java/frc/robot/Drive/Drive.java | 288 ++++++++++++++++++ .../java/frc/robot/Drive/DriveConstants.java | 14 + src/main/java/frc/robot/Drive/GyroIO.java | 20 ++ .../java/frc/robot/Drive/GyroIOPigeon.java | 38 +++ src/main/java/frc/robot/Drive/Module.java | 159 ++++++++++ src/main/java/frc/robot/Drive/ModuleIO.java | 34 +++ .../java/frc/robot/Drive/ModuleIOSim.java | 24 ++ .../java/frc/robot/Drive/ModuleIOTalonFX.java | 5 + 8 files changed, 582 insertions(+) diff --git a/src/main/java/frc/robot/Drive/Drive.java b/src/main/java/frc/robot/Drive/Drive.java index e69de29..6f224dc 100644 --- a/src/main/java/frc/robot/Drive/Drive.java +++ b/src/main/java/frc/robot/Drive/Drive.java @@ -0,0 +1,288 @@ +package frc.robot.Drive; + +import static edu.wpi.first.units.Units.*; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.swerve.SwerveModule; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Kinematics; +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.math.util.Units; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.AngularVelocityUnit; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.VelocityUnit; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.units.mutable.MutableMeasureBase; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Drive.DriveConstants.Mode; +import frc.robot.Drive.GyroIO.GyroIOInputs; +import frc.robot.Drive.ModuleIO.ModuleIOInputs; + + +public class Drive extends SubsystemBase { + private static Drive instance; + private static final double MAX_LINEAR_SPEED = Units.feetToMeters(5); + private static final double TRACK_WIDTH_X = Units.inchesToMeters(21.25); + private static final double TRACK_WIDTH_Y = Units.inchesToMeters(21.25); + private static final double DRIVE_BASE_RADIUS = + Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroIOInputs = new GyroIOInputsAutoLogged(); + private final ModuleIOInputs inputs = new ModuleIOInputs(); + private final Module[] modules = new Module[4]; + private final SysIdRoutine sysId; + private final MutableMeasure m_appliedVoltage = new MutVoltage(0, 0, Volt); + private final MutableMeasure m_position = new MutAngle(0,0, Radian); + private final MutableMeasure m_velocity = new MutAngularVelocity(0, 0, RadiansPerSecond); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations2d()); + private Rotation2d rawGyroRotation2d = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation2d, lastModulePositions, new Pose2d()); + + public static Drive getInstance(){ + return instance; + } + + public static Drive initialize (GyroIO gyro, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { + if (instance == null) { + instance = new Drive(gyro, fl, fr, bl, br); + } + return instance; + } + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + + AutoBuilder.configureHolonomic( + this::getPose, + this::setPose, + () -> Kinematics.toChassisSpeeds(getModuleStates()), + this:: runVelocity, + new HolonomicPathFollowerConfig( + MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red, + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])) + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + sysId = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (voltage) -> { + for (int i = 0; i < 4; i++){ + modules[i].runCharacterization(voltage.in(Volts)); + } + }, + log -> { + log.motor("driveSparkMax") + .voltage(m_appliedVoltage.mustreplace(inputs.driveAppliedVolts, Volts)) + .angularPosition(m_position.must_replace(inputs.drivePositionRad, Radians)) + .AngularVelocity( + m_velocity.must_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond)); + + }, + this)); + } + + public void periodic() { + gyroIO.updateInputs(Inputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + + + if (DriverStation.isDisabled()){ + for (var module : modules){ + module.stop(); + } + } + + if (DriverStation.isDisabled()){ + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + SwerveModulePosition[] modulePositions = SwerveModulePosition(); + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex ++){ + moduleDeltas [moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + if (gyr.connected) { + + rawGyroRotation2d = gyroInputs.yawPosition; + } else { + + Twist2d twist = Kinematics.toTwist2d(moduleDeltas); + rawGyroRotation2d = rawGyroRotation2d.plus(new Rotation2d(twist.dtheta)); + } + +poseEstimator.update(rawGyroRotation2d, modulePositions); + + } + + +public void runVelocity(chassisSpeeds speeds){ + + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = Kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates,MAX_LINEAR_SPEED); + + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); +} + +public void (){ + runVelocity(new ChassisSpeeds()); +} + + + + +public void stopWithx() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + Kinematics.resetHeadings(headings); + stop(); +} + + public void zeroHeading() { + if (DriveConstants.currentMode == Mode.SIM){ + poseEstimator.resetPosition( + new Rotation2d(), + new SwerveModulePosition[]{ + modules[0].getPosition(), + modules[1].getPosition(), + modules[2].getPosition(), + modules[3].getPosition(), + }, + getPose()); + } + + gyroIO.reset(); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } + + @AutoLogOutput (key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates(){ + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++){ + states[i] = modules[i].getState(); + } + return states; + } + + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation2d, getModulePositions(),pose); + } + + + + + + + + + public void addVisionMeasurement(Pose2d visionPose, double timestamp){ + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + + + + public double getMaxLinearSpeedMeterPerSec(){ + return MAX_LINEAR_SPEED; + } + + + public double getMaxAngluarSpeeddRadPerSec(){ + return MAX_ANGULAR_SPEED; + } + + + public static Translation2d[] getModuleTranslation2ds(){ + return new Translation2d[]{ + new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), + new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), + new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), + new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), + }; + } + } + \ No newline at end of file diff --git a/src/main/java/frc/robot/Drive/DriveConstants.java b/src/main/java/frc/robot/Drive/DriveConstants.java index e69de29..41473d1 100644 --- a/src/main/java/frc/robot/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/Drive/DriveConstants.java @@ -0,0 +1,14 @@ +package frc.robot.Drive; + +public class DriveConstants { + public static final Mode currentMode = Mode.REAL; + + public static enum Mode { + REAL, + + SIM, + + REPLAY, + } + +} diff --git a/src/main/java/frc/robot/Drive/GyroIO.java b/src/main/java/frc/robot/Drive/GyroIO.java index e69de29..c50d242 100644 --- a/src/main/java/frc/robot/Drive/GyroIO.java +++ b/src/main/java/frc/robot/Drive/GyroIO.java @@ -0,0 +1,20 @@ +package frc.robot.Drive; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Rotation2d; + + +public interface GyroIO { + @AutoLog +public static class GyroIOInputs{ + public boolean conected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; +} + +public default void reset() {} + +public default void updateInputs(GyroIOInputs inputs) {} +} + diff --git a/src/main/java/frc/robot/Drive/GyroIOPigeon.java b/src/main/java/frc/robot/Drive/GyroIOPigeon.java index e69de29..80f357d 100644 --- a/src/main/java/frc/robot/Drive/GyroIOPigeon.java +++ b/src/main/java/frc/robot/Drive/GyroIOPigeon.java @@ -0,0 +1,38 @@ +package frc.robot.Drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +public class GyroIOPigeon implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2(20); + private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(100); + yawVelocity.setUpdateFrequency(100); + pigeon.optimizeBusUtilization(); + } + +@Override +public void reset(){ + pigeon.reset(); +} + +@Override +public void updateInputs(GyroIOInputs inputs) { + inputs.conected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); +} +} diff --git a/src/main/java/frc/robot/Drive/Module.java b/src/main/java/frc/robot/Drive/Module.java index e69de29..379460e 100644 --- a/src/main/java/frc/robot/Drive/Module.java +++ b/src/main/java/frc/robot/Drive/Module.java @@ -0,0 +1,159 @@ +package frc.robot.Drive; + +import com.pathplanner.lib.util.DriveFeedforwards; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import org.littletonrobotics.junction.Logger; + +public class Module { + private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final SimpleMotorFeedforward drivFeedforward; + private final PIDController driveFeddbackPidController; + private final PIDController turnFeePidController; + private Rotation2d angleSetpoint = null; + private Double speedSetPoint = null; + private Rotation2d turnRelativeOffset = null; + + public Module (ModuleIO io, int index) { + this.io = io; + this.index = index; + + switch (DriveConstants.currentMode): + case REAL: + drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); + turnFeePidController = new PIDController(3.5, 0.0, 0.0); + break; + case REPLAY: + drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); + turnFeePidController = new PIDController(7, 0.0, 0.0); + break; + case SIM: + drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); + turnFeePidController = new PIDController(10, 0.0, 0.0); + break; + default: + drivFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + driveFeddbackPidController = new PIDController(0.0, 0.0, 0.0); + turnFeePidController = new PIDController(0.0, 0.0, 0.0); + break; + } + + turnFeedback.enableContinuousInput(-Math.PI,Math.PI); + setBrakeMode(ture); + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module"+ Integer.toString(index), inputs); + + + + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeePidController.calculate(getAngle(), angleSetpoint.getRadians())); + + + if (speedSetPoint != null) { + + + + + double adjustSpeedSetpoint = adjustSpeedSetpoint*Math.cos(turnFeePidController.getPositionError()); + + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + drivFeedforward.calculate(velocityRadPerSec) + + driveFeddbackPidController.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + } + } + } + + + public SwerveModuleState runSetpoint(SwerveModulePosition state) { + + + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + + + angleSetpoint = optimizedState.angle; + speedSetPoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + public void runCharacterization(double volts) { + + angleSetpoint = new Rotation2d(); + + + io.setDriveVoltage(volts); + speedSetPoint = null; + } + + + public void stop(){ + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); + + + angleSetpoint = null; + speedSetPoint = null; + } + + + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + + public Rotation2d getAngle (){ + if (turnRelativeOffset == null){ + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } + } + + + public double getVelocityRadPerSec(){ + return inputs.driveVelocityRadPerSec*WHEEL_RADIUS; + } + + + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + + public SwerveModuleState getState () { + return new SwerveModuleState(getVelocityRadPerSec(), getAngle()); + } + + + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/Drive/ModuleIO.java b/src/main/java/frc/robot/Drive/ModuleIO.java index e69de29..75fee9f 100644 --- a/src/main/java/frc/robot/Drive/ModuleIO.java +++ b/src/main/java/frc/robot/Drive/ModuleIO.java @@ -0,0 +1,34 @@ +package frc.robot.Drive; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface ModuleIO { + + @AutoLog + public static class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedRadPerSec = 0.0; + public double[] turnCurrentAmps = new double[] {}; + } + + public default void updateInputs(ModuleIOInputs inputs) {} + + public default void setDriveVoltage(double volts) {} + + public default void runTalonPID (double desiredStateRotPerSecond) {} + + public default void setTurnVoltage (double volts) {} + + public default void setDriveBrakeMode(boolean enable) {} + + public default void setTurnBrakeMode(boolean enable) {} +} diff --git a/src/main/java/frc/robot/Drive/ModuleIOSim.java b/src/main/java/frc/robot/Drive/ModuleIOSim.java index e69de29..688b953 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/Drive/ModuleIOSim.java @@ -0,0 +1,24 @@ +package frc.robot.Drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Drive.ModuleIO.ModuleIOInputs; + +public class ModuleIOSim implements ModuleIO { + private static final double LOOP_PERIOD_SECS = 0.02; + //change gearbox argument + private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 6.75, 0.025), DCMotor.getNeoVortex(4)); + private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 6.75, 0.025), DCMotor.getKrakenX60(4)); + + private final Rotation2d turnAbsoluteInitPositon = new Rotation2d(Math.random()*2.0*Math.PI); + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + @Override + public void updateInputs(ModuleIOInputs inputs){ + driveSim.update(LOOP_PERIOD_SECS); + turnSim.update(LOOP_PERIOD_SECS); + } +} diff --git a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java index e69de29..6a1c69d 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java @@ -0,0 +1,5 @@ +package frc.robot.Drive; + +public class ModuleIOTalonFX { + +} From 3a603225bd46082942af2b465a1a7a1c19aadef2 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Mon, 13 Jan 2025 18:20:34 -0500 Subject: [PATCH 012/110] Swerve written --- src/main/java/frc/robot/Drive/Drive.java | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Drive/Drive.java b/src/main/java/frc/robot/Drive/Drive.java index 6f224dc..237ed30 100644 --- a/src/main/java/frc/robot/Drive/Drive.java +++ b/src/main/java/frc/robot/Drive/Drive.java @@ -55,14 +55,15 @@ public class Drive extends SubsystemBase { private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroIOInputs = new GyroIOInputsAutoLogged(); - private final ModuleIOInputs inputs = new ModuleIOInputs(); + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final Module[] modules = new Module[4]; private final SysIdRoutine sysId; private final MutableMeasure m_appliedVoltage = new MutVoltage(0, 0, Volt); private final MutableMeasure m_position = new MutAngle(0,0, Radian); private final MutableMeasure m_velocity = new MutAngularVelocity(0, 0, RadiansPerSecond); + private static SwerveDriveKinematics Modulekinematics = new SwerveDriveKinematics(getModuleTranslation2d()); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations2d()); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslation2d()); private Rotation2d rawGyroRotation2d = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = new SwerveModulePosition[] { @@ -141,8 +142,8 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), } public void periodic() { - gyroIO.updateInputs(Inputs); - Logger.processInputs("Drive/Gyro", gyroInputs); + gyroIO.updateInputs(inputs); + Logger.processInputs("Drive/Gyro", gyroIOInputs); for (var module : modules) { module.periodic(); } @@ -170,7 +171,7 @@ public void periodic() { lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } - if (gyr.connected) { + if (gyro.connected) { rawGyroRotation2d = gyroInputs.yawPosition; } else { @@ -184,7 +185,7 @@ public void periodic() { } -public void runVelocity(chassisSpeeds speeds){ +public void runVelocity(ChassisSpeeds speeds){ ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = Kinematics.toSwerveModuleStates(discreteSpeeds); @@ -199,7 +200,7 @@ public void runVelocity(chassisSpeeds speeds){ Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); } -public void (){ +public void stop(){ runVelocity(new ChassisSpeeds()); } @@ -211,7 +212,7 @@ public void stopWithx() { for (int i = 0; i < 4; i++) { headings[i] = getModuleTranslations()[i].getAngle(); } - Kinematics.resetHeadings(headings); + Modulekinematics.resetHeadings(); stop(); } @@ -276,7 +277,7 @@ public double getMaxAngluarSpeeddRadPerSec(){ } - public static Translation2d[] getModuleTranslation2ds(){ + public static Translation2d[] getModuleTranslation2d(){ return new Translation2d[]{ new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), From e646b200f14741ae0768b7bef45111f93ed0e922 Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Mon, 13 Jan 2025 18:21:02 -0500 Subject: [PATCH 013/110] elevator code update (more constant reference later --- .../robot/subsystems/elevator/Elevator.java | 60 +++++++------------ .../elevator/ElevatorConstants.java | 4 ++ .../robot/subsystems/elevator/ElevatorIO.java | 1 + .../subsystems/elevator/ElevatorIOInputs.java | 5 -- .../subsystems/elevator/ElevatorIOReal.java | 6 +- .../subsystems/elevator/ElevatorIOSim.java | 1 + 6 files changed, 31 insertions(+), 46 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputs.java diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 000d3a9..279d214 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -1,42 +1,17 @@ package frc.robot.subsystems.elevator; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; -import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.hardware.TalonFX; -import com.google.flatbuffers.Constants; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.CircularBuffer; -import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.motorcontrol.PWMTalonSRX; -import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; + import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.math.controller.PIDController; public class Elevator extends SubsystemBase { private ElevatorIO io; - private ElevatorIOInputs inputs = new ElevatorIOInputs(); + private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); @AutoLogOutput private double leftMovingSpeed = 0.0; @@ -47,11 +22,24 @@ public class Elevator extends SubsystemBase { private double maxDistance = ElevatorConstants.maxDistance; private static Elevator instance; + + private PIDController controller = new PIDController( + ElevatorConstants.kP, + ElevatorConstants.kI, + ElevatorConstants.kD, + ElevatorConstants.constraints); + + private ElevatorFeedforward feedforward = new ElevatorFeedforward(rightMovingSpeed, maxDistance, leftMovingSpeed); + public static Elevator getInstance(){ return instance; } + public void periodic () { + + } + public static Elevator initialize(ElevatorIO io){ if(instance == null){ instance = new Elevator(io); @@ -61,7 +49,7 @@ public static Elevator initialize(ElevatorIO io){ private Elevator(ElevatorIO elevatorIO){ io = elevatorIO; - io.updateElevatorIOInputs(inputs); + io.updateInputs(inputs); } public void setMovingSpeedRPM(double leftSpeed, double rightSpeed){ @@ -69,31 +57,23 @@ public void setMovingSpeedRPM(double leftSpeed, double rightSpeed){ rightMovingSpeed = rightSpeed; } - private ElevatorFeedforward feedFoward = new ElevatorFeedforward(rightMovingSpeed, maxDistance, leftMovingSpeed); - - private PIDController controller = new PIDController( - ElevatorConstants.kP, - ElevatorConstants.kI, - ElevatorConstants.kD, - ElevatorConstants.constraints); public void setGoal(double goal){ } - private void setElevatorMode(Elevator mode){ - - } - public double getTruePosition() { return getPosition(); } public double getPosition(){ + return maxDistance; //change later } public double getVelocity() { + return maxDistance; //change later + } public void reset() { @@ -102,6 +82,7 @@ public void reset() { this.setGoal(getPosition()); } + } @@ -109,3 +90,4 @@ public void reset() { + diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 7fe6e91..0d61fea 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -2,4 +2,8 @@ public final class ElevatorConstants{ public static final double maxDistance = 0.7112; + public static double constraints; //figure this out later + public static double kD; //figure this out later + public static double kI; //figure this out later + public static double kP; //figure this out later } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 726cb29..1551dc3 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -25,5 +25,6 @@ public default void setVoltage(double volts) {} public default void setIdleMode(boolean mode) {} public default void resetEncoders() {} + } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputs.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputs.java deleted file mode 100644 index 008b940..0000000 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputs.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.elevator; - -public interface ElevatorIOInputs { - -} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 4d83631..0ab7936 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -12,6 +12,8 @@ public class ElevatorIOReal implements ElevatorIO { - private TalonFX leftElevatorMotor = new TalonFX(0); - private TalonFX rightElevatorMotor = new TalonFX(1); + private TalonFX rightElevatorMotor = new TalonFX(0); + private TalonFX leftElevatorMotor = new TalonFX(1); + + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 3fb9c45..1dcf1ff 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -7,4 +7,5 @@ import edu.wpi.first.wpilibj.simulation.ElevatorSim; public class ElevatorIOSim implements ElevatorIO { + } \ No newline at end of file From 77d05edf78d2e077394a35d880027f624d3a6db0 Mon Sep 17 00:00:00 2001 From: ColinH0 <119466406+ColinH0@users.noreply.github.com> Date: Tue, 14 Jan 2025 18:00:32 -0500 Subject: [PATCH 014/110] Intake sim works, working on real --- src/main/java/frc/robot/Constants.java | 6 ++++++ src/main/java/frc/robot/RobotContainer.java | 15 ++++++++++++--- .../java/frc/robot/subsystems/intake/Intake.java | 5 ++++- .../robot/subsystems/intake/IntakeConstants.java | 2 ++ .../frc/robot/subsystems/intake/IntakeIOReal.java | 6 ++++-- 5 files changed, 28 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c305837..dd50fe3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,5 +1,7 @@ package frc.robot; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + public final class Constants { public static final class IntakeSetpoints{ //placeholders @@ -23,4 +25,8 @@ static Mode fromState() { } } } + public static class OperatorConstants { + public static final int kDriverControllerPort = 0; + } } + diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07ecae6..6cbcf5e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,15 +8,20 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIOReal; import frc.robot.subsystems.intake.IntakeIOSim; public class RobotContainer { private final Intake intake; - + + private final CommandXboxController m_driverController = + new CommandXboxController(OperatorConstants.kDriverControllerPort); + public RobotContainer() { - configureBindings(); + switch (Constants.currentMode) { case REAL: intake = Intake.initialize(new IntakeIOSim()); @@ -28,10 +33,14 @@ public RobotContainer() { intake = Intake.initialize(new IntakeIOSim()); } + configureBindings(); } private void configureBindings() { - + m_driverController.a().whileTrue(Commands.startEnd(()->intake.setIntakeSpeed(1), + ()->intake.setIntakeSpeed(0), intake)); + m_driverController.b().whileTrue(Commands.startEnd(()->intake.setIntakeSpeed(-1), + ()->intake.setIntakeSpeed(0), intake)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 344f1b4..7d194d1 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -42,7 +42,10 @@ public Command intake() { public Command reverse() { return startEnd(() -> setIntakeSpeed(IntakeSetpoints.reverse), () -> setIntakeSpeed(0)); } - + public boolean isIntaking() { + return inputs.currentAmps > IntakeConstants.intakeCurrent && inputs.angularVelocityRPM > 2400; + } + @Override public void periodic(){ io.updateInputs(inputs); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 92ee792..3dee92b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.intake; +import com.ctre.phoenix6.hardware.DeviceIdentifier; + public final class IntakeConstants { //PLACEHOLDERS public static final int canID = 0; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index dcacee2..49d3afd 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -4,7 +4,9 @@ import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.AngularVelocity; @@ -16,14 +18,14 @@ public class IntakeIOReal implements IntakeIO { private final TalonFX intakeMotor = new TalonFX(IntakeConstants.canID); private TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - + // private TalonFXConfigurator taIonFXConfig = new TalonFXConfigurator(IntakeConstants.canID) private final StatusSignal current = intakeMotor.getStatorCurrent(); private final StatusSignal voltage = intakeMotor.getMotorVoltage(); private final StatusSignal velocity = intakeMotor.getVelocity(); public IntakeIOReal() { - intakeMotor.setInverted(true); + talonFXConfig.MotorOutput.withInverted(InvertedValue.Clockwise_Positive); talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; talonFXConfig.CurrentLimits.StatorCurrentLimit = IntakeConstants.currentLimit; From 42d1fd6bb950ee0ae3f3457405adeb15e6f6fefb Mon Sep 17 00:00:00 2001 From: Arenacloserr <162641455+Arenacloserr@users.noreply.github.com> Date: Tue, 14 Jan 2025 18:18:51 -0500 Subject: [PATCH 015/110] Talon code written --- src/main/java/frc/robot/Drive/ModuleIO.java | 3 +- .../frc/robot/Drive/ModuleIOSparkMax.Java | 153 ++++++++++++ .../java/frc/robot/Drive/ModuleIOTalonFX.java | 227 +++++++++++++++++- 3 files changed, 380 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Drive/ModuleIO.java b/src/main/java/frc/robot/Drive/ModuleIO.java index 75fee9f..535cbc2 100644 --- a/src/main/java/frc/robot/Drive/ModuleIO.java +++ b/src/main/java/frc/robot/Drive/ModuleIO.java @@ -13,8 +13,9 @@ public static class ModuleIOInputs { public double driveAppliedVolts = 0.0; public double[] driveCurrentAmps = new double[] {}; - public Rotation2d turnbsolutePosition = new Rotation2d(); + public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); + public double turnAppliedVolts = 0.0; public double turnVelocityRadPerSec = 0.0; public double turnAppliedRadPerSec = 0.0; public double[] turnCurrentAmps = new double[] {}; diff --git a/src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java b/src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java index e69de29..30be722 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java +++ b/src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java @@ -0,0 +1,153 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +/** + * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO + * or NEO 550), and analog absolute encoder connected to the RIO + * + *

NOTE: This implementation should be used as a starting point and adapted to different hardware + * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOSparkMax implements ModuleIO { + // Gear ratios for SDS MK4i L2, adjust as necessary + private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final CANSparkMax driveSparkMax; + private final CANSparkFlex turnSparkMax; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AbsoluteEncoder turnAbsoluteEncoder; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOSparkMax(int index) { + switch (index) { + case 0: + driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); + turnSparkMax = new CANSparkFlex(2, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveSparkMax = new CANSparkMax(10, MotorType.kBrushless); + turnSparkMax = new CANSparkFlex(9, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveSparkMax = new CANSparkMax(4, MotorType.kBrushless); + turnSparkMax = new CANSparkFlex(6, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); + turnSparkMax = new CANSparkFlex(8, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); + + turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians + turnAbsoluteEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); // Radians per second + + driveSparkMax.setCANTimeout(250); + turnSparkMax.setCANTimeout(250); + + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + driveSparkMax.setSmartCurrentLimit(40); + turnSparkMax.setSmartCurrentLimit(30); + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); + inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; + + inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition()); + inputs.turnPosition = + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + } + + @Override + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java index 6a1c69d..2c2cfbf 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java @@ -1,5 +1,228 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + package frc.robot.Drive; -public class ModuleIOTalonFX { +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.AlternateEncoderConfig; +import com.revrobotics.spark.config.AlternateEncoderConfigAccessor; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.ExternalEncoderConfig; +import com.revrobotics.spark.config.ExternalEncoderConfigAccessor; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +/** + * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO + * or NEO 550), and analog absolute encoder connected to the RIO + * + *

NOTE: This implementation should be used as a starting point and adapted to different hardware + * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOTalonFX implements ModuleIO { + final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); + // Gear ratios for SDS MK4i L2, adjust as necessary + private static final double DRIVE_GEAR_RATIO = 5.46; + private static final double TURN_GEAR_RATIO = 11.3142; + + private final TalonFX driveTalonFX; + private final SparkFlex turnSparkMax; + + private final RelativeEncoder turnRelativeEncoder; + private final AbsoluteEncoder turnAbsoluteEncoder; + + private final boolean isTurnMotorInverted = true; + + public ModuleIOTalonFX(int index) { + + switch (index) { + case 0: + // Front left + driveTalonFX = new TalonFX(1); + turnSparkMax = new SparkFlex(2, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + driveTalonFX.setInverted(false); + break; + case 1: + // Front right + driveTalonFX = new TalonFX(10); + turnSparkMax = new SparkFlex(9, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + break; + case 2: + // Back left + driveTalonFX = new TalonFX(4); + turnSparkMax = new SparkFlex(6, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + driveTalonFX.setInverted(false); + break; + case 3: + // Back right + driveTalonFX = new TalonFX(7); + turnSparkMax = new SparkFlex(8, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + break; + default: + throw new RuntimeException("Invalid module index"); + } -} + //final TalonFXConfiguration drivingConfig = new TalonFXConfiguration(); + //turnSparkMax.restoreFactoryDefaults(); + + //turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians + //turnAbsoluteEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); // Radians per second + + turnSparkMax.setCANTimeout(250); + + turnRelativeEncoder = turnSparkMax.getEncoder(); // ??? + final ExternalEncoderConfig encoderconfig = new ExternalEncoderConfig(); + + encoderconfig.measurementPeriod(10); + turnRelativeEncoder.setPosition(0.0); + encoderconfig.measurementPeriod(10); + encoderconfig.averageDepth(2); + + turnSparkMax.setInverted(isTurnMotorInverted); + + driveTalonFX.setPosition(0); + driveTalonFX.setInverted(true); + + turnRelativeEncoder.setPosition(0.0); + encoderconfig.measurementPeriod(10); + encoderconfig.averageDepth(2); + + turnSparkMax.setCANTimeout(0); + + //turnSparkMax.burnFlash(); + + var config = config(); + var configSpark = configSpark(null); + + driveTalonFX.optimizeBusUtilization(); + + driveTalonFX.clearStickyFaults(); + + StatusCode status = StatusCode.StatusCodeNotInitialized; + REVLibError status2 = REVLibError.kError; // not work maybe + for (int i = 0; i < 5; i++) { + status = driveTalonFX.getConfigurator().apply(config); + turnSparkMax.configure(configSpark, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + if (status.isOK()) break; + } + + if (!status.isOK()) { + System.out.println( + "Talon ID " + + driveTalonFX.getDeviceID() + + " failed config with error " + + status.toString()); + } + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(driveTalonFX.getPosition().getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble()) + / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveTalonFX.getMotorVoltage().getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveTalonFX.getSupplyCurrent().getValueAsDouble()}; + + inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition()); + inputs.turnPosition = + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + } + + private SparkMaxConfig configSpark(IdleMode idle) { + var turningConfig = new SparkMaxConfig(); + + turningConfig.smartCurrentLimit(30); + turningConfig.voltageCompensation(12.0); + + if (idle != null) { + turningConfig.idleMode(idle); + } + + return turningConfig; + } + + private TalonFXConfiguration config() { + var talonFXConfig = new TalonFXConfiguration(); + + talonFXConfig.Slot0.kP = 1; + talonFXConfig.Slot0.kI = 0; + talonFXConfig.Slot0.kD = 0; + talonFXConfig.Slot0.kS = 0; + talonFXConfig.Slot0.kV = 0; + + talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; + talonFXConfig.CurrentLimits.StatorCurrentLimit = 40; // CurrentLimits.drive; + + talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + talonFXConfig.Audio.BeepOnBoot = true; + + return talonFXConfig; + } + + @Override + public void setDriveVoltage(double volts) { + driveTalonFX.setVoltage(volts); + } + + @Override + public void runTalonPID(double desiredStateRotPerSecond) { + driveTalonFX.setControl(m_request.withVelocity(desiredStateRotPerSecond)); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + // @Override + // public void setDriveBrakeMode(boolean enable) { + // driveTalonFX.setBrakeMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + // } + + @Override + public void setTurnBrakeMode(boolean enable) { + // turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + configSpark(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +} \ No newline at end of file From ea12046fc73685bc30b6498aae22b8134d2f8dbe Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Tue, 14 Jan 2025 18:23:03 -0500 Subject: [PATCH 016/110] more elevtor stuff --- .../robot/subsystems/elevator/Elevator.java | 1 + .../elevator/ElevatorConstants.java | 1 + .../subsystems/elevator/ElevatorIOReal.java | 26 +++++++++++++------ .../subsystems/elevator/ElevatorIOSim.java | 22 ++++++++++++++++ 4 files changed, 42 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 279d214..6be6b1f 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -51,6 +51,7 @@ private Elevator(ElevatorIO elevatorIO){ io = elevatorIO; io.updateInputs(inputs); } + public void setMovingSpeedRPM(double leftSpeed, double rightSpeed){ leftMovingSpeed = leftSpeed; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 0d61fea..af0b88e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -6,4 +6,5 @@ public final class ElevatorConstants{ public static double kD; //figure this out later public static double kI; //figure this out later public static double kP; //figure this out later + } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 0ab7936..eb1b2f8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -1,19 +1,29 @@ package frc.robot.subsystems.elevator; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; +import com.revrobotics.AbsoluteEncoder; + public class ElevatorIOReal implements ElevatorIO { private TalonFX rightElevatorMotor = new TalonFX(0); - private TalonFX leftElevatorMotor = new TalonFX(1); + private TalonFX leftElevatorMotor = new TalonFX(1); + + private double inputVolts = 0.0; + + final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); + + + + public ElevatorIOReal(){ + leftElevatorMotor.setControl(new Follower(rightElevatorMotor.getDeviceID(), true)); + + + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 1dcf1ff..765d243 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,11 +1,33 @@ package frc.robot.subsystems.elevator; +import java.lang.System.Logger; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; + public class ElevatorIOSim implements ElevatorIO { + private ElevatorSim elevatorSim = new ElevatorSim(0, 0, null, 0, 0, false, 0, null); + + private double ElevatorAppliedVolts = 0.0; + + + +@Override +public void moveElevator(double speed) { + ElevatorAppliedVolts = MathUtil.clamp(speed, speed, speed); + +} +@Override +public void setVoltage(double volts){ + ElevatorAppliedVolts = volts; +} + } \ No newline at end of file From 0671de3145cb99903806a12bd61a23583505275d Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Thu, 16 Jan 2025 16:22:43 -0500 Subject: [PATCH 017/110] arm stuff --- src/main/java/frc/robot/Constants.java | 6 +++ .../java/frc/robot/subsystems/arm/ArmIO.java | 10 ++--- .../frc/robot/subsystems/arm/ArmIOReal.java | 27 ++++++------ .../frc/robot/subsystems/arm/ArmIOSim.java | 41 ++++++++++++++++++- 4 files changed, 64 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eb7d9c5..57f0baa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,5 +1,7 @@ package frc.robot; +import frc.robot.subsystems.arm.CurrentLimits; + public class Constants { public static Mode currentMode = Mode.REAL; public static enum Mode { @@ -7,4 +9,8 @@ public static enum Mode { SIM, REPLAY } + public static class CurrentLimits { + public static int armKraken = 40; + //for now + } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index c84a0cb..5019561 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -10,10 +10,10 @@ public static class ArmIOInputs{ public double angularVelocity = 0.0; public double current = 0.0; } - public default void setVoltage(double voltage) { - } - public default void setVelocity(double velocity){ - - } + public default void setVoltage(double voltage) {} + + public default void setVelocity(double velocity) {} + + public default void updateInputs(ArmIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index f189900..fa918a7 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -11,33 +11,31 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.CurrentLimits; -import frc.robot.util.KrakenLogger; public class ArmIOReal implements ArmIO { - private final TalonFX armMotor = new TalonFX(ArmConstants.canID); + public final TalonFX armMotor = new TalonFX(ArmConstants.canID); - private TalonFXConfiguration talonnFXConfig = new TalonFXConfiguration(); + public TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - private final StatusSignal current = armMotor.getStatorCurrent(); - private final StatusSignal voltage = armMotor.getMotorVoltage(); - private final StatusSignal velocity = armMotor.getVelocity(); + public final StatusSignal current = armMotor.getStatorCurrent(); + public final StatusSignal voltage = armMotor.getMotorVoltage(); + public final StatusSignal velocity = armMotor.getVelocity(); public ArmIOReal(){ - TalonFXConfiguration talonFXConfig; - talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; - Object talonFX; - talonFX.Config.CurrentLimits.StatorCurrentLimit = CurrentLimits.armKraken; + talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; + talonFXConfig.CurrentLimits.StatorCurrentLimit = CurrentLimits.armKraken; talonFXConfig.Audio.BeepOnBoot = true; - talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.coast; + talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; armMotor.clearStickyFaults(); BaseStatusSignal.setUpdateFrequencyForAll (50,velocity, voltage, current ); - armMotor.optimizeBusUtilization(1.0); + //random number for optimizedfreqhz + armMotor.optimizeBusUtilization(9,1.0); StatusCode response = armMotor.getConfigurator().apply(talonFXConfig); if(!response.isOK()){ @@ -48,13 +46,14 @@ public ArmIOReal(){ + response.toString()); } } - + @Override public void updateInputs(ArmIOInputs inputs) { BaseStatusSignal.refreshAll(velocity, voltage, current); inputs.angularVelocity = velocity.getValueAsDouble()*60; inputs.current = current.getValueAsDouble(); inputs.voltage = voltage.getValueAsDouble(); + inputs.angularPosition = armMotor.getPosition().getValueAsDouble()*2*Math.PI; } @Override @@ -65,4 +64,6 @@ public void setVelocity(double velocity) { public void setVoltage(double voltage) { armMotor.set(-voltage); } + + } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 043f05a..de2c252 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -1,5 +1,42 @@ package frc.robot.subsystems.arm; -public class ArmIOSim { - +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +public class ArmIOSim implements ArmIO { + private SingleJointedArmSim armMotor; + private double armAppliedVolts = 0.0; + private ArmFeedforward ff = new ArmFeedforward(0.1, 0.1, 0.1); + + public ArmIOSim() { + armMotor = new SingleJointedArmSim( + null, + 0, + 0, + 0, + 0, + 0, + false, + 0, + null); + } + + @Override + public void updateInputs(ArmIOInputs inputs) { + inputs.angularPosition = armMotor.getAngleRads(); + inputs.angularVelocity = armMotor.getVelocityRadPerSec(); + inputs.current = armMotor.getCurrentDrawAmps(); + inputs.voltage = armAppliedVolts; + } + + @Override + public void setVoltage(double voltage) { + armMotor.setInputVoltage(voltage); + } + + @Override + public void setVelocity(double velocity) { + armMotor.setInputVoltage(ff.calculate(armMotor.getAngleRads(), velocity)); + } } + From 6247d15de7bb018b8fd7e6d9e23fd56ed44d182f Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Thu, 16 Jan 2025 16:51:17 -0500 Subject: [PATCH 018/110] Robot should drive --- src/main/java/frc/robot/Drive/Drive.java | 227 +++++++++++++++--- .../java/frc/robot/Drive/DriveConstants.java | 7 +- src/main/java/frc/robot/Drive/GyroIO.java | 2 +- src/main/java/frc/robot/Drive/Module.java | 29 +-- .../java/frc/robot/Drive/ModuleIOSim.java | 1 - .../java/frc/robot/Drive/ModuleIOTalonFX.java | 5 - src/main/java/frc/robot/Robot.java | 10 +- .../java/frc/robot/Util/LocalADStarAK.java | 149 ++++++++++++ 8 files changed, 370 insertions(+), 60 deletions(-) create mode 100644 src/main/java/frc/robot/Util/LocalADStarAK.java diff --git a/src/main/java/frc/robot/Drive/Drive.java b/src/main/java/frc/robot/Drive/Drive.java index 237ed30..615855c 100644 --- a/src/main/java/frc/robot/Drive/Drive.java +++ b/src/main/java/frc/robot/Drive/Drive.java @@ -7,6 +7,10 @@ import com.ctre.phoenix6.swerve.SwerveModule; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; @@ -36,6 +40,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.units.mutable.MutableMeasureBase; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.drive.DifferentialDrive.WheelSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -52,17 +57,16 @@ public class Drive extends SubsystemBase { private static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; - + private static final SwerveDriveKinematics K_DRIVE_KINEMATICS = new SwerveDriveKinematics(getModuleTranslation2d()); private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroIOInputs = new GyroIOInputsAutoLogged(); + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final Module[] modules = new Module[4]; - private final SysIdRoutine sysId; + private static SysIdRoutine sysId; private final MutableMeasure m_appliedVoltage = new MutVoltage(0, 0, Volt); private final MutableMeasure m_position = new MutAngle(0,0, Radian); private final MutableMeasure m_velocity = new MutAngularVelocity(0, 0, RadiansPerSecond); private static SwerveDriveKinematics Modulekinematics = new SwerveDriveKinematics(getModuleTranslation2d()); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslation2d()); private Rotation2d rawGyroRotation2d = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = @@ -99,23 +103,55 @@ public Drive( modules[2] = new Module(blModuleIO, 2); modules[3] = new Module(brModuleIO, 3); - - AutoBuilder.configureHolonomic( - this::getPose, - this::setPose, - () -> Kinematics.toChassisSpeeds(getModuleStates()), - this:: runVelocity, - new HolonomicPathFollowerConfig( - MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), - () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red, - this); - Pathfinding.setPathfinder(new LocalADStarAK()); + RobotConfig config; + try{ + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + } finally { + config = null; // if something went horribly wrong + } + + // Configure AutoBuilder last + AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } + + + + public void robotInit(){ + + + + + + //Pathfinding.setPathfinder(new LocalADStarAK()); // AK code needs to be added PathPlannerLogging.setLogActivePathCallback( (activePath) -> { Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])) + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); }); PathPlannerLogging.setLogTargetPoseCallback( (targetPose) -> { @@ -132,18 +168,18 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), }, log -> { log.motor("driveSparkMax") - .voltage(m_appliedVoltage.mustreplace(inputs.driveAppliedVolts, Volts)) - .angularPosition(m_position.must_replace(inputs.drivePositionRad, Radians)) - .AngularVelocity( - m_velocity.must_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond)); + .voltage(m_appliedVoltage.mut_replace(inputs.driveAppliedVolts, Volts)) + .angularPosition(m_position.mut_replace(inputs.drivePositionRad, Radians)) + .angularVelocity( + m_velocity.mut_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond)); }, this)); - } + } public void periodic() { - gyroIO.updateInputs(inputs); - Logger.processInputs("Drive/Gyro", gyroIOInputs); + gyroIO.updateInputs(gyroInputs);; + Logger.processInputs("Drive/Gyro", gyroInputs); for (var module : modules) { module.periodic(); } @@ -160,7 +196,7 @@ public void periodic() { Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } - SwerveModulePosition[] modulePositions = SwerveModulePosition(); + SwerveModulePosition[] modulePositions = getModulePositions(); SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex ++){ moduleDeltas [moduleIndex] = @@ -171,12 +207,12 @@ public void periodic() { lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } - if (gyro.connected) { + if (gyroInputs.connected) { rawGyroRotation2d = gyroInputs.yawPosition; } else { - Twist2d twist = Kinematics.toTwist2d(moduleDeltas); + Twist2d twist = K_DRIVE_KINEMATICS.toTwist2d(moduleDeltas, moduleDeltas); rawGyroRotation2d = rawGyroRotation2d.plus(new Rotation2d(twist.dtheta)); } @@ -188,7 +224,7 @@ public void periodic() { public void runVelocity(ChassisSpeeds speeds){ ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = Kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveModuleState[] setpointStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates,MAX_LINEAR_SPEED); SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; @@ -210,7 +246,7 @@ public void stop(){ public void stopWithx() { Rotation2d[] headings = new Rotation2d[4]; for (int i = 0; i < 4; i++) { - headings[i] = getModuleTranslations()[i].getAngle(); + headings[i] = getModuleTranslation2d()[i].getAngle(); } Modulekinematics.resetHeadings(); stop(); @@ -244,7 +280,23 @@ private SwerveModuleState[] getModuleStates(){ } return states; } + + + private SwerveModulePosition[] getModulePositions(){ + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++){ + states[i] = modules[i].getPosition(); + } + return states; + } + + + @AutoLogOutput(key = "odometry/Robot") + public Pose2d getPose(){ + return poseEstimator.getEstimatedPosition(); + } + public Rotation2d getRotation() { return getPose().getRotation(); } @@ -254,11 +306,15 @@ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation2d, getModulePositions(),pose); } + private ChassisSpeeds getRobotRelativeSpeeds(){ + return K_DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates()); + + } - - - - + public void driveRobotRelative(ChassisSpeeds speeds){ + SwerveModuleState[] moduleStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(speeds); + setModuleStates(moduleStates); + } public void addVisionMeasurement(Pose2d visionPose, double timestamp){ @@ -285,5 +341,108 @@ public static Translation2d[] getModuleTranslation2d(){ new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), }; } + + public static void setInstance(Drive instance) { + Drive.instance = instance; + } + + public static double getMaxLinearSpeed() { + return MAX_LINEAR_SPEED; + } + + public static double getTrackWidthX() { + return TRACK_WIDTH_X; + } + + public static double getTrackWidthY() { + return TRACK_WIDTH_Y; + } + + public static double getDriveBaseRadius() { + return DRIVE_BASE_RADIUS; + } + + public static double getMaxAngularSpeed() { + return MAX_ANGULAR_SPEED; + } + + public static SwerveDriveKinematics getkDriveKinematics() { + return K_DRIVE_KINEMATICS; + } + + public GyroIO getGyroIO() { + return gyroIO; + } + + public GyroIOInputsAutoLogged getGyroIOInputs() { + return gyroInputs; + } + + public ModuleIOInputsAutoLogged getInputs() { + return inputs; + } + + public Module[] getModules() { + return modules; + } + + public SysIdRoutine getSysId() { + return sysId; + } + + public MutableMeasure getM_appliedVoltage() { + return m_appliedVoltage; + } + + public MutableMeasure getM_position() { + return m_position; + } + + public MutableMeasure getM_velocity() { + return m_velocity; + } + + public static SwerveDriveKinematics getModulekinematics() { + return Modulekinematics; + } + + public static void setModulekinematics(SwerveDriveKinematics modulekinematics) { + Modulekinematics = modulekinematics; + } + + public SwerveDriveKinematics getKinematics() { + return kinematics; + } + + public void setKinematics(SwerveDriveKinematics kinematics) { + this.kinematics = kinematics; + } + + public Rotation2d getRawGyroRotation2d() { + return rawGyroRotation2d; + } + + public void setRawGyroRotation2d(Rotation2d rawGyroRotation2d) { + this.rawGyroRotation2d = rawGyroRotation2d; + } + + public SwerveModulePosition[] getLastModulePositions() { + return lastModulePositions; + } + + public void setLastModulePositions(SwerveModulePosition[] lastModulePositions) { + this.lastModulePositions = lastModulePositions; + } + + public SwerveDrivePoseEstimator getPoseEstimator() { + return poseEstimator; + } + + public void setPoseEstimator(SwerveDrivePoseEstimator poseEstimator) { + this.poseEstimator = poseEstimator; + } } + + + \ No newline at end of file diff --git a/src/main/java/frc/robot/Drive/DriveConstants.java b/src/main/java/frc/robot/Drive/DriveConstants.java index 41473d1..89fbba2 100644 --- a/src/main/java/frc/robot/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/Drive/DriveConstants.java @@ -1,5 +1,9 @@ package frc.robot.Drive; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.drive.DifferentialDrive.WheelSpeeds; + public class DriveConstants { public static final Mode currentMode = Mode.REAL; @@ -10,5 +14,6 @@ public static enum Mode { REPLAY, } - } + + diff --git a/src/main/java/frc/robot/Drive/GyroIO.java b/src/main/java/frc/robot/Drive/GyroIO.java index c50d242..549bf1f 100644 --- a/src/main/java/frc/robot/Drive/GyroIO.java +++ b/src/main/java/frc/robot/Drive/GyroIO.java @@ -8,7 +8,7 @@ public interface GyroIO { @AutoLog public static class GyroIOInputs{ - public boolean conected = false; + public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); public double yawVelocityRadPerSec = 0.0; } diff --git a/src/main/java/frc/robot/Drive/Module.java b/src/main/java/frc/robot/Drive/Module.java index 379460e..7daca6b 100644 --- a/src/main/java/frc/robot/Drive/Module.java +++ b/src/main/java/frc/robot/Drive/Module.java @@ -6,12 +6,8 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import org.littletonrobotics.junction.Logger; public class Module { @@ -32,7 +28,7 @@ public Module (ModuleIO io, int index) { this.io = io; this.index = index; - switch (DriveConstants.currentMode): + switch (DriveConstants.currentMode) { case REAL: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); @@ -53,11 +49,12 @@ public Module (ModuleIO io, int index) { driveFeddbackPidController = new PIDController(0.0, 0.0, 0.0); turnFeePidController = new PIDController(0.0, 0.0, 0.0); break; - } - - turnFeedback.enableContinuousInput(-Math.PI,Math.PI); - setBrakeMode(ture); + } + + turnFeePidController.enableContinuousInput(-Math.PI,Math.PI); + setBrakeMode(true); + } public void periodic() { io.updateInputs(inputs); Logger.processInputs("Drive/Module"+ Integer.toString(index), inputs); @@ -71,7 +68,7 @@ public void periodic() { if (angleSetpoint != null) { io.setTurnVoltage( - turnFeePidController.calculate(getAngle(), angleSetpoint.getRadians())); + turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); if (speedSetPoint != null) { @@ -79,7 +76,7 @@ public void periodic() { - double adjustSpeedSetpoint = adjustSpeedSetpoint*Math.cos(turnFeePidController.getPositionError()); + double adjustSpeedSetpoint = speedSetPoint*Math.cos(turnFeePidController.getPositionError()); double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; io.setDriveVoltage( @@ -90,13 +87,9 @@ public void periodic() { } - public SwerveModuleState runSetpoint(SwerveModulePosition state) { - + public SwerveModuleState runSetpoint(SwerveModuleState state) { var optimizedState = SwerveModuleState.optimize(state, getAngle()); - - - angleSetpoint = optimizedState.angle; speedSetPoint = optimizedState.speedMetersPerSecond; @@ -143,6 +136,10 @@ public double getVelocityRadPerSec(){ } + public double getPositionMeters() { + return inputs.drivePositionRad*WHEEL_RADIUS; + } + public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } diff --git a/src/main/java/frc/robot/Drive/ModuleIOSim.java b/src/main/java/frc/robot/Drive/ModuleIOSim.java index 688b953..5410f88 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/Drive/ModuleIOSim.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.Drive.ModuleIO.ModuleIOInputs; public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; diff --git a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java index 2c2cfbf..888230b 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java @@ -20,14 +20,9 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.REVLibError; -import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.AlternateEncoderConfig; -import com.revrobotics.spark.config.AlternateEncoderConfigAccessor; -import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.ExternalEncoderConfig; -import com.revrobotics.spark.config.ExternalEncoderConfigAccessor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.ResetMode; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0f8efc1..80bdef4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,10 +11,11 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import com.pathplanner.lib.pathfinding.Pathfinding; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Util.LocalADStarAK; public class Robot extends LoggedRobot { private Command m_autonomousCommand; @@ -50,6 +51,11 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); } + @Override + public void robotInit(){ + Pathfinding.setPathfinder(new LocalADStarAK()); + } + @Override public void disabledInit() {} diff --git a/src/main/java/frc/robot/Util/LocalADStarAK.java b/src/main/java/frc/robot/Util/LocalADStarAK.java new file mode 100644 index 0000000..3f4170b --- /dev/null +++ b/src/main/java/frc/robot/Util/LocalADStarAK.java @@ -0,0 +1,149 @@ +package frc.robot.Util; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.Pathfinder; +import com.pathplanner.lib.pathfinding.LocalADStar; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} \ No newline at end of file From ed10518682315cfcb6e89ff0442c07a9a91c42dd Mon Sep 17 00:00:00 2001 From: Arenacloserr <162641455+Arenacloserr@users.noreply.github.com> Date: Thu, 16 Jan 2025 17:35:51 -0500 Subject: [PATCH 019/110] setModuleStates --- src/main/java/frc/robot/Drive/Drive.java | 7 ++++++- src/main/java/frc/robot/Drive/GyroIOPigeon.java | 2 +- src/main/java/frc/robot/Drive/Module.java | 4 ++++ src/main/java/frc/robot/Drive/ModuleIO.java | 3 +++ 4 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Drive/Drive.java b/src/main/java/frc/robot/Drive/Drive.java index 615855c..4e8d547 100644 --- a/src/main/java/frc/robot/Drive/Drive.java +++ b/src/main/java/frc/robot/Drive/Drive.java @@ -290,7 +290,12 @@ private SwerveModulePosition[] getModulePositions(){ return states; } - + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, null); // DriveConstants.kMaxSpeedMetersPerSecond + for (int i = 0; i < 4; i++) { + modules[i].setDesiredState(desiredStates[i]); + } + } @AutoLogOutput(key = "odometry/Robot") public Pose2d getPose(){ diff --git a/src/main/java/frc/robot/Drive/GyroIOPigeon.java b/src/main/java/frc/robot/Drive/GyroIOPigeon.java index 80f357d..656c6bf 100644 --- a/src/main/java/frc/robot/Drive/GyroIOPigeon.java +++ b/src/main/java/frc/robot/Drive/GyroIOPigeon.java @@ -31,7 +31,7 @@ public void reset(){ @Override public void updateInputs(GyroIOInputs inputs) { - inputs.conected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); } diff --git a/src/main/java/frc/robot/Drive/Module.java b/src/main/java/frc/robot/Drive/Module.java index 7daca6b..687e4e8 100644 --- a/src/main/java/frc/robot/Drive/Module.java +++ b/src/main/java/frc/robot/Drive/Module.java @@ -153,4 +153,8 @@ public SwerveModuleState getState () { public double getCharacterizationVelocity() { return inputs.driveVelocityRadPerSec; } + + public void setDesiredState(SwerveModuleState desiredState) { + this.io.setDesiredState(desiredState); + } } diff --git a/src/main/java/frc/robot/Drive/ModuleIO.java b/src/main/java/frc/robot/Drive/ModuleIO.java index 535cbc2..b0ae5f6 100644 --- a/src/main/java/frc/robot/Drive/ModuleIO.java +++ b/src/main/java/frc/robot/Drive/ModuleIO.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.AutoLog; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; public interface ModuleIO { @@ -27,6 +28,8 @@ public default void setDriveVoltage(double volts) {} public default void runTalonPID (double desiredStateRotPerSecond) {} + public default void setDesiredState(SwerveModuleState state) {} + public default void setTurnVoltage (double volts) {} public default void setDriveBrakeMode(boolean enable) {} From 586df678507a408fca9468bc3ba33f1654923d4f Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Thu, 16 Jan 2025 18:17:52 -0500 Subject: [PATCH 020/110] elevator code update IOsim priority (similar to last years pivotIOsim), updateinput in IOsim, IOreal, IO, and elevator.java, IOreal configure needs to be changed from talon to sparkflex, and button binding in robotcontainer (needs to be deleted before merging) --- src/main/java/frc/robot/RobotContainer.java | 27 ++++++++++- .../elevator/ElevatorConstants.java | 2 + .../subsystems/elevator/ElevatorIOReal.java | 45 +++++++++++++++++-- 3 files changed, 69 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..fb5d651 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,13 +6,38 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorIO; +import frc.robot.subsystems.elevator.ElevatorIOReal; +import frc.robot.subsystems.elevator.ElevatorIOSim; public class RobotContainer { + + Elevator elevator; + public RobotContainer() { + switch (Constants.currentMode){ + case REAL: + elevator = Elevator.initialize(new ElevatorIOReal()); + break; + case SIM: + elevator = Elevator.initialize(new ElevatorIOSim()); + break; + case REPLAY: + elevator = Elevator.initialize(new ElevatorIOReal()); + break; + default: + } configureBindings(); } - private void configureBindings() {} + + private void configureBindings() { + CommandXboxController xboxController = new CommandXboxController(0); + xboxController.x().onTrue(Commands.run(() -> elevator.setMovingSpeedRPM(0, 0))); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index af0b88e..373fc73 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -6,5 +6,7 @@ public final class ElevatorConstants{ public static double kD; //figure this out later public static double kI; //figure this out later public static double kP; //figure this out later + public static double rightDeviceID = 1; + public static double leftDeviceID = 2; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index eb1b2f8..e924ba6 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -1,25 +1,62 @@ package frc.robot.subsystems.elevator; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.units.measure.AngularVelocity; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; + +import edu.wpi.first.units.measure.Current; -public class ElevatorIOReal implements ElevatorIO { - private TalonFX rightElevatorMotor = new TalonFX(0); - private TalonFX leftElevatorMotor = new TalonFX(1); +public class ElevatorIOReal implements ElevatorIO { + private SparkFlex rightElevatorMotor = new SparkFlex(1, MotorType.kBrushless); + private SparkFlex leftElevatorMotor = new SparkFlex(2, MotorType.kBrushless); + private double inputVolts = 0.0; final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); + // private final StatusSignal topRPM = leftElevatorMotor.getVelocity(); + // private final StatusSignal bottomRPM = rightElevatorMotor.getVelocity(); + // private final StatusSignal topCurrent = leftElevatorMotor.getTorqueCurrent(); + // private final StatusSignal bottomCurrent = rightElevatorMotor.getTorqueCurrent(); + + private boolean isEnabled; + private boolean hasPlayed = false; + + + private SparkFlexConfig config(double kV) { + var SparkFlexConfig = new SparkFlexConfig(); + + // SparkFlexConfig.Slot0.kP = ElevatorConstants.kP; + // SparkFlexConfig.Slot0.kI = 0; + // SparkFlexConfig.Slot0.kD = 0; + // SparkFlexConfig.Slot0.kS = 0; + // SparkFlexConfig.Slot0.kV = kV; + + // SparkFlexConfig.CurrentLimits.StatorCurrentLimitEnable = true; + // SparkFlexConfig.CurrentLimits.StatorCurrentLimit = CurrentLimits.shooter; + + // SparkFlexConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + + // SparkFlexConfig.Audio.BeepOnBoot = true; + + return SparkFlexConfig; + } public ElevatorIOReal(){ - leftElevatorMotor.setControl(new Follower(rightElevatorMotor.getDeviceID(), true)); + //leftElevatorMotor.setControl(new Follower(rightElevatorMotor.getDeviceID(), true)); } From d9b6f61b2f8ecbd3953773c945b9ba08a0901333 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Thu, 16 Jan 2025 22:39:36 -0500 Subject: [PATCH 021/110] Some Code Clean up --- simgui-ds.json | 6 ++++ src/main/java/frc/robot/Drive/Drive.java | 36 ++++++------------- .../java/frc/robot/Drive/DriveConstants.java | 9 +++-- src/main/java/frc/robot/Drive/Module.java | 2 -- .../java/frc/robot/Drive/ModuleIOSim.java | 4 +-- .../java/frc/robot/Drive/ModuleIOTalonFX.java | 4 +-- 6 files changed, 27 insertions(+), 34 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/java/frc/robot/Drive/Drive.java b/src/main/java/frc/robot/Drive/Drive.java index 4e8d547..9b9d331 100644 --- a/src/main/java/frc/robot/Drive/Drive.java +++ b/src/main/java/frc/robot/Drive/Drive.java @@ -5,13 +5,10 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.swerve.SwerveModule; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.pathfinding.LocalADStar; -import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -20,43 +17,32 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.Kinematics; 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.math.util.Units; import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.AngularVelocityUnit; -import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.VelocityUnit; import edu.wpi.first.units.VoltageUnit; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.units.measure.MutAngularVelocity; import edu.wpi.first.units.measure.MutVoltage; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.units.mutable.MutableMeasureBase; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.drive.DifferentialDrive.WheelSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Drive.DriveConstants.Mode; -import frc.robot.Drive.GyroIO.GyroIOInputs; -import frc.robot.Drive.ModuleIO.ModuleIOInputs; public class Drive extends SubsystemBase { private static Drive instance; - private static final double MAX_LINEAR_SPEED = Units.feetToMeters(5); - private static final double TRACK_WIDTH_X = Units.inchesToMeters(21.25); - private static final double TRACK_WIDTH_Y = Units.inchesToMeters(21.25); private static final double DRIVE_BASE_RADIUS = - Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); - private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + Math.hypot(DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0); + private static final double MAX_ANGULAR_SPEED = DriveConstants.MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; private static final SwerveDriveKinematics K_DRIVE_KINEMATICS = new SwerveDriveKinematics(getModuleTranslation2d()); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); @@ -225,7 +211,7 @@ public void runVelocity(ChassisSpeeds speeds){ ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates,MAX_LINEAR_SPEED); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates,DriveConstants.MAX_LINEAR_SPEED); SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { @@ -329,7 +315,7 @@ public void addVisionMeasurement(Pose2d visionPose, double timestamp){ public double getMaxLinearSpeedMeterPerSec(){ - return MAX_LINEAR_SPEED; + return DriveConstants.MAX_LINEAR_SPEED; } @@ -340,10 +326,10 @@ public double getMaxAngluarSpeeddRadPerSec(){ public static Translation2d[] getModuleTranslation2d(){ return new Translation2d[]{ - new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), - new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), - new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), - new Translation2d(TRACK_WIDTH_X/2.0, TRACK_WIDTH_Y/2.0), + new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), + new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), + new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), + new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), }; } @@ -352,15 +338,15 @@ public static void setInstance(Drive instance) { } public static double getMaxLinearSpeed() { - return MAX_LINEAR_SPEED; + return DriveConstants.MAX_LINEAR_SPEED; } public static double getTrackWidthX() { - return TRACK_WIDTH_X; + return DriveConstants.TRACK_LENGTH; } public static double getTrackWidthY() { - return TRACK_WIDTH_Y; + return DriveConstants.TRACK_WIDTH; } public static double getDriveBaseRadius() { diff --git a/src/main/java/frc/robot/Drive/DriveConstants.java b/src/main/java/frc/robot/Drive/DriveConstants.java index 89fbba2..127dd79 100644 --- a/src/main/java/frc/robot/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/Drive/DriveConstants.java @@ -1,8 +1,6 @@ package frc.robot.Drive; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.wpilibj.drive.DifferentialDrive.WheelSpeeds; +import edu.wpi.first.math.util.Units; public class DriveConstants { public static final Mode currentMode = Mode.REAL; @@ -14,6 +12,11 @@ public static enum Mode { REPLAY, } + + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(5); + public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); + public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); + } diff --git a/src/main/java/frc/robot/Drive/Module.java b/src/main/java/frc/robot/Drive/Module.java index 687e4e8..c954674 100644 --- a/src/main/java/frc/robot/Drive/Module.java +++ b/src/main/java/frc/robot/Drive/Module.java @@ -1,7 +1,5 @@ package frc.robot.Drive; -import com.pathplanner.lib.util.DriveFeedforwards; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/Drive/ModuleIOSim.java b/src/main/java/frc/robot/Drive/ModuleIOSim.java index 5410f88..09484f4 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/Drive/ModuleIOSim.java @@ -8,8 +8,8 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; //change gearbox argument - private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 6.75, 0.025), DCMotor.getNeoVortex(4)); - private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 6.75, 0.025), DCMotor.getKrakenX60(4)); + private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 6.75, 0.025), DCMotor.getNeoVortex(4)); //needs to change + private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 6.75, 0.025), DCMotor.getKrakenX60(4)); //needs to change private final Rotation2d turnAbsoluteInitPositon = new Rotation2d(Math.random()*2.0*Math.PI); private double driveAppliedVolts = 0.0; diff --git a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java index 888230b..4ca07d1 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java @@ -46,8 +46,8 @@ public class ModuleIOTalonFX implements ModuleIO { final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = 5.46; - private static final double TURN_GEAR_RATIO = 11.3142; + private static final double DRIVE_GEAR_RATIO = 5.46; //needs to change + private static final double TURN_GEAR_RATIO = 11.3142; // needs to change private final TalonFX driveTalonFX; private final SparkFlex turnSparkMax; From 098e59b4a15075735252dbe0fdf28fd8c8bc5744 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Fri, 17 Jan 2025 11:46:58 -0500 Subject: [PATCH 022/110] code clean up --- src/main/java/frc/robot/Drive/Drive.java | 14 +++++--------- src/main/java/frc/robot/Drive/DriveConstants.java | 11 +++++++++++ src/main/java/frc/robot/Drive/Module.java | 9 ++++----- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Drive/Drive.java b/src/main/java/frc/robot/Drive/Drive.java index 9b9d331..96d7fee 100644 --- a/src/main/java/frc/robot/Drive/Drive.java +++ b/src/main/java/frc/robot/Drive/Drive.java @@ -20,7 +20,6 @@ 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.math.util.Units; import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.MutableMeasure; @@ -40,9 +39,6 @@ public class Drive extends SubsystemBase { private static Drive instance; - private static final double DRIVE_BASE_RADIUS = - Math.hypot(DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0); - private static final double MAX_ANGULAR_SPEED = DriveConstants.MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; private static final SwerveDriveKinematics K_DRIVE_KINEMATICS = new SwerveDriveKinematics(getModuleTranslation2d()); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); @@ -106,8 +102,8 @@ public Drive( this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + new PIDConstants(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD), // Translation PID constants + new PIDConstants(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD) // Rotation PID constants ), config, // The robot configuration () -> { @@ -320,7 +316,7 @@ public double getMaxLinearSpeedMeterPerSec(){ public double getMaxAngluarSpeeddRadPerSec(){ - return MAX_ANGULAR_SPEED; + return DriveConstants.MAX_ANGULAR_SPEED; } @@ -350,11 +346,11 @@ public static double getTrackWidthY() { } public static double getDriveBaseRadius() { - return DRIVE_BASE_RADIUS; + return DriveConstants.DRIVE_BASE_RADIUS; } public static double getMaxAngularSpeed() { - return MAX_ANGULAR_SPEED; + return DriveConstants.MAX_ANGULAR_SPEED; } public static SwerveDriveKinematics getkDriveKinematics() { diff --git a/src/main/java/frc/robot/Drive/DriveConstants.java b/src/main/java/frc/robot/Drive/DriveConstants.java index 127dd79..5245305 100644 --- a/src/main/java/frc/robot/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/Drive/DriveConstants.java @@ -16,6 +16,17 @@ public static enum Mode { public static final double MAX_LINEAR_SPEED = Units.feetToMeters(5); public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); + public static final double DRIVE_BASE_RADIUS = + Math.hypot(DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0); + public static final double MAX_ANGULAR_SPEED = DriveConstants.MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + public static final double TranslationKP = 5.0; + public static final double TranslationKI = 0.0; + public static final double TranslationKD = 0.0; + public static final double RotationKP = 5.0; + public static final double RotationKI = 0.0; + public static final double RotationKD = 0.0; + public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + } diff --git a/src/main/java/frc/robot/Drive/Module.java b/src/main/java/frc/robot/Drive/Module.java index c954674..45848ca 100644 --- a/src/main/java/frc/robot/Drive/Module.java +++ b/src/main/java/frc/robot/Drive/Module.java @@ -3,13 +3,12 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import org.littletonrobotics.junction.Logger; public class Module { - private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); @@ -76,7 +75,7 @@ public void periodic() { double adjustSpeedSetpoint = speedSetPoint*Math.cos(turnFeePidController.getPositionError()); - double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS; io.setDriveVoltage( drivFeedforward.calculate(velocityRadPerSec) + driveFeddbackPidController.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); @@ -130,12 +129,12 @@ public Rotation2d getAngle (){ public double getVelocityRadPerSec(){ - return inputs.driveVelocityRadPerSec*WHEEL_RADIUS; + return inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS; } public double getPositionMeters() { - return inputs.drivePositionRad*WHEEL_RADIUS; + return inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS; } public SwerveModulePosition getPosition() { From 0ff9e865d202280db20d60576c2b5ea0cd5bb3df Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Fri, 17 Jan 2025 17:19:05 -0500 Subject: [PATCH 023/110] Code clean up --- .../java/frc/robot/Drive/DriveConstants.java | 3 +++ .../java/frc/robot/Drive/ModuleIOSim.java | 4 ++-- ...IOSparkMax.Java => ModuleIOSparkFlex.Java} | 15 +++++-------- .../java/frc/robot/Drive/ModuleIOTalonFX.java | 22 +++++++++---------- 4 files changed, 22 insertions(+), 22 deletions(-) rename src/main/java/frc/robot/Drive/{ModuleIOSparkMax.Java => ModuleIOSparkFlex.Java} (93%) diff --git a/src/main/java/frc/robot/Drive/DriveConstants.java b/src/main/java/frc/robot/Drive/DriveConstants.java index 5245305..fd2cf8c 100644 --- a/src/main/java/frc/robot/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/Drive/DriveConstants.java @@ -26,6 +26,9 @@ public static enum Mode { public static final double RotationKI = 0.0; public static final double RotationKD = 0.0; public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + public static final double DriveGearing = 13.371; + public static final double TurnGearing = 6.75; + public static final int turnCurrentLimit = 60; //May need to change later } diff --git a/src/main/java/frc/robot/Drive/ModuleIOSim.java b/src/main/java/frc/robot/Drive/ModuleIOSim.java index 09484f4..12f7273 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/Drive/ModuleIOSim.java @@ -8,8 +8,8 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; //change gearbox argument - private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 6.75, 0.025), DCMotor.getNeoVortex(4)); //needs to change - private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 6.75, 0.025), DCMotor.getKrakenX60(4)); //needs to change + private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 0.025, DriveConstants.DriveGearing), DCMotor.getNeoVortex(4)); //needs to change + private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 0.004, DriveConstants.TurnGearing), DCMotor.getKrakenX60(4)); //needs to change private final Rotation2d turnAbsoluteInitPositon = new Rotation2d(Math.random()*2.0*Math.PI); private double driveAppliedVolts = 0.0; diff --git a/src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java b/src/main/java/frc/robot/Drive/ModuleIOSparkFlex.Java similarity index 93% rename from src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java rename to src/main/java/frc/robot/Drive/ModuleIOSparkFlex.Java index 30be722..7babc58 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOSparkMax.Java +++ b/src/main/java/frc/robot/Drive/ModuleIOSparkFlex.Java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.drive; +package frc.robot.subsystems.Drive; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase.IdleMode; @@ -35,11 +35,8 @@ import edu.wpi.first.math.util.Units; * "/Drive/ModuleX/TurnAbsolutePositionRad" */ public class ModuleIOSparkMax implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double TURN_GEAR_RATIO = 150.0 / 7.0; - private final CANSparkMax driveSparkMax; + ; private final CANSparkFlex turnSparkMax; private final RelativeEncoder driveEncoder; @@ -115,18 +112,18 @@ public class ModuleIOSparkMax implements ModuleIO { @Override public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(driveEncoder.getPosition()) / DriveConstants.DriveGearing; inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DriveConstants.DriveGearing; inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition()); inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / DriveConstants.TurnGearing); inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; + / DriveConstants.TurnGearing; inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; } diff --git a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java index 4ca07d1..60bf2d9 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java @@ -23,7 +23,7 @@ import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.ExternalEncoderConfig; -import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -46,8 +46,8 @@ public class ModuleIOTalonFX implements ModuleIO { final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = 5.46; //needs to change - private static final double TURN_GEAR_RATIO = 11.3142; // needs to change + + private final TalonFX driveTalonFX; private final SparkFlex turnSparkMax; @@ -146,28 +146,28 @@ public ModuleIOTalonFX(int index) { @Override public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = - Units.rotationsToRadians(driveTalonFX.getPosition().getValueAsDouble()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(driveTalonFX.getPosition().getValueAsDouble()) / DriveConstants.DriveGearing; inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble()) - / DRIVE_GEAR_RATIO; + / DriveConstants.DriveGearing; inputs.driveAppliedVolts = driveTalonFX.getMotorVoltage().getValueAsDouble(); inputs.driveCurrentAmps = new double[] {driveTalonFX.getSupplyCurrent().getValueAsDouble()}; inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition()); inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / DriveConstants.TurnGearing); inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; + / DriveConstants.TurnGearing; inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; } - private SparkMaxConfig configSpark(IdleMode idle) { - var turningConfig = new SparkMaxConfig(); + private SparkFlexConfig configSpark(IdleMode idle) { + var turningConfig = new SparkFlexConfig(); - turningConfig.smartCurrentLimit(30); - turningConfig.voltageCompensation(12.0); + turningConfig.smartCurrentLimit(DriveConstants.turnCurrentLimit); + // turningConfig.voltageCompensation(12.0); if (idle != null) { turningConfig.idleMode(idle); From ca2a29e492cd9c6e591e7f9bd7a56dd35f373d97 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Fri, 17 Jan 2025 22:54:31 -0500 Subject: [PATCH 024/110] Button Binding in Process --- .../java/frc/robot/Commands/DriveCommands | 23 ++++++++ src/main/java/frc/robot/Constants.java | 7 +++ src/main/java/frc/robot/RobotContainer.java | 57 ++++++++++++++++++- .../{ => subsystems/Drive}/Drive/Drive.java | 6 +- .../Drive}/Drive/DriveConstants.java | 2 +- .../{ => subsystems/Drive}/Drive/GyroIO.java | 2 +- .../Drive}/Drive/GyroIOPigeon.java | 2 +- .../{ => subsystems/Drive}/Drive/Module.java | 4 +- .../Drive}/Drive/ModuleIO.java | 2 +- .../Drive}/Drive/ModuleIOSim.java | 2 +- .../Drive}/Drive/ModuleIOSparkFlex.Java | 3 +- .../Drive}/Drive/ModuleIOTalonFX.java | 3 +- 12 files changed, 100 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc/robot/Commands/DriveCommands rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/Drive.java (98%) rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/DriveConstants.java (96%) rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/GyroIO.java (90%) rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/GyroIOPigeon.java (96%) rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/Module.java (98%) rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/ModuleIO.java (96%) rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/ModuleIOSim.java (96%) rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/ModuleIOSparkFlex.Java (99%) rename src/main/java/frc/robot/{ => subsystems/Drive}/Drive/ModuleIOTalonFX.java (99%) diff --git a/src/main/java/frc/robot/Commands/DriveCommands b/src/main/java/frc/robot/Commands/DriveCommands new file mode 100644 index 0000000..dfe66df --- /dev/null +++ b/src/main/java/frc/robot/Commands/DriveCommands @@ -0,0 +1,23 @@ + + + + + + +public class DriveCommands{ + + private static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + ()-> { + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), Constants.OIConstants.KAxisDeadband); + ) + } + } + } + diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eb7d9c5..c992ea1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class Constants { public static Mode currentMode = Mode.REAL; public static enum Mode { @@ -7,4 +8,10 @@ public static enum Mode { SIM, REPLAY } + + public static final class OIConstants{ + public static final double KAxisDeadband = 0.1; + public static final CommandXboxController driverController = new CommandXboxController(0); + public static final CommandXboxController operatorController = new CommandXboxController(1); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..b51f3b3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,13 +6,66 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.Drive.Drive.Drive; +import frc.robot.subsystems.Drive.Drive.GyroIO; +import frc.robot.subsystems.Drive.Drive.GyroIOPigeon; +import frc.robot.subsystems.Drive.Drive.ModuleIO; +import frc.robot.subsystems.Drive.Drive.ModuleIOSim; +import frc.robot.subsystems.Drive.Drive.ModuleIOTalonFX; public class RobotContainer { - public RobotContainer() { + + private final Drive drive; + + public RobotContainer() { + + switch (Constants.currentMode) { + case REAL: + + drive = + new Drive( + new GyroIOPigeon(), + new ModuleIOTalonFX(0), + new ModuleIOTalonFX(1), + new ModuleIOTalonFX(2), + new ModuleIOTalonFX(3)); + break; + + case SIM: + + // drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + + default: + + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO(){}, + new ModuleIO() {}, + new ModuleIO() {}); + break; + } + + configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/Drive/Drive.java b/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java similarity index 98% rename from src/main/java/frc/robot/Drive/Drive.java rename to src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java index 96d7fee..9a74ea2 100644 --- a/src/main/java/frc/robot/Drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java @@ -1,4 +1,4 @@ -package frc.robot.Drive; +package frc.robot.subsystems.Drive.Drive; import static edu.wpi.first.units.Units.*; @@ -34,7 +34,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Drive.DriveConstants.Mode; +import frc.robot.Drive.GyroIOInputsAutoLogged; +import frc.robot.Drive.ModuleIOInputsAutoLogged; +import frc.robot.subsystems.Drive.Drive.DriveConstants.Mode; public class Drive extends SubsystemBase { diff --git a/src/main/java/frc/robot/Drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java similarity index 96% rename from src/main/java/frc/robot/Drive/DriveConstants.java rename to src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java index fd2cf8c..603a6c1 100644 --- a/src/main/java/frc/robot/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java @@ -1,4 +1,4 @@ -package frc.robot.Drive; +package frc.robot.subsystems.Drive.Drive; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/Drive/GyroIO.java b/src/main/java/frc/robot/subsystems/Drive/Drive/GyroIO.java similarity index 90% rename from src/main/java/frc/robot/Drive/GyroIO.java rename to src/main/java/frc/robot/subsystems/Drive/Drive/GyroIO.java index 549bf1f..9ea91ef 100644 --- a/src/main/java/frc/robot/Drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/GyroIO.java @@ -1,4 +1,4 @@ -package frc.robot.Drive; +package frc.robot.subsystems.Drive.Drive; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/Drive/GyroIOPigeon.java b/src/main/java/frc/robot/subsystems/Drive/Drive/GyroIOPigeon.java similarity index 96% rename from src/main/java/frc/robot/Drive/GyroIOPigeon.java rename to src/main/java/frc/robot/subsystems/Drive/Drive/GyroIOPigeon.java index 656c6bf..26d8305 100644 --- a/src/main/java/frc/robot/Drive/GyroIOPigeon.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/GyroIOPigeon.java @@ -1,4 +1,4 @@ -package frc.robot.Drive; +package frc.robot.subsystems.Drive.Drive; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; diff --git a/src/main/java/frc/robot/Drive/Module.java b/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java similarity index 98% rename from src/main/java/frc/robot/Drive/Module.java rename to src/main/java/frc/robot/subsystems/Drive/Drive/Module.java index 45848ca..ee06584 100644 --- a/src/main/java/frc/robot/Drive/Module.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java @@ -1,10 +1,12 @@ -package frc.robot.Drive; +package frc.robot.subsystems.Drive.Drive; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.Drive.ModuleIOInputsAutoLogged; + import org.littletonrobotics.junction.Logger; public class Module { diff --git a/src/main/java/frc/robot/Drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIO.java similarity index 96% rename from src/main/java/frc/robot/Drive/ModuleIO.java rename to src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIO.java index b0ae5f6..4a86711 100644 --- a/src/main/java/frc/robot/Drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIO.java @@ -1,4 +1,4 @@ -package frc.robot.Drive; +package frc.robot.subsystems.Drive.Drive; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/Drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java similarity index 96% rename from src/main/java/frc/robot/Drive/ModuleIOSim.java rename to src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java index 12f7273..a985c9e 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java @@ -1,4 +1,4 @@ -package frc.robot.Drive; +package frc.robot.subsystems.Drive.Drive; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; diff --git a/src/main/java/frc/robot/Drive/ModuleIOSparkFlex.Java b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSparkFlex.Java similarity index 99% rename from src/main/java/frc/robot/Drive/ModuleIOSparkFlex.Java rename to src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSparkFlex.Java index 7babc58..2468259 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOSparkFlex.Java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSparkFlex.Java @@ -11,7 +11,8 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.Drive; +package frc.robot.subsystems.Drive.Drive; + import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase.IdleMode; diff --git a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOTalonFX.java similarity index 99% rename from src/main/java/frc/robot/Drive/ModuleIOTalonFX.java rename to src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOTalonFX.java index 60bf2d9..4bd33df 100644 --- a/src/main/java/frc/robot/Drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOTalonFX.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.Drive; +package frc.robot.subsystems.Drive.Drive; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -45,7 +45,6 @@ */ public class ModuleIOTalonFX implements ModuleIO { final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); - // Gear ratios for SDS MK4i L2, adjust as necessary From 330c789114eeb942c77ac4d564e41af3cd229274 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Fri, 17 Jan 2025 23:23:56 -0500 Subject: [PATCH 025/110] Buttons Configured --- .../java/frc/robot/Commands/DriveCommands | 23 ------- .../frc/robot/Commands/DriveCommands.java | 65 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 21 +++--- .../robot/subsystems/Drive/Drive/Drive.java | 6 +- .../robot/subsystems/Drive/Drive/Module.java | 2 - 5 files changed, 80 insertions(+), 37 deletions(-) delete mode 100644 src/main/java/frc/robot/Commands/DriveCommands create mode 100644 src/main/java/frc/robot/Commands/DriveCommands.java diff --git a/src/main/java/frc/robot/Commands/DriveCommands b/src/main/java/frc/robot/Commands/DriveCommands deleted file mode 100644 index dfe66df..0000000 --- a/src/main/java/frc/robot/Commands/DriveCommands +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - -public class DriveCommands{ - - private static Command joystickDrive( - Drive drive, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { - return Commands.run( - ()-> { - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), Constants.OIConstants.KAxisDeadband); - ) - } - } - } - diff --git a/src/main/java/frc/robot/Commands/DriveCommands.java b/src/main/java/frc/robot/Commands/DriveCommands.java new file mode 100644 index 0000000..2fad06b --- /dev/null +++ b/src/main/java/frc/robot/Commands/DriveCommands.java @@ -0,0 +1,65 @@ +package frc.robot.Commands; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants; +import frc.robot.subsystems.Drive.Drive.Drive; + +public class DriveCommands { + private DriveCommands() {} + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), Constants.OIConstants.KAxisDeadband); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), Constants.OIConstants.KAxisDeadband); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec(), + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } +} + diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b51f3b3..e0541b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Commands.DriveCommands; import frc.robot.subsystems.Drive.Drive.Drive; import frc.robot.subsystems.Drive.Drive.GyroIO; import frc.robot.subsystems.Drive.Drive.GyroIOPigeon; @@ -16,7 +16,7 @@ public class RobotContainer { - private final Drive drive; + private Drive drive; public RobotContainer() { @@ -34,7 +34,7 @@ public RobotContainer() { case SIM: - // drive = + drive = new Drive( new GyroIO() {}, new ModuleIOSim(), @@ -44,7 +44,7 @@ public RobotContainer() { default: - drive = + drive = new Drive( new GyroIO() {}, new ModuleIO() {}, @@ -62,10 +62,15 @@ private void configureBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - } + () -> -Constants.OIConstants.driverController.getLeftY(), + () -> -Constants.OIConstants.driverController.getLeftX(), + () -> -Constants.OIConstants.driverController.getRightX())); + + Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(()-> drive.zeroHeading(), drive)); + + } + + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java b/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java index 9a74ea2..5680a20 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java @@ -34,8 +34,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Drive.GyroIOInputsAutoLogged; -import frc.robot.Drive.ModuleIOInputsAutoLogged; import frc.robot.subsystems.Drive.Drive.DriveConstants.Mode; @@ -312,12 +310,12 @@ public void addVisionMeasurement(Pose2d visionPose, double timestamp){ - public double getMaxLinearSpeedMeterPerSec(){ + public double getMaxLinearSpeedMetersPerSec(){ return DriveConstants.MAX_LINEAR_SPEED; } - public double getMaxAngluarSpeeddRadPerSec(){ + public double getMaxAngularSpeedRadPerSec(){ return DriveConstants.MAX_ANGULAR_SPEED; } diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java b/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java index ee06584..675e471 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java @@ -5,8 +5,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.Drive.ModuleIOInputsAutoLogged; - import org.littletonrobotics.junction.Logger; public class Module { From e7e8b3b345c61ffa31675f93a6ffe393e5f280ff Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Fri, 17 Jan 2025 23:48:41 -0500 Subject: [PATCH 026/110] updated sim --- .../subsystems/Drive/Drive/ModuleIOSim.java | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java index a985c9e..6a01434 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.Drive.Drive; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -19,5 +20,29 @@ public class ModuleIOSim implements ModuleIO { public void updateInputs(ModuleIOInputs inputs){ driveSim.update(LOOP_PERIOD_SECS); turnSim.update(LOOP_PERIOD_SECS); + + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularPositionRad(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + + + inputs.turnAbsolutePosition = + new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPositon); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPosition()); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + } + + @Override + public void setDriveVoltage(double volts){ + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(driveAppliedVolts); + } + + @Override + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + turnSim.setInputVoltage(turnAppliedVolts); } } From fa42a35e56335fd4b848d756379974a5db5f6dc2 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sat, 18 Jan 2025 12:56:43 -0500 Subject: [PATCH 027/110] Arm sim + preliminary talon code --- simgui-ds.json | 5 ++ src/main/java/frc/robot/Constants.java | 10 +-- src/main/java/frc/robot/RobotContainer.java | 24 ++++++- .../java/frc/robot/subsystems/arm/Arm.java | 45 ++++++++++++- .../robot/subsystems/arm/ArmConstants.java | 13 +++- .../java/frc/robot/subsystems/arm/ArmIO.java | 20 ++++-- .../frc/robot/subsystems/arm/ArmIOReal.java | 39 ++++++----- .../frc/robot/subsystems/arm/ArmIOSim.java | 66 ++++++++++++------- .../robot/subsystems/arm/CurrentLimits.java | 5 -- 9 files changed, 163 insertions(+), 64 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/arm/CurrentLimits.java diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..f84381f 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "03000000c82d00000631000014010000" + } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 57f0baa..96b7af7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,16 +1,16 @@ package frc.robot; -import frc.robot.subsystems.arm.CurrentLimits; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class Constants { - public static Mode currentMode = Mode.REAL; + public static Mode currentMode = Mode.SIM; public static enum Mode { REAL, SIM, REPLAY } - public static class CurrentLimits { - public static int armKraken = 40; - //for now + + public static class OIConstants { + public static final CommandXboxController driverController = new CommandXboxController(0); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..7f60974 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,13 +6,33 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.Constants.OIConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.ArmIOReal; +import frc.robot.subsystems.arm.ArmIOSim; -public class RobotContainer { +public class RobotContainer { public RobotContainer() { + switch (Constants.currentMode) { + case REAL: + Arm.initialize(new ArmIOReal()); + break; + case SIM: + Arm.initialize(new ArmIOSim()); + break; + default: + Arm.initialize(new ArmIOReal()); + break; + + } configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + OIConstants.driverController.povUp().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(Math.PI / 2))); + OIConstants.driverController.povDown().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(0))); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 49857b2..51c4c6d 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -1,5 +1,46 @@ package frc.robot.subsystems.arm; -public class Arm { - +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.arm.ArmIO.ArmIOInputs; + +public class Arm extends SubsystemBase { + private ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + private boolean openLoop = true; + private ArmIO io; + private static Arm instance; + + private Arm(ArmIO io) { + this.io = io; + } + + public static void initialize(ArmIO io) { + instance = new Arm(io); + } + + public static Arm getInstance() { + return instance; + } + + public void setGoal(double angle) { + openLoop = false; + io.setGoal(angle); + } + + public void setOpenLoop(boolean openLoop) { + this.openLoop = openLoop; + } + + @Override + public void periodic() { + if(!openLoop) { + io.updateLoop(); + } + + this.inputs.openLoop = openLoop; + + io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); + } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index a79523d..aa15f26 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -1,5 +1,16 @@ package frc.robot.subsystems.arm; public class ArmConstants { - public static int canID = 10; + public static int CAN_ID = 10; + public static double CURRENT_LIMIT = 40; + + public static class Sim { + public static double GEARING = 6; + public static double MOI = 2; + public static double LENGTH = 0.5; + public static double MIN_ANGLE = -Math.PI / 6; + public static double MAX_ANGLE = 2 * Math.PI / 3; + public static boolean GRAVITY = false; + public static double INIT_ANGLE = -Math.PI / 6; + } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 5019561..e5bd0bf 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -9,11 +9,23 @@ public static class ArmIOInputs{ public double angularPosition = 0.0; public double angularVelocity = 0.0; public double current = 0.0; + public double goal = 0.0; + public boolean openLoop = false; } - public default void setVoltage(double voltage) {} - - public default void setVelocity(double velocity) {} + public default void updateInputs(ArmIOInputs inputs) { + + } + public default void setVoltage(double voltage) { + + } + public default void setVelocity(double velocity){ + + } + public default void setGoal(double angle) { - public default void updateInputs(ArmIOInputs inputs) {} + } + public default void updateLoop() { + + } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index fa918a7..ca1a319 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -7,37 +7,34 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CurrentLimits; public class ArmIOReal implements ArmIO { - public final TalonFX armMotor = new TalonFX(ArmConstants.canID); - - public TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - - - public final StatusSignal current = armMotor.getStatorCurrent(); - public final StatusSignal voltage = armMotor.getMotorVoltage(); - public final StatusSignal velocity = armMotor.getVelocity(); + private final TalonFX armMotor = new TalonFX(ArmConstants.CAN_ID); + private TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); + private final StatusSignal current = armMotor.getStatorCurrent(); + private final StatusSignal voltage = armMotor.getMotorVoltage(); + private final StatusSignal velocity = armMotor.getVelocity(); + private final StatusSignal position = armMotor.getPosition(); + private final ArmFeedforward ffmodel = new ArmFeedforward(0.1, 0.1, 0.1); public ArmIOReal(){ talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfig.CurrentLimits.StatorCurrentLimit = CurrentLimits.armKraken; - + talonFXConfig.CurrentLimits.StatorCurrentLimit = ArmConstants.CURRENT_LIMIT; talonFXConfig.Audio.BeepOnBoot = true; - - talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - + talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; armMotor.clearStickyFaults(); - BaseStatusSignal.setUpdateFrequencyForAll (50,velocity, voltage, current ); + BaseStatusSignal.setUpdateFrequencyForAll(50,velocity, voltage, current); - //random number for optimizedfreqhz - armMotor.optimizeBusUtilization(9,1.0); + armMotor.optimizeBusUtilization(50, 1); StatusCode response = armMotor.getConfigurator().apply(talonFXConfig); + if(!response.isOK()){ System.out.println( "Talon ID" @@ -46,11 +43,13 @@ public ArmIOReal(){ + response.toString()); } } + @Override public void updateInputs(ArmIOInputs inputs) { BaseStatusSignal.refreshAll(velocity, voltage, current); - inputs.angularVelocity = velocity.getValueAsDouble()*60; + inputs.angularPosition = position.getValueAsDouble() * 2 * Math.PI; + inputs.angularVelocity = velocity.getValueAsDouble() * 2 * Math.PI; inputs.current = current.getValueAsDouble(); inputs.voltage = voltage.getValueAsDouble(); inputs.angularPosition = armMotor.getPosition().getValueAsDouble()*2*Math.PI; @@ -58,11 +57,11 @@ public void updateInputs(ArmIOInputs inputs) { @Override public void setVelocity(double velocity) { - armMotor.set(-velocity); + setVoltage(ffmodel.calculate(position.getValueAsDouble() * 2 * Math.PI, velocity)); } public void setVoltage(double voltage) { - armMotor.set(-voltage); + armMotor.setVoltage(-voltage); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index de2c252..780ed5a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -1,42 +1,58 @@ package frc.robot.subsystems.arm; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.AngularMomentum; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.subsystems.arm.ArmIO.ArmIOInputs; public class ArmIOSim implements ArmIO { - private SingleJointedArmSim armMotor; - private double armAppliedVolts = 0.0; - private ArmFeedforward ff = new ArmFeedforward(0.1, 0.1, 0.1); - - public ArmIOSim() { - armMotor = new SingleJointedArmSim( - null, - 0, - 0, - 0, - 0, - 0, - false, - 0, - null); + private final SingleJointedArmSim armSim = new SingleJointedArmSim( + DCMotor.getKrakenX60(2), + ArmConstants.Sim.GEARING, + ArmConstants.Sim.MOI, + ArmConstants.Sim.LENGTH, + ArmConstants.Sim.MIN_ANGLE, + ArmConstants.Sim.MAX_ANGLE, + ArmConstants.Sim.GRAVITY, + ArmConstants.Sim.INIT_ANGLE + ); + private double voltage = 0; + private final ArmFeedforward ffmodel = new ArmFeedforward(0.1, 0.1, 0.1); + private final PIDController controller = new PIDController(0.01, 0.02, 0); + private double goal = ArmConstants.Sim.INIT_ANGLE; + + @Override + public void setVoltage(double voltage) { + this.voltage = voltage; + armSim.setInputVoltage(voltage); + } + + @Override + public void setVelocity(double velocity) { + setVoltage(ffmodel.calculate(armSim.getAngleRads(), velocity)); } @Override - public void updateInputs(ArmIOInputs inputs) { - inputs.angularPosition = armMotor.getAngleRads(); - inputs.angularVelocity = armMotor.getVelocityRadPerSec(); - inputs.current = armMotor.getCurrentDrawAmps(); - inputs.voltage = armAppliedVolts; + public void setGoal(double angle) { + goal = angle; + } @Override - public void setVoltage(double voltage) { - armMotor.setInputVoltage(voltage); + public void updateLoop() { + setVoltage(controller.calculate(armSim.getAngleRads(), goal)); } + @Override - public void setVelocity(double velocity) { - armMotor.setInputVoltage(ff.calculate(armMotor.getAngleRads(), velocity)); + public void updateInputs(ArmIOInputs inputs) { + inputs.angularPosition = armSim.getAngleRads(); + inputs.angularVelocity = armSim.getVelocityRadPerSec(); + inputs.current = armSim.getCurrentDrawAmps(); + inputs.voltage = voltage; + inputs.goal = goal; + armSim.update(0.02); } } - diff --git a/src/main/java/frc/robot/subsystems/arm/CurrentLimits.java b/src/main/java/frc/robot/subsystems/arm/CurrentLimits.java deleted file mode 100644 index 1dd5d3a..0000000 --- a/src/main/java/frc/robot/subsystems/arm/CurrentLimits.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.arm; - -public class CurrentLimits { - -} From 3d4e4a2ce7569bfe9e3e8ff034689068ae44493b Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Sat, 18 Jan 2025 14:51:42 -0500 Subject: [PATCH 028/110] drive sim work but not turn yet --- src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Example Path.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 32 +++++++++++ src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Main.java | 15 +++++- src/main/java/frc/robot/Robot.java | 5 +- src/main/java/frc/robot/RobotContainer.java | 27 ++++++---- .../robot/subsystems/Drive/Drive/Drive.java | 25 +++++---- .../Drive/Drive/DriveConstants.java | 4 +- .../robot/subsystems/Drive/Drive/Module.java | 15 +++--- .../subsystems/Drive/Drive/ModuleIOSim.java | 6 +-- 11 files changed, 148 insertions(+), 38 deletions(-) create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..6232c20 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.6, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..603f423 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 5.0, + "defaultMaxAccel": 5.6, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 48.0, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c992ea1..3aee49f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,7 +2,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class Constants { - public static Mode currentMode = Mode.REAL; + public static Mode currentMode = Mode.SIM; public static enum Mode { REAL, SIM, diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fe215d7..0d622b4 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -4,12 +4,23 @@ package frc.robot; +import java.io.IOException; +import java.text.ParseException; + import edu.wpi.first.wpilibj.RobotBase; -public final class Main { +public final class Main { private Main() {} public static void main(String... args) { - RobotBase.startRobot(Robot::new); + RobotBase.startRobot(() -> { + try { + return new Robot(); + } catch (IOException | org.json.simple.parser.ParseException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + return null; + }); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 80bdef4..0e01211 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,9 @@ package frc.robot; +import java.io.IOException; + +import org.json.simple.parser.ParseException; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -22,7 +25,7 @@ public class Robot extends LoggedRobot { private final RobotContainer m_robotContainer; - public Robot() { + public Robot() throws IOException, ParseException { Logger.recordMetadata("ProjectName", "2025-Reefscape"); switch (Constants.currentMode) { case REAL: diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0541b1..29a64b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,10 @@ package frc.robot; +import java.io.IOException; + +import org.json.simple.parser.ParseException; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Commands.DriveCommands; @@ -18,7 +22,7 @@ public class RobotContainer { private Drive drive; - public RobotContainer() { + public RobotContainer() throws IOException, ParseException { switch (Constants.currentMode) { case REAL: @@ -45,12 +49,13 @@ public RobotContainer() { default: drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO(){}, - new ModuleIO() {}, - new ModuleIO() {}); + new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + break; } @@ -62,9 +67,11 @@ private void configureBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -Constants.OIConstants.driverController.getLeftY(), - () -> -Constants.OIConstants.driverController.getLeftX(), - () -> -Constants.OIConstants.driverController.getRightX())); + () -> Constants.OIConstants.driverController.getLeftY(), + () -> Constants.OIConstants.driverController.getLeftX(), + () -> -Constants.OIConstants.driverController.getRightX() + // () -> -Constants.OIConstants.driverController.getRightY() + )); Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(()-> drive.zeroHeading(), drive)); diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java b/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java index 5680a20..5faca32 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java @@ -2,6 +2,9 @@ import static edu.wpi.first.units.Units.*; +import java.io.IOException; + +import org.json.simple.parser.ParseException; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -59,14 +62,14 @@ public class Drive extends SubsystemBase { new SwerveModulePosition() }; - private SwerveDrivePoseEstimator poseEstimator = + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation2d, lastModulePositions, new Pose2d()); public static Drive getInstance(){ return instance; } - public static Drive initialize (GyroIO gyro, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { + public static Drive initialize (GyroIO gyro, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) throws IOException, ParseException { if (instance == null) { instance = new Drive(gyro, fl, fr, bl, br); } @@ -78,7 +81,7 @@ public Drive( ModuleIO flModuleIO, ModuleIO frModuleIO, ModuleIO blModuleIO, - ModuleIO brModuleIO) { + ModuleIO brModuleIO) throws IOException, ParseException { this.gyroIO = gyroIO; modules[0] = new Module(flModuleIO, 0); modules[1] = new Module(frModuleIO, 1); @@ -86,14 +89,17 @@ public Drive( modules[3] = new Module(brModuleIO, 3); RobotConfig config; + + config = RobotConfig.fromGUISettings(); + try{ config = RobotConfig.fromGUISettings(); } catch (Exception e) { // Handle exception as needed - e.printStackTrace(); - } finally { - config = null; // if something went horribly wrong - } + e.printStackTrace(); } + // } finally { + // config = null; // if something went horribly wrong + // } // Configure AutoBuilder last AutoBuilder.configure( @@ -198,13 +204,12 @@ public void periodic() { rawGyroRotation2d = rawGyroRotation2d.plus(new Rotation2d(twist.dtheta)); } -poseEstimator.update(rawGyroRotation2d, modulePositions); + poseEstimator.update(rawGyroRotation2d, modulePositions); } public void runVelocity(ChassisSpeeds speeds){ - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates,DriveConstants.MAX_LINEAR_SPEED); @@ -273,7 +278,7 @@ private SwerveModulePosition[] getModulePositions(){ } public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, null); // DriveConstants.kMaxSpeedMetersPerSecond + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, 4.8); // DriveConstants.kMaxSpeedMetersPerSecond for (int i = 0; i < 4; i++) { modules[i].setDesiredState(desiredStates[i]); } diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java index 603a6c1..e140054 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java @@ -13,7 +13,7 @@ public static enum Mode { REPLAY, } - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(5); + public static final double MAX_LINEAR_SPEED = Units.metersToFeet(5); public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); public static final double DRIVE_BASE_RADIUS = @@ -25,7 +25,7 @@ public static enum Mode { public static final double RotationKP = 5.0; public static final double RotationKI = 0.0; public static final double RotationKD = 0.0; - public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + public static final double WHEEL_RADIUS = Units.inchesToMeters(1.5); public static final double DriveGearing = 13.371; public static final double TurnGearing = 6.75; public static final int turnCurrentLimit = 60; //May need to change later diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java b/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java index 675e471..0d44e36 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java @@ -63,10 +63,8 @@ public void periodic() { } - if (angleSetpoint != null) { - io.setTurnVoltage( - turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); - + if (angleSetpoint != null) { + io.setTurnVoltage(turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); if (speedSetPoint != null) { @@ -86,11 +84,10 @@ public void periodic() { public SwerveModuleState runSetpoint(SwerveModuleState state) { - var optimizedState = SwerveModuleState.optimize(state, getAngle()); - angleSetpoint = optimizedState.angle; - speedSetPoint = optimizedState.speedMetersPerSecond; - - return optimizedState; + state.optimize(getAngle()); + angleSetpoint = state.angle; + speedSetPoint = state.speedMetersPerSecond; + return state; } public void runCharacterization(double volts) { diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java index 6a01434..afb1b11 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java @@ -9,8 +9,8 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; //change gearbox argument - private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 0.025, DriveConstants.DriveGearing), DCMotor.getNeoVortex(4)); //needs to change - private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 0.004, DriveConstants.TurnGearing), DCMotor.getKrakenX60(4)); //needs to change + private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 0.025, DriveConstants.DriveGearing), DCMotor.getKrakenX60(4)); //needs to change + private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 0.004, DriveConstants.TurnGearing), DCMotor.getNeoVortex(4)); //needs to change private final Rotation2d turnAbsoluteInitPositon = new Rotation2d(Math.random()*2.0*Math.PI); private double driveAppliedVolts = 0.0; @@ -22,7 +22,7 @@ public void updateInputs(ModuleIOInputs inputs){ turnSim.update(LOOP_PERIOD_SECS); inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; From ba3ca0a0e4bdffca50aafd43a1d1f8c235fdeea3 Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Sat, 18 Jan 2025 14:53:55 -0500 Subject: [PATCH 029/110] changed sim motors --- .../java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java index afb1b11..50d51c4 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java @@ -9,8 +9,8 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; //change gearbox argument - private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 0.025, DriveConstants.DriveGearing), DCMotor.getKrakenX60(4)); //needs to change - private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 0.004, DriveConstants.TurnGearing), DCMotor.getNeoVortex(4)); //needs to change + private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 0.025, DriveConstants.DriveGearing), DCMotor.getKrakenX60(4)); //needs to change + private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 0.004, DriveConstants.TurnGearing), DCMotor.getNeoVortex(4)); //needs to change private final Rotation2d turnAbsoluteInitPositon = new Rotation2d(Math.random()*2.0*Math.PI); private double driveAppliedVolts = 0.0; From 19bf4b7d0495acebf8b7755462831c2b77c752ac Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Sat, 18 Jan 2025 15:43:35 -0500 Subject: [PATCH 030/110] only 1 current mode now in constants --- src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java | 4 ++-- .../frc/robot/subsystems/Drive/Drive/DriveConstants.java | 8 -------- .../java/frc/robot/subsystems/Drive/Drive/Module.java | 8 ++++++-- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java b/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java index 5faca32..54eafd1 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java @@ -37,7 +37,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.subsystems.Drive.Drive.DriveConstants.Mode; +import frc.robot.Constants; public class Drive extends SubsystemBase { @@ -240,7 +240,7 @@ public void stopWithx() { } public void zeroHeading() { - if (DriveConstants.currentMode == Mode.SIM){ + if (Constants.currentMode == frc.robot.Constants.Mode.SIM){ poseEstimator.resetPosition( new Rotation2d(), new SwerveModulePosition[]{ diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java index e140054..4e0ed4a 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java @@ -3,15 +3,7 @@ import edu.wpi.first.math.util.Units; public class DriveConstants { - public static final Mode currentMode = Mode.REAL; - public static enum Mode { - REAL, - - SIM, - - REPLAY, - } public static final double MAX_LINEAR_SPEED = Units.metersToFeet(5); public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java b/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java index 0d44e36..f866b26 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java @@ -5,8 +5,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.Constants; + import org.littletonrobotics.junction.Logger; + public class Module { @@ -25,7 +28,7 @@ public Module (ModuleIO io, int index) { this.io = io; this.index = index; - switch (DriveConstants.currentMode) { + switch (Constants.currentMode) { case REAL: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); @@ -64,7 +67,8 @@ public void periodic() { if (angleSetpoint != null) { - io.setTurnVoltage(turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + io.setTurnVoltage(turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + if (speedSetPoint != null) { From 92e5f565516ece865a1b6e203e29336c7ad31eb3 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Mon, 20 Jan 2025 14:44:25 -0500 Subject: [PATCH 031/110] motion profiling in sim --- simgui-ds.json | 3 +- .../java/frc/robot/subsystems/arm/Arm.java | 6 --- .../robot/subsystems/arm/ArmConstants.java | 2 +- .../java/frc/robot/subsystems/arm/ArmIO.java | 10 ++-- .../frc/robot/subsystems/arm/ArmIOReal.java | 5 -- .../frc/robot/subsystems/arm/ArmIOSim.java | 46 ++++++++++--------- 6 files changed, 30 insertions(+), 42 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index f84381f..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,8 @@ ], "robotJoysticks": [ { - "guid": "03000000c82d00000631000014010000" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 51c4c6d..b20ec66 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -3,7 +3,6 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.arm.ArmIO.ArmIOInputs; public class Arm extends SubsystemBase { private ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); @@ -34,12 +33,7 @@ public void setOpenLoop(boolean openLoop) { @Override public void periodic() { - if(!openLoop) { - io.updateLoop(); - } - this.inputs.openLoop = openLoop; - io.updateInputs(inputs); Logger.processInputs(getName(), inputs); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index aa15f26..86f74d4 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -5,7 +5,7 @@ public class ArmConstants { public static double CURRENT_LIMIT = 40; public static class Sim { - public static double GEARING = 6; + public static double GEARING = 100; public static double MOI = 2; public static double LENGTH = 0.5; public static double MIN_ANGLE = -Math.PI / 6; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index e5bd0bf..ecdd43e 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -9,8 +9,9 @@ public static class ArmIOInputs{ public double angularPosition = 0.0; public double angularVelocity = 0.0; public double current = 0.0; - public double goal = 0.0; + public double goalAngle = 0.0; public boolean openLoop = false; + public double setpointVelocity = 0.0; } public default void updateInputs(ArmIOInputs inputs) { @@ -19,13 +20,8 @@ public default void updateInputs(ArmIOInputs inputs) { public default void setVoltage(double voltage) { } - public default void setVelocity(double velocity){ - - } + public default void setGoal(double angle) { - } - public default void updateLoop() { - } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index ca1a319..1c3e16c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -55,11 +55,6 @@ public void updateInputs(ArmIOInputs inputs) { inputs.angularPosition = armMotor.getPosition().getValueAsDouble()*2*Math.PI; } - @Override - public void setVelocity(double velocity) { - setVoltage(ffmodel.calculate(position.getValueAsDouble() * 2 * Math.PI, velocity)); - } - public void setVoltage(double voltage) { armMotor.setVoltage(-voltage); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 780ed5a..8a7076b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -1,15 +1,14 @@ package frc.robot.subsystems.arm; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.AngularMomentum; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import frc.robot.subsystems.arm.ArmIO.ArmIOInputs; public class ArmIOSim implements ArmIO { private final SingleJointedArmSim armSim = new SingleJointedArmSim( - DCMotor.getKrakenX60(2), + DCMotor.getNeoVortex(2), ArmConstants.Sim.GEARING, ArmConstants.Sim.MOI, ArmConstants.Sim.LENGTH, @@ -19,40 +18,43 @@ public class ArmIOSim implements ArmIO { ArmConstants.Sim.INIT_ANGLE ); private double voltage = 0; - private final ArmFeedforward ffmodel = new ArmFeedforward(0.1, 0.1, 0.1); - private final PIDController controller = new PIDController(0.01, 0.02, 0); - private double goal = ArmConstants.Sim.INIT_ANGLE; + private final SimpleMotorFeedforward ffmodel = new SimpleMotorFeedforward(0.03, 1.65); + private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 2); + private final PIDController controller = new PIDController(0.02, 0,0.00); + private final TrapezoidProfile profile = new TrapezoidProfile(constraints); + private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE + 0.5, 0); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - @Override - public void setVoltage(double voltage) { - this.voltage = voltage; - armSim.setInputVoltage(voltage); - } + public ArmIOSim() { - @Override - public void setVelocity(double velocity) { - setVoltage(ffmodel.calculate(armSim.getAngleRads(), velocity)); } @Override - public void setGoal(double angle) { - goal = angle; - + public void setVoltage(double voltage) { + armSim.setInputVoltage(voltage); + this.voltage = voltage; } @Override - public void updateLoop() { - setVoltage(controller.calculate(armSim.getAngleRads(), goal)); + public void setGoal(double angle) { + goal = new TrapezoidProfile.State(angle + 0.5, 0); } - @Override public void updateInputs(ArmIOInputs inputs) { + if(!inputs.openLoop) { + setpoint = profile.calculate(0.02, setpoint, goal); + setVoltage( + ffmodel.calculate(setpoint.velocity) + ); + } + inputs.angularPosition = armSim.getAngleRads(); inputs.angularVelocity = armSim.getVelocityRadPerSec(); inputs.current = armSim.getCurrentDrawAmps(); inputs.voltage = voltage; - inputs.goal = goal; + inputs.goalAngle = goal.position - 0.5; + inputs.setpointVelocity = setpoint.velocity; armSim.update(0.02); } } From ec5e4954f48ae32598d9d706bb8b1bd2e34b1ea7 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Mon, 20 Jan 2025 22:01:20 -0500 Subject: [PATCH 032/110] started visualization --- src/main/java/frc/robot/subsystems/arm/Arm.java | 14 ++++++++++++++ .../frc/robot/subsystems/arm/ArmConstants.java | 1 + .../java/frc/robot/subsystems/arm/ArmIOSim.java | 11 +++++++---- 3 files changed, 22 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index b20ec66..9da7c4b 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -1,7 +1,13 @@ package frc.robot.subsystems.arm; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { @@ -31,8 +37,16 @@ public void setOpenLoop(boolean openLoop) { this.openLoop = openLoop; } + public Pose3d getArmPose(){ + return new Pose3d( + new Translation3d(10 + -(0.45) * Math.cos(inputs.angularPosition), 1, 1 + (0.45) * Math.sin(inputs.angularPosition)), + new Rotation3d(0, inputs.angularPosition, 0) + ); + } + @Override public void periodic() { + Logger.recordOutput(getName() + "/Pose", getArmPose()); this.inputs.openLoop = openLoop; io.updateInputs(inputs); Logger.processInputs(getName(), inputs); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 86f74d4..c464b41 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -4,6 +4,7 @@ public class ArmConstants { public static int CAN_ID = 10; public static double CURRENT_LIMIT = 40; + // need to be tuned public static class Sim { public static double GEARING = 100; public static double MOI = 2; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 8a7076b..abae417 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -1,8 +1,13 @@ package frc.robot.subsystems.arm; +import com.revrobotics.sim.SparkFlexSim; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; @@ -18,7 +23,7 @@ public class ArmIOSim implements ArmIO { ArmConstants.Sim.INIT_ANGLE ); private double voltage = 0; - private final SimpleMotorFeedforward ffmodel = new SimpleMotorFeedforward(0.03, 1.65); + private final SimpleMotorFeedforward ffmodel = new SimpleMotorFeedforward(0.04, 1.65); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 2); private final PIDController controller = new PIDController(0.02, 0,0.00); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); @@ -43,10 +48,8 @@ public void setGoal(double angle) { @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { + setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(armSim.getVelocityRadPerSec(), setpoint.velocity)); setpoint = profile.calculate(0.02, setpoint, goal); - setVoltage( - ffmodel.calculate(setpoint.velocity) - ); } inputs.angularPosition = armSim.getAngleRads(); From d0b59e6f31d2a817803d75076ae42f6043d3fe29 Mon Sep 17 00:00:00 2001 From: elsaroni <150392934+elsaroni@users.noreply.github.com> Date: Mon, 20 Jan 2025 22:17:27 -0500 Subject: [PATCH 033/110] elevator sim (might need to fix) --- .../robot/subsystems/elevator/ElevatorIOSim.java | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 765d243..5dede45 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -15,19 +15,31 @@ public class ElevatorIOSim implements ElevatorIO { private ElevatorSim elevatorSim = new ElevatorSim(0, 0, null, 0, 0, false, 0, null); - private double ElevatorAppliedVolts = 0.0; + private double elevatorAppliedVolts = 0.0; +@Override +public void updateInputs(ElevatorIOInputs inputs) { + elevatorSim.update(ElevatorConstants.kDt); + inputs.elevatorPositionRads = elevatorSim.getAngleRads(); + inputs.elevatorMotorPositionRads = elevatorSim.getAngleRads(); + inputs.elevatorMotorVelocityRadPerSec = elevatorSim.getVelocityRadPerSec(); + inputs.elevatorCurrent = elevatorSim.getCurrentDrawAmps(); + inputs.elevatorAppliedVolts = elevatorAppliedVolts; @Override public void moveElevator(double speed) { ElevatorAppliedVolts = MathUtil.clamp(speed, speed, speed); + elevatorSim.setInputVoltage(elevatorAppliedVolts) } @Override public void setVoltage(double volts){ ElevatorAppliedVolts = volts; + Logger.recordOutput("Setting output", volts); + elevatorSim.setInputVoltage(elevatorAppliedVolts - (voltage, voltage, voltage)); } + } } \ No newline at end of file From e911db50a29d7789a4da5fe03391f9c7fe3bda17 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sat, 25 Jan 2025 16:14:01 -0500 Subject: [PATCH 034/110] switched arm code to sparkflex --- .vscode/settings.json | 3 +- .../robot/subsystems/arm/ArmConstants.java | 6 +- .../frc/robot/subsystems/arm/ArmIOReal.java | 77 ++++++++----------- .../frc/robot/subsystems/arm/ArmIOSim.java | 5 -- 4 files changed, 38 insertions(+), 53 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index dccbc7c..0df8b28 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index c464b41..49d2bb0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -2,12 +2,12 @@ public class ArmConstants { public static int CAN_ID = 10; - public static double CURRENT_LIMIT = 40; + public static int CURRENT_LIMIT = 40; // need to be tuned public static class Sim { - public static double GEARING = 100; - public static double MOI = 2; + public static double GEARING = 40; + public static double MOI = 2.09670337984; public static double LENGTH = 0.5; public static double MIN_ANGLE = -Math.PI / 6; public static double MAX_ANGLE = 2 * Math.PI / 3; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 1c3e16c..e856236 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -1,63 +1,52 @@ package frc.robot.subsystems.arm; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.units.measure.Angle; -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.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; public class ArmIOReal implements ArmIO { - private final TalonFX armMotor = new TalonFX(ArmConstants.CAN_ID); - private TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - private final StatusSignal current = armMotor.getStatorCurrent(); - private final StatusSignal voltage = armMotor.getMotorVoltage(); - private final StatusSignal velocity = armMotor.getVelocity(); - private final StatusSignal position = armMotor.getPosition(); - private final ArmFeedforward ffmodel = new ArmFeedforward(0.1, 0.1, 0.1); + private final SparkFlex armMotor = new SparkFlex(ArmConstants.CAN_ID, MotorType.kBrushless); + private SparkFlexConfig config = new SparkFlexConfig(); + private AbsoluteEncoder armEncoder = armMotor.getAbsoluteEncoder(); + + private final ArmFeedforward ffmodel = new ArmFeedforward(0, 0, 0); + private final PIDController controller = new PIDController(0.02, 0,0.00); + private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 2); + private final TrapezoidProfile profile = new TrapezoidProfile(constraints); + private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); public ArmIOReal(){ - talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfig.CurrentLimits.StatorCurrentLimit = ArmConstants.CURRENT_LIMIT; - talonFXConfig.Audio.BeepOnBoot = true; - talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - armMotor.clearStickyFaults(); - - BaseStatusSignal.setUpdateFrequencyForAll(50,velocity, voltage, current); - - armMotor.optimizeBusUtilization(50, 1); - - StatusCode response = armMotor.getConfigurator().apply(talonFXConfig); - - if(!response.isOK()){ - System.out.println( - "Talon ID" - + armMotor.getDeviceID() - + "failed config with error" - + response.toString()); - } + config.smartCurrentLimit(ArmConstants.CURRENT_LIMIT); + config.idleMode(IdleMode.kBrake); + armMotor.clearFaults(); + } + + @Override + public void setGoal(double angle) { + goal = new TrapezoidProfile.State(angle, 0); } @Override public void updateInputs(ArmIOInputs inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, current); + if(!inputs.openLoop) { + setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); + setpoint = profile.calculate(0.02, setpoint, goal); + } - inputs.angularPosition = position.getValueAsDouble() * 2 * Math.PI; - inputs.angularVelocity = velocity.getValueAsDouble() * 2 * Math.PI; - inputs.current = current.getValueAsDouble(); - inputs.voltage = voltage.getValueAsDouble(); - inputs.angularPosition = armMotor.getPosition().getValueAsDouble()*2*Math.PI; + inputs.angularPosition = armEncoder.getPosition() * 2 * Math.PI; + inputs.angularVelocity = armEncoder.getVelocity() * 2 * Math.PI / 60; + inputs.current = armMotor.getOutputCurrent(); + inputs.voltage = armMotor.getAppliedOutput(); } public void setVoltage(double voltage) { armMotor.setVoltage(-voltage); } - - } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index abae417..33786ed 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -1,13 +1,8 @@ package frc.robot.subsystems.arm; -import com.revrobotics.sim.SparkFlexSim; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; From 0ab7c37cc1b56993808b064084f65d13aa5b024a Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sat, 25 Jan 2025 16:41:20 -0500 Subject: [PATCH 035/110] apply motor config --- src/main/java/frc/robot/subsystems/arm/ArmIOReal.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index e856236..649ec9a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -2,6 +2,8 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; @@ -26,6 +28,7 @@ public ArmIOReal(){ config.smartCurrentLimit(ArmConstants.CURRENT_LIMIT); config.idleMode(IdleMode.kBrake); armMotor.clearFaults(); + armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @Override From 9dcc7060e0f9621a5cd10f5cb59151221db1d62c Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sat, 25 Jan 2025 18:02:13 -0500 Subject: [PATCH 036/110] fixed turning sim --- simgui-ds.json | 3 +-- src/main/java/frc/robot/RobotContainer.java | 14 ++++++-------- .../{Commands => commands}/DriveCommands.java | 4 ++-- .../subsystems/{Drive/Drive => drive}/Drive.java | 13 +++++-------- .../{Drive/Drive => drive}/DriveConstants.java | 8 ++------ .../subsystems/{Drive/Drive => drive}/GyroIO.java | 2 +- .../{Drive/Drive => drive}/GyroIOPigeon.java | 2 +- .../subsystems/{Drive/Drive => drive}/Module.java | 10 +++------- .../{Drive/Drive => drive}/ModuleIO.java | 2 +- .../{Drive/Drive => drive}/ModuleIOSim.java | 2 +- .../{Drive/Drive => drive}/ModuleIOSparkFlex.Java | 0 .../{Drive/Drive => drive}/ModuleIOTalonFX.java | 2 +- 12 files changed, 24 insertions(+), 38 deletions(-) rename src/main/java/frc/robot/{Commands => commands}/DriveCommands.java (97%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/Drive.java (98%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/DriveConstants.java (86%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/GyroIO.java (90%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/GyroIOPigeon.java (96%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/Module.java (97%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/ModuleIO.java (96%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/ModuleIOSim.java (98%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/ModuleIOSparkFlex.Java (100%) rename src/main/java/frc/robot/subsystems/{Drive/Drive => drive}/ModuleIOTalonFX.java (99%) diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..f84381f 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,8 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "03000000c82d00000631000014010000" } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29a64b1..4ab381d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,13 +10,12 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Commands.DriveCommands; -import frc.robot.subsystems.Drive.Drive.Drive; -import frc.robot.subsystems.Drive.Drive.GyroIO; -import frc.robot.subsystems.Drive.Drive.GyroIOPigeon; -import frc.robot.subsystems.Drive.Drive.ModuleIO; -import frc.robot.subsystems.Drive.Drive.ModuleIOSim; -import frc.robot.subsystems.Drive.Drive.ModuleIOTalonFX; +import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.GyroIO; +import frc.robot.subsystems.drive.GyroIOPigeon; +import frc.robot.subsystems.drive.ModuleIOSim; +import frc.robot.subsystems.drive.ModuleIOTalonFX; public class RobotContainer { @@ -70,7 +69,6 @@ private void configureBindings() { () -> Constants.OIConstants.driverController.getLeftY(), () -> Constants.OIConstants.driverController.getLeftX(), () -> -Constants.OIConstants.driverController.getRightX() - // () -> -Constants.OIConstants.driverController.getRightY() )); Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(()-> drive.zeroHeading(), drive)); diff --git a/src/main/java/frc/robot/Commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java similarity index 97% rename from src/main/java/frc/robot/Commands/DriveCommands.java rename to src/main/java/frc/robot/commands/DriveCommands.java index 2fad06b..8ed1fd2 100644 --- a/src/main/java/frc/robot/Commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -1,4 +1,4 @@ -package frc.robot.Commands; +package frc.robot.commands; import java.util.function.DoubleSupplier; @@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants; -import frc.robot.subsystems.Drive.Drive.Drive; +import frc.robot.subsystems.drive.Drive; public class DriveCommands { private DriveCommands() {} diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java similarity index 98% rename from src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java rename to src/main/java/frc/robot/subsystems/drive/Drive.java index 54eafd1..f62cf80 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Drive.Drive; +package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.*; @@ -200,12 +200,10 @@ public void periodic() { rawGyroRotation2d = gyroInputs.yawPosition; } else { - Twist2d twist = K_DRIVE_KINEMATICS.toTwist2d(moduleDeltas, moduleDeltas); + Twist2d twist = K_DRIVE_KINEMATICS.toTwist2d(moduleDeltas); rawGyroRotation2d = rawGyroRotation2d.plus(new Rotation2d(twist.dtheta)); } - poseEstimator.update(rawGyroRotation2d, modulePositions); - } @@ -216,7 +214,6 @@ public void runVelocity(ChassisSpeeds speeds){ SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); } Logger.recordOutput("SwerveStates/Setpoints", setpointStates); @@ -328,9 +325,9 @@ public double getMaxAngularSpeedRadPerSec(){ public static Translation2d[] getModuleTranslation2d(){ return new Translation2d[]{ new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), - new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), - new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), - new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), + new Translation2d(DriveConstants.TRACK_LENGTH/2.0, -DriveConstants.TRACK_WIDTH/2.0), + new Translation2d(-DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), + new Translation2d(-DriveConstants.TRACK_LENGTH/2.0, -DriveConstants.TRACK_WIDTH/2.0), }; } diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java similarity index 86% rename from src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java rename to src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 4e0ed4a..7eaae0e 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -1,16 +1,14 @@ -package frc.robot.subsystems.Drive.Drive; +package frc.robot.subsystems.drive; import edu.wpi.first.math.util.Units; public class DriveConstants { - - public static final double MAX_LINEAR_SPEED = Units.metersToFeet(5); public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); public static final double DRIVE_BASE_RADIUS = Math.hypot(DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0); - public static final double MAX_ANGULAR_SPEED = DriveConstants.MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + public static final double MAX_ANGULAR_SPEED = Units.feetToMeters(DriveConstants.MAX_LINEAR_SPEED) / DRIVE_BASE_RADIUS; public static final double TranslationKP = 5.0; public static final double TranslationKI = 0.0; public static final double TranslationKD = 0.0; @@ -21,8 +19,6 @@ public class DriveConstants { public static final double DriveGearing = 13.371; public static final double TurnGearing = 6.75; public static final int turnCurrentLimit = 60; //May need to change later - - } diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java similarity index 90% rename from src/main/java/frc/robot/subsystems/Drive/Drive/GyroIO.java rename to src/main/java/frc/robot/subsystems/drive/GyroIO.java index 9ea91ef..484f8ef 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Drive.Drive; +package frc.robot.subsystems.drive; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/GyroIOPigeon.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java similarity index 96% rename from src/main/java/frc/robot/subsystems/Drive/Drive/GyroIOPigeon.java rename to src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java index 26d8305..3e48653 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/GyroIOPigeon.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Drive.Drive; +package frc.robot.subsystems.drive; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java similarity index 97% rename from src/main/java/frc/robot/subsystems/Drive/Drive/Module.java rename to src/main/java/frc/robot/subsystems/drive/Module.java index f866b26..49710a7 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Drive.Drive; +package frc.robot.subsystems.drive; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -66,22 +66,18 @@ public void periodic() { } - if (angleSetpoint != null) { + if (angleSetpoint != null) { io.setTurnVoltage(turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + } if (speedSetPoint != null) { - - - - double adjustSpeedSetpoint = speedSetPoint*Math.cos(turnFeePidController.getPositionError()); double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS; io.setDriveVoltage( drivFeedforward.calculate(velocityRadPerSec) + driveFeddbackPidController.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - } } } diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java similarity index 96% rename from src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIO.java rename to src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 4a86711..1f267a1 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Drive.Drive; +package frc.robot.subsystems.drive; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java similarity index 98% rename from src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java rename to src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 50d51c4..73c332e 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Drive.Drive; +package frc.robot.subsystems.drive; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSparkFlex.Java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java similarity index 100% rename from src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOSparkFlex.Java rename to src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java similarity index 99% rename from src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOTalonFX.java rename to src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 4bd33df..9564195 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.Drive.Drive; +package frc.robot.subsystems.drive; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.TalonFXConfiguration; From 64253e06b9e42adff96825a5276a57e3c8b3cdf4 Mon Sep 17 00:00:00 2001 From: elsaroni <150392934+elsaroni@users.noreply.github.com> Date: Sun, 26 Jan 2025 00:12:53 -0500 Subject: [PATCH 037/110] elevatorIOReal stuff made a currentlimits class in constants.java and set the elevator current limit to 30 --- src/main/java/frc/robot/Constants.java | 6 +++ .../elevator/ElevatorConstants.java | 4 ++ .../subsystems/elevator/ElevatorIOReal.java | 40 +++++++------------ 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eb7d9c5..0c4bb8b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,4 +7,10 @@ public static enum Mode { SIM, REPLAY } + + + + public static final class CurrentLimits { + public static final int elevator = 30; + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 373fc73..22c5da0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -9,4 +9,8 @@ public final class ElevatorConstants{ public static double rightDeviceID = 1; public static double leftDeviceID = 2; + + + + } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index e924ba6..a705fd2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -1,18 +1,15 @@ package frc.robot.subsystems.elevator; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.AbsoluteEncoder; + import com.revrobotics.RelativeEncoder; import edu.wpi.first.units.measure.AngularVelocity; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkFlexConfig; +import frc.robot.Constants.CurrentLimits; + import edu.wpi.first.units.measure.Current; @@ -21,16 +18,17 @@ public class ElevatorIOReal implements ElevatorIO { private SparkFlex rightElevatorMotor = new SparkFlex(1, MotorType.kBrushless); private SparkFlex leftElevatorMotor = new SparkFlex(2, MotorType.kBrushless); + + + leftElevatorMotor.setSmartCurrentLimit(CurrentLimits.elevator); + rightElevatorMotor.setSmartCurrentLimit(CurrentLimits.elevator); + private double inputVolts = 0.0; final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); - // private final StatusSignal topRPM = leftElevatorMotor.getVelocity(); - // private final StatusSignal bottomRPM = rightElevatorMotor.getVelocity(); - // private final StatusSignal topCurrent = leftElevatorMotor.getTorqueCurrent(); - // private final StatusSignal bottomCurrent = rightElevatorMotor.getTorqueCurrent(); - + private boolean isEnabled; private boolean hasPlayed = false; @@ -38,25 +36,17 @@ public class ElevatorIOReal implements ElevatorIO { private SparkFlexConfig config(double kV) { var SparkFlexConfig = new SparkFlexConfig(); - // SparkFlexConfig.Slot0.kP = ElevatorConstants.kP; - // SparkFlexConfig.Slot0.kI = 0; - // SparkFlexConfig.Slot0.kD = 0; - // SparkFlexConfig.Slot0.kS = 0; - // SparkFlexConfig.Slot0.kV = kV; - - // SparkFlexConfig.CurrentLimits.StatorCurrentLimitEnable = true; - // SparkFlexConfig.CurrentLimits.StatorCurrentLimit = CurrentLimits.shooter; - - // SparkFlexConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - - // SparkFlexConfig.Audio.BeepOnBoot = true; - return SparkFlexConfig; } + @Override + public void updateInputs(ElevatorIOInputs inputs) { + } + public ElevatorIOReal(){ - //leftElevatorMotor.setControl(new Follower(rightElevatorMotor.getDeviceID(), true)); + + leftElevatorMotor.setControl(new Follower(rightElevatorMotor.getDeviceID(), true)); } From c0e9a89a523742f44a5cc691417707c7ca3df055 Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Sun, 26 Jan 2025 13:18:32 -0500 Subject: [PATCH 038/110] added current sensing in case we want to do that --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/intake/Intake.java | 5 +++-- .../frc/robot/subsystems/intake/IntakeConstants.java | 1 + .../java/frc/robot/subsystems/intake/IntakeIO.java | 4 ++++ .../frc/robot/subsystems/intake/IntakeIOReal.java | 12 ++++++++---- .../frc/robot/subsystems/intake/IntakeIOSim.java | 7 ++++++- 6 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6cbcf5e..1c23eba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,7 +24,7 @@ public RobotContainer() { switch (Constants.currentMode) { case REAL: - intake = Intake.initialize(new IntakeIOSim()); + intake = Intake.initialize(new IntakeIOReal()); break; case SIM: intake = Intake.initialize(new IntakeIOSim()); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 7d194d1..403c0d1 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -42,8 +42,8 @@ public Command intake() { public Command reverse() { return startEnd(() -> setIntakeSpeed(IntakeSetpoints.reverse), () -> setIntakeSpeed(0)); } - public boolean isIntaking() { - return inputs.currentAmps > IntakeConstants.intakeCurrent && inputs.angularVelocityRPM > 2400; + public boolean hasGamepiece() { + return io.hasGamepiece(); } @Override @@ -52,6 +52,7 @@ public void periodic(){ Logger.processInputs("Intake", inputs); io.setMotorSpeed(intakeSpeed); Logger.recordOutput("Intake/Intake speed", intakeSpeed); + Logger.recordOutput("Intake/Gamepiece detected", hasGamepiece()); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 3dee92b..651e600 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -11,4 +11,5 @@ public final class IntakeConstants { public static final double intakeCurrent = 0; public static final double currentLimit = 30; + public static final double currentThreshold = 20; //change later based on akit numbers for gamepiece } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index bfa08ae..cee9632 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -17,4 +17,8 @@ public default void updateInputs(IntakeIOInputs inputs) { public default void setMotorSpeed(double speed) { } + + public default boolean hasGamepiece() { + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index 49d3afd..6f246b0 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.SparkLimitSwitch; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -18,7 +19,6 @@ public class IntakeIOReal implements IntakeIO { private final TalonFX intakeMotor = new TalonFX(IntakeConstants.canID); private TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - // private TalonFXConfigurator taIonFXConfig = new TalonFXConfigurator(IntakeConstants.canID) private final StatusSignal current = intakeMotor.getStatorCurrent(); private final StatusSignal voltage = intakeMotor.getMotorVoltage(); @@ -30,7 +30,6 @@ public IntakeIOReal() { talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; talonFXConfig.CurrentLimits.StatorCurrentLimit = IntakeConstants.currentLimit; - // talonFXConfig. talonFXConfig.Audio.BeepOnBoot = true; @@ -40,7 +39,7 @@ public IntakeIOReal() { BaseStatusSignal.setUpdateFrequencyForAll(50, velocity, voltage, current); - intakeMotor.optimizeBusUtilization(1.0); + intakeMotor.optimizeBusUtilization(4, 1); StatusCode response = intakeMotor.getConfigurator().apply(talonFXConfig); if (!response.isOK()) { @@ -58,7 +57,12 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.angularVelocityRPM = velocity.getValueAsDouble() * 60; inputs.angularPositionRot = intakeMotor.getPosition().getValueAsDouble(); inputs.currentAmps = current.getValueAsDouble(); - inputs.voltage = voltage.getValueAsDouble(); + inputs.voltage = voltage.getValueAsDouble(); + } + + @Override + public boolean hasGamepiece() { + return current.getValueAsDouble() > IntakeConstants.currentThreshold; } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 831557e..31912b4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -10,7 +10,7 @@ public class IntakeIOSim implements IntakeIO { // not done private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1), DCMotor.getKrakenX60(1), 0.0, 0.0); - + private boolean isGamepieceDetected = false; @Override public void updateInputs(IntakeIOInputs inputs) { motor.update(0.02); @@ -18,6 +18,11 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.angularPositionRot = motor.getAngularPositionRotations(); inputs.angularVelocityRPM = motor.getAngularVelocityRPM(); inputs.currentAmps = motor.getCurrentDrawAmps(); + isGamepieceDetected = inputs.currentAmps > IntakeConstants.currentThreshold; + } + @Override + public boolean hasGamepiece(){ + return isGamepieceDetected; } @Override From 046226c94a22da4730dfcba709db7231c156433e Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Sun, 26 Jan 2025 16:33:13 -0500 Subject: [PATCH 039/110] elevator code stuff --- simgui-ds.json | 6 +++ src/main/java/frc/robot/Constants.java | 6 --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../robot/subsystems/elevator/Elevator.java | 2 +- .../elevator/ElevatorConstants.java | 14 +++-- .../robot/subsystems/elevator/ElevatorIO.java | 22 ++++---- .../subsystems/elevator/ElevatorIOReal.java | 53 +++++++++---------- .../subsystems/elevator/ElevatorIOSim.java | 21 ++++---- 8 files changed, 68 insertions(+), 60 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0c4bb8b..eb7d9c5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,10 +7,4 @@ public static enum Mode { SIM, REPLAY } - - - - public static final class CurrentLimits { - public static final int elevator = 30; - } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fb5d651..18672aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,7 +36,9 @@ public RobotContainer() { private void configureBindings() { CommandXboxController xboxController = new CommandXboxController(0); - xboxController.x().onTrue(Commands.run(() -> elevator.setMovingSpeedRPM(0, 0))); + xboxController.leftTrigger().onTrue(Commands.run(() -> elevator.setMovingSpeedRPM(0, 0))); + xboxController.rightTrigger().onTrue(Commands.run(() -> elevator.setMovingSpeedRPM(0,0))); + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 6be6b1f..3ece4a2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -29,7 +29,7 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kD, ElevatorConstants.constraints); - private ElevatorFeedforward feedforward = new ElevatorFeedforward(rightMovingSpeed, maxDistance, leftMovingSpeed); + private ElevatorFeedforward feedforward = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg, ElevatorConstants.kv); public static Elevator getInstance(){ diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 22c5da0..9fce6b7 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -2,12 +2,18 @@ public final class ElevatorConstants{ public static final double maxDistance = 0.7112; - public static double constraints; //figure this out later - public static double kD; //figure this out later - public static double kI; //figure this out later - public static double kP; //figure this out later + public static double constraints = 1; //figure this out later + public static double kD = 1; //figure this out later + public static double kI = 1; //figure this out later + public static double kP = 1; //figure this out later public static double rightDeviceID = 1; public static double leftDeviceID = 2; + public static double ks = 0; + public static double kg = 0; + public static double kv = 0; + + + public static final int elevatorCurrentLimits = 20; //figure this out later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 1551dc3..a20c46d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -5,19 +5,23 @@ public interface ElevatorIO { @AutoLog public static class ElevatorIOInputs { - public double ElevatorInputVolts = 0.0; - public double ElevatorAppliedVolts = 0.0; - public double ElevatorPositionRads = 0.0; - public double ElevatorVelocityRads = 0.0; - public double ElevatorCurrent = 0.0; + public double elevatorInputVolts = 0.0; + public double elevatorAppliedVolts = 0.0; + public double elevatorPositionMetersPerSecond = 0.0; + public double elevatorVelocityMetersPerSecond = 0.0; + public double elevatorCurrent = 0.0; - public double ElevatorMotorVelocityRadPerSec = 0.0; - public double ElevatorMotorPositionRads = 0.0; + public double elevatorMotorVelocityMetersPerSecond = 0.0; + public double elevatorMotorPositionMeters = 0.0; } - public default void updateInputs(ElevatorIOInputs inputs) {} + public default void updateInputs(ElevatorIOInputs inputs) { + + } - public default void moveElevator(double speed) {} + public default void moveElevator(double speed) { + + } public default void setVoltage(double volts) {} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index a705fd2..0d0d41b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -1,17 +1,21 @@ package frc.robot.subsystems.elevator; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.revrobotics.AbsoluteEncoder; - +import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import edu.wpi.first.units.measure.AngularVelocity; + +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkFlexConfig; -import frc.robot.Constants.CurrentLimits; - - import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.motorcontrol.Spark; public class ElevatorIOReal implements ElevatorIO { @@ -19,38 +23,31 @@ public class ElevatorIOReal implements ElevatorIO { private SparkFlex rightElevatorMotor = new SparkFlex(1, MotorType.kBrushless); private SparkFlex leftElevatorMotor = new SparkFlex(2, MotorType.kBrushless); - - leftElevatorMotor.setSmartCurrentLimit(CurrentLimits.elevator); - rightElevatorMotor.setSmartCurrentLimit(CurrentLimits.elevator); - - + private SparkFlexConfig config= new SparkFlexConfig(); + private double inputVolts = 0.0; - final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); + final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); - - private boolean isEnabled; - private boolean hasPlayed = false; + private boolean isEnabled; + private boolean hasPlayed = false; + + + public ElevatorIOReal(){ + + config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); + leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - private SparkFlexConfig config(double kV) { - var SparkFlexConfig = new SparkFlexConfig(); + leftElevatorMotor.clearFaults(); + rightElevatorMotor.clearFaults(); +} - return SparkFlexConfig; - } + @Override public void updateInputs(ElevatorIOInputs inputs) { + } - - - public ElevatorIOReal(){ - - leftElevatorMotor.setControl(new Follower(rightElevatorMotor.getDeviceID(), true)); - - - } - - - } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 5dede45..33a3559 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -20,26 +20,25 @@ public class ElevatorIOSim implements ElevatorIO { @Override public void updateInputs(ElevatorIOInputs inputs) { - elevatorSim.update(ElevatorConstants.kDt); - inputs.elevatorPositionRads = elevatorSim.getAngleRads(); - inputs.elevatorMotorPositionRads = elevatorSim.getAngleRads(); - inputs.elevatorMotorVelocityRadPerSec = elevatorSim.getVelocityRadPerSec(); + elevatorSim.update(0.02); + inputs.elevatorPositionMetersPerSecond = elevatorSim.getPositionMeters(); + inputs.elevatorMotorPositionMeters = elevatorSim.getPositionMeters(); + inputs.elevatorMotorVelocityMetersPerSecond = elevatorSim.getVelocityMetersPerSecond(); inputs.elevatorCurrent = elevatorSim.getCurrentDrawAmps(); inputs.elevatorAppliedVolts = elevatorAppliedVolts; +} @Override public void moveElevator(double speed) { - ElevatorAppliedVolts = MathUtil.clamp(speed, speed, speed); - elevatorSim.setInputVoltage(elevatorAppliedVolts) - + elevatorAppliedVolts = MathUtil.clamp(speed, -1, 1); + elevatorSim.setInputVoltage(elevatorAppliedVolts); } @Override public void setVoltage(double volts){ - ElevatorAppliedVolts = volts; - Logger.recordOutput("Setting output", volts); - elevatorSim.setInputVoltage(elevatorAppliedVolts - (voltage, voltage, voltage)); + elevatorAppliedVolts = volts; + // Logger.recordOutput("Setting output", volts); + elevatorSim.setInputVoltage(volts); } - } } \ No newline at end of file From a7ebee08c37b84057451cfaa98c1f3bd606e1e51 Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Sun, 26 Jan 2025 22:32:01 -0500 Subject: [PATCH 040/110] some cleaning up, still not updating some values like elevatorPosition Need to update elevator position and elevator velocity. --- .../elevator/ElevatorConstants.java | 7 +++-- .../subsystems/elevator/ElevatorIOReal.java | 31 +++++++++++++++---- .../subsystems/elevator/ElevatorIOSim.java | 7 +++-- 3 files changed, 34 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 9fce6b7..d4f0f82 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -6,11 +6,14 @@ public final class ElevatorConstants{ public static double kD = 1; //figure this out later public static double kI = 1; //figure this out later public static double kP = 1; //figure this out later - public static double rightDeviceID = 1; - public static double leftDeviceID = 2; + public static int rightDeviceID = 1; + public static int leftDeviceID = 2; public static double ks = 0; public static double kg = 0; public static double kv = 0; + + //change this later + public static double conversionFactor = 0.1; public static final int elevatorCurrentLimits = 20; //figure this out later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 0d0d41b..25e36fc 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.elevator; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.controls.VelocityVoltage; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.REVLibError; @@ -20,14 +22,17 @@ public class ElevatorIOReal implements ElevatorIO { - private SparkFlex rightElevatorMotor = new SparkFlex(1, MotorType.kBrushless); - private SparkFlex leftElevatorMotor = new SparkFlex(2, MotorType.kBrushless); + private SparkFlex rightElevatorMotor = new SparkFlex(ElevatorConstants.rightDeviceID, MotorType.kBrushless); + private SparkFlex leftElevatorMotor = new SparkFlex(ElevatorConstants.leftDeviceID, MotorType.kBrushless); + + private RelativeEncoder leftEncoder = leftElevatorMotor.getEncoder(); + private RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder(); - private SparkFlexConfig config= new SparkFlexConfig(); + private SparkFlexConfig config = new SparkFlexConfig(); private double inputVolts = 0.0; - final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); + //final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); private boolean isEnabled; private boolean hasPlayed = false; @@ -44,10 +49,24 @@ public ElevatorIOReal(){ rightElevatorMotor.clearFaults(); } - + @Override + public void setVoltage(double voltage) { + inputVolts = voltage; + leftElevatorMotor.setVoltage(voltage); + rightElevatorMotor.setVoltage(-voltage); + } @Override public void updateInputs(ElevatorIOInputs inputs) { - + //might want to separate by motor or you can average if they don't need to be ran in reverse + inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); + inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); + + inputs.elevatorMotorPositionMeters = (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2; + inputs.elevatorMotorVelocityMetersPerSecond = (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2; + inputs.elevatorInputVolts = inputVolts; + + Logger.recordOutput("Elevator/Left Motor", leftEncoder.getPosition()); + Logger.recordOutput("Elevator/Right Motor", rightEncoder.getPosition()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 33a3559..746455a 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.elevator; -import java.lang.System.Logger; + +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -30,14 +31,14 @@ public void updateInputs(ElevatorIOInputs inputs) { @Override public void moveElevator(double speed) { - elevatorAppliedVolts = MathUtil.clamp(speed, -1, 1); + elevatorAppliedVolts = MathUtil.clamp(speed * 12, -12, 12); elevatorSim.setInputVoltage(elevatorAppliedVolts); } @Override public void setVoltage(double volts){ elevatorAppliedVolts = volts; - // Logger.recordOutput("Setting output", volts); + Logger.recordOutput("Setting output", volts); elevatorSim.setInputVoltage(volts); } From c2f671ddb3662a28c3f986c33d0edd25806a3fad Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Mon, 27 Jan 2025 15:49:47 -0500 Subject: [PATCH 041/110] basic moving command --- simgui-ds.json | 3 +++ src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 7 ++++--- .../frc/robot/subsystems/elevator/Elevator.java | 12 ++++++++---- .../subsystems/elevator/ElevatorConstants.java | 16 ++++++++-------- .../subsystems/elevator/ElevatorIOReal.java | 16 +++++++++++----- .../robot/subsystems/elevator/ElevatorIOSim.java | 4 +++- 7 files changed, 38 insertions(+), 22 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..172c4e4 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -93,6 +93,9 @@ { "guid": "78696e70757401000000000000000000", "useGamepad": true + }, + { + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eb7d9c5..752c1dd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,7 +1,7 @@ package frc.robot; public class Constants { - public static Mode currentMode = Mode.REAL; + public static Mode currentMode = Mode.SIM; public static enum Mode { REAL, SIM, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18672aa..487bc56 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,9 +35,10 @@ public RobotContainer() { private void configureBindings() { - CommandXboxController xboxController = new CommandXboxController(0); - xboxController.leftTrigger().onTrue(Commands.run(() -> elevator.setMovingSpeedRPM(0, 0))); - xboxController.rightTrigger().onTrue(Commands.run(() -> elevator.setMovingSpeedRPM(0,0))); + CommandXboxController xboxController = new CommandXboxController(1); + xboxController.leftTrigger().onTrue(Commands.run(() -> elevator.moveElevator(xboxController.getLeftTriggerAxis()/2))); + xboxController.leftTrigger().onTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis()/2))); + } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 3ece4a2..1e4a702 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -53,16 +53,20 @@ private Elevator(ElevatorIO elevatorIO){ } - public void setMovingSpeedRPM(double leftSpeed, double rightSpeed){ - leftMovingSpeed = leftSpeed; - rightMovingSpeed = rightSpeed; - } + // public void setMovingSpeedRPM(double leftSpeed, double rightSpeed){ + // leftMovingSpeed = leftSpeed; + // rightMovingSpeed = rightSpeed; + // } public void setGoal(double goal){ } + public void moveElevator(double speed) { + io.moveElevator(speed); + } + public double getTruePosition() { return getPosition(); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index d4f0f82..d35250b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -1,22 +1,22 @@ package frc.robot.subsystems.elevator; public final class ElevatorConstants{ - public static final double maxDistance = 0.7112; - public static double constraints = 1; //figure this out later - public static double kD = 1; //figure this out later - public static double kI = 1; //figure this out later + public static final double maxDistance = 0.80; + public static double constraints = 0.2; //figure this out later + public static double kD = 0; //figure this out later + public static double kI = 0; //figure this out later public static double kP = 1; //figure this out later - public static int rightDeviceID = 1; - public static int leftDeviceID = 2; + public static int rightDeviceID = 9; + public static int leftDeviceID = 10; public static double ks = 0; public static double kg = 0; - public static double kv = 0; + public static double kv = 1; //change this later public static double conversionFactor = 0.1; - public static final int elevatorCurrentLimits = 20; //figure this out later + public static final int elevatorCurrentLimits = 40; //figure this out later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 25e36fc..86fb7b3 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -41,10 +41,10 @@ public class ElevatorIOReal implements ElevatorIO { public ElevatorIOReal(){ config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); - - leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - + config.follow(ElevatorConstants.rightDeviceID); + leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + leftElevatorMotor.clearFaults(); rightElevatorMotor.clearFaults(); } @@ -52,8 +52,7 @@ public ElevatorIOReal(){ @Override public void setVoltage(double voltage) { inputVolts = voltage; - leftElevatorMotor.setVoltage(voltage); - rightElevatorMotor.setVoltage(-voltage); + rightElevatorMotor.setVoltage(voltage); } @Override @@ -69,4 +68,11 @@ public void updateInputs(ElevatorIOInputs inputs) { Logger.recordOutput("Elevator/Left Motor", leftEncoder.getPosition()); Logger.recordOutput("Elevator/Right Motor", rightEncoder.getPosition()); } + + @Override + public void moveElevator(double speed) { + rightElevatorMotor.set(speed); + + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 746455a..96ae5e2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -9,12 +9,13 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; public class ElevatorIOSim implements ElevatorIO { - private ElevatorSim elevatorSim = new ElevatorSim(0, 0, null, 0, 0, false, 0, null); + private ElevatorSim elevatorSim = new ElevatorSim(0, 1, DCMotor.getNeoVortex(2), 0, 0.8, true, 0); private double elevatorAppliedVolts = 0.0; @@ -41,5 +42,6 @@ public void setVoltage(double volts){ Logger.recordOutput("Setting output", volts); elevatorSim.setInputVoltage(volts); } + } \ No newline at end of file From 0d2a9165aa5adc702a7bbb4563a0a01e1bab8c9c Mon Sep 17 00:00:00 2001 From: ColinH0 Date: Mon, 27 Jan 2025 18:36:33 -0500 Subject: [PATCH 042/110] CAN IDs+ small config changes --- .../robot/subsystems/drive/ModuleIOTalonFX.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 9564195..3221bdb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -68,28 +68,28 @@ public ModuleIOTalonFX(int index) { break; case 1: // Front right - driveTalonFX = new TalonFX(10); - turnSparkMax = new SparkFlex(9, MotorType.kBrushless); + driveTalonFX = new TalonFX(3); + turnSparkMax = new SparkFlex(4, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); break; case 2: // Back left - driveTalonFX = new TalonFX(4); - turnSparkMax = new SparkFlex(6, MotorType.kBrushless); + driveTalonFX = new TalonFX(8); + turnSparkMax = new SparkFlex(7, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); driveTalonFX.setInverted(false); break; case 3: // Back right - driveTalonFX = new TalonFX(7); - turnSparkMax = new SparkFlex(8, MotorType.kBrushless); + driveTalonFX = new TalonFX(6); + turnSparkMax = new SparkFlex(5, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); break; default: throw new RuntimeException("Invalid module index"); } - //final TalonFXConfiguration drivingConfig = new TalonFXConfiguration(); + final TalonFXConfiguration drivingConfig = new TalonFXConfiguration(); //turnSparkMax.restoreFactoryDefaults(); //turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians @@ -107,8 +107,8 @@ public ModuleIOTalonFX(int index) { turnSparkMax.setInverted(isTurnMotorInverted); - driveTalonFX.setPosition(0); driveTalonFX.setInverted(true); + driveTalonFX.setPosition(0); turnRelativeEncoder.setPosition(0.0); encoderconfig.measurementPeriod(10); From 2c4366f91998b3b739bf22f52eb782ddbead0abf Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Mon, 27 Jan 2025 18:45:57 -0500 Subject: [PATCH 043/110] elevator stuff --- simgui-ds.json | 4 --- .../robot/subsystems/elevator/Elevator.java | 3 +- .../elevator/ElevatorConstants.java | 6 +++- .../elevator/ElevatorController.java | 0 .../robot/subsystems/elevator/ElevatorIO.java | 1 + .../subsystems/elevator/ElevatorIOReal.java | 21 ++++++++++++++ .../subsystems/elevator/ElevatorIOSim.java | 28 +++++++++++++------ 7 files changed, 48 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorController.java diff --git a/simgui-ds.json b/simgui-ds.json index 172c4e4..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,10 +90,6 @@ } ], "robotJoysticks": [ - { - "guid": "78696e70757401000000000000000000", - "useGamepad": true - }, { "guid": "Keyboard0" } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 1e4a702..232f1e9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -26,8 +26,7 @@ public class Elevator extends SubsystemBase { private PIDController controller = new PIDController( ElevatorConstants.kP, ElevatorConstants.kI, - ElevatorConstants.kD, - ElevatorConstants.constraints); + ElevatorConstants.kD); private ElevatorFeedforward feedforward = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg, ElevatorConstants.kv); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index d35250b..2881047 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -1,8 +1,12 @@ package frc.robot.subsystems.elevator; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; + public final class ElevatorConstants{ public static final double maxDistance = 0.80; - public static double constraints = 0.2; //figure this out later + // public static double constraints = 0.2; //figure this out later + public static final Constraints constraints = new Constraints(4, 5); //change later + public static double kD = 0; //figure this out later public static double kI = 0; //figure this out later public static double kP = 1; //figure this out later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorController.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorController.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index a20c46d..411c16b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -13,6 +13,7 @@ public static class ElevatorIOInputs { public double elevatorMotorVelocityMetersPerSecond = 0.0; public double elevatorMotorPositionMeters = 0.0; + public boolean openLoop = false; } public default void updateInputs(ElevatorIOInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 86fb7b3..c103bf8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -7,6 +7,10 @@ import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.AngularVelocity; import com.revrobotics.spark.SparkBase; @@ -28,6 +32,15 @@ public class ElevatorIOReal implements ElevatorIO { private RelativeEncoder leftEncoder = leftElevatorMotor.getEncoder(); private RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder(); + + private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(0, 0, 0); + private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 1); + private final PIDController controller = new PIDController(1, 0, 0); + private final TrapezoidProfile profile = new TrapezoidProfile(constraints); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + + + private SparkFlexConfig config = new SparkFlexConfig(); private double inputVolts = 0.0; @@ -55,9 +68,15 @@ public void setVoltage(double voltage) { rightElevatorMotor.setVoltage(voltage); } + @Override public void updateInputs(ElevatorIOInputs inputs) { //might want to separate by motor or you can average if they don't need to be ran in reverse + + if(!inputs.openLoop){ + setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); + setpoint = profile.calculate(0.1, setpoint, setpoint); + } inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); @@ -67,6 +86,8 @@ public void updateInputs(ElevatorIOInputs inputs) { Logger.recordOutput("Elevator/Left Motor", leftEncoder.getPosition()); Logger.recordOutput("Elevator/Right Motor", rightEncoder.getPosition()); + + } @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 96ae5e2..1ebce80 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,27 +1,40 @@ package frc.robot.subsystems.elevator; - -import org.littletonrobotics.junction.Logger; +import java.lang.System.Logger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; - +import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class ElevatorIOSim implements ElevatorIO { - private ElevatorSim elevatorSim = new ElevatorSim(0, 1, DCMotor.getNeoVortex(2), 0, 0.8, true, 0); + private ElevatorSim elevatorSim = new ElevatorSim(0, 0.1, DCMotor.getNeoVortex(2), 0, 0, false, 0); private double elevatorAppliedVolts = 0.0; +private final SimpleMotorFeedforward ffmodel = new SimpleMotorFeedforward(0, 0); +private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 1); +private final PIDController controller = new PIDController(1, 0, 0); +private final TrapezoidProfile profile = new TrapezoidProfile(constraints); +private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + + @Override public void updateInputs(ElevatorIOInputs inputs) { + +if(!inputs.openLoop){ + setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); + setpoint = profile.calculate(0.1, setpoint, setpoint); + } + elevatorSim.update(0.02); inputs.elevatorPositionMetersPerSecond = elevatorSim.getPositionMeters(); inputs.elevatorMotorPositionMeters = elevatorSim.getPositionMeters(); @@ -32,16 +45,15 @@ public void updateInputs(ElevatorIOInputs inputs) { @Override public void moveElevator(double speed) { - elevatorAppliedVolts = MathUtil.clamp(speed * 12, -12, 12); + elevatorAppliedVolts = MathUtil.clamp(speed, 0, 0); elevatorSim.setInputVoltage(elevatorAppliedVolts); } @Override public void setVoltage(double volts){ elevatorAppliedVolts = volts; - Logger.recordOutput("Setting output", volts); + // Logger.recordOutput("Setting output", volts); elevatorSim.setInputVoltage(volts); } - } \ No newline at end of file From 36891f5702d8f64b2280a20e0085fcb64c95d3d2 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Mon, 27 Jan 2025 19:13:41 -0500 Subject: [PATCH 044/110] added vortex intake code --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../subsystems/intake/IntakeConstants.java | 4 +- .../subsystems/intake/IntakeIORealVortex.java | 56 +++++++++++++++++++ 3 files changed, 60 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1c23eba..124f974 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIOReal; +import frc.robot.subsystems.intake.IntakeIORealVortex; import frc.robot.subsystems.intake.IntakeIOSim; public class RobotContainer { @@ -24,7 +25,7 @@ public RobotContainer() { switch (Constants.currentMode) { case REAL: - intake = Intake.initialize(new IntakeIOReal()); + intake = Intake.initialize(new IntakeIORealVortex()); break; case SIM: intake = Intake.initialize(new IntakeIOSim()); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 651e600..f48a53f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -4,12 +4,12 @@ public final class IntakeConstants { //PLACEHOLDERS - public static final int canID = 0; + public static final int canID = 18; public static final double simGearRatio = 0; public static final double overrideSpeed = -0.0; public static final double intakeCurrent = 0; - public static final double currentLimit = 30; + public static final int currentLimit = 30; public static final double currentThreshold = 20; //change later based on akit numbers for gamepiece } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java b/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java new file mode 100644 index 0000000..6f42156 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLimitSwitch; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + + +public class IntakeIORealVortex implements IntakeIO { + private final SparkFlex intakeMotor = new SparkFlex(IntakeConstants.canID, MotorType.kBrushless); + private final RelativeEncoder encoder = intakeMotor.getEncoder(); + + private SparkFlexConfig config = new SparkFlexConfig(); + + public IntakeIORealVortex() { + config.smartCurrentLimit(IntakeConstants.currentLimit); + config.idleMode(IdleMode.kCoast); + + intakeMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void updateInputs(IntakeIOInputs inputs) { + inputs.angularVelocityRPM = encoder.getVelocity(); + inputs.angularPositionRot = encoder.getPosition(); + inputs.currentAmps = intakeMotor.getOutputCurrent(); + inputs.voltage = intakeMotor.getBusVoltage(); + } + + @Override + public boolean hasGamepiece() { + return intakeMotor.getOutputCurrent() > IntakeConstants.currentThreshold; + } + + @Override + public void setMotorSpeed(double speed) { + intakeMotor.set(-speed); + } + +} + From b6ee165ee602f893ad628aa59ee256404b3f2740 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Tue, 28 Jan 2025 16:14:23 -0500 Subject: [PATCH 045/110] arm physical constants --- .vscode/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 +++-- .../java/frc/robot/subsystems/arm/Arm.java | 17 +++++++---- .../robot/subsystems/arm/ArmConstants.java | 11 ++++--- .../java/frc/robot/subsystems/arm/ArmIO.java | 5 ++-- .../frc/robot/subsystems/arm/ArmIOReal.java | 9 +++--- .../frc/robot/subsystems/arm/ArmIOSim.java | 29 +++++++++++-------- 7 files changed, 49 insertions(+), 32 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 0df8b28..2c3170b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,5 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "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 7f60974..253c50e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,11 +30,13 @@ public RobotContainer() { } private void configureBindings() { - OIConstants.driverController.povUp().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(Math.PI / 2))); - OIConstants.driverController.povDown().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(0))); + if(OIConstants.driverController.isConnected()) { + OIConstants.driverController.povUp().onTrue(Arm.getInstance().setGoal(Math.PI / 2)); + OIConstants.driverController.povDown().onTrue(Arm.getInstance().setGoal(0)); + } } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return Arm.getInstance().setGoal(Math.PI / 2); } } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 9da7c4b..3ede6f2 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.arm; +import java.util.Arrays; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -8,11 +10,12 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { private ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - private boolean openLoop = true; private ArmIO io; private static Arm instance; @@ -28,13 +31,16 @@ public static Arm getInstance() { return instance; } - public void setGoal(double angle) { - openLoop = false; - io.setGoal(angle); + public Command setGoal(double angle) { + return Commands.runOnce(() -> setOpenLoop(false)).andThen(() -> io.setGoal(angle), this); + } + + public Command manualVoltage(double voltage) { + return Commands.startEnd(() -> io.setVoltage(voltage), () -> io.setVoltage(voltage), this); } public void setOpenLoop(boolean openLoop) { - this.openLoop = openLoop; + inputs.openLoop = openLoop; } public Pose3d getArmPose(){ @@ -47,7 +53,6 @@ public Pose3d getArmPose(){ @Override public void periodic() { Logger.recordOutput(getName() + "/Pose", getArmPose()); - this.inputs.openLoop = openLoop; io.updateInputs(inputs); Logger.processInputs(getName(), inputs); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 49d2bb0..8ec28e0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -3,15 +3,18 @@ public class ArmConstants { public static int CAN_ID = 10; public static int CURRENT_LIMIT = 40; + public static double kG = 1.338; + public static double kV = 1.01; + public static double kA = 0.05; + public static double kS = 0.1; - // need to be tuned public static class Sim { - public static double GEARING = 40; + public static double GEARING = 60; public static double MOI = 2.09670337984; - public static double LENGTH = 0.5; + public static double LENGTH = 0.6604; public static double MIN_ANGLE = -Math.PI / 6; public static double MAX_ANGLE = 2 * Math.PI / 3; - public static boolean GRAVITY = false; + public static boolean GRAVITY = true; public static double INIT_ANGLE = -Math.PI / 6; } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index ecdd43e..6dc6d1f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.arm; +import java.util.Arrays; + import org.littletonrobotics.junction.AutoLog; public interface ArmIO { @@ -10,7 +12,7 @@ public static class ArmIOInputs{ public double angularVelocity = 0.0; public double current = 0.0; public double goalAngle = 0.0; - public boolean openLoop = false; + public boolean openLoop = true; public double setpointVelocity = 0.0; } @@ -22,6 +24,5 @@ public default void setVoltage(double voltage) { } public default void setGoal(double angle) { - } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 649ec9a..3a99897 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -8,6 +8,7 @@ import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -17,8 +18,8 @@ public class ArmIOReal implements ArmIO { private SparkFlexConfig config = new SparkFlexConfig(); private AbsoluteEncoder armEncoder = armMotor.getAbsoluteEncoder(); - private final ArmFeedforward ffmodel = new ArmFeedforward(0, 0, 0); - private final PIDController controller = new PIDController(0.02, 0,0.00); + private final ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.kS, ArmConstants.kG, ArmConstants.kV, ArmConstants.kA); + private final PIDController controller = new PIDController(0.04, 0,0.00); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 2); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0); @@ -39,7 +40,7 @@ public void setGoal(double angle) { @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { - setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); + setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(inputs.angularVelocity, setpoint.velocity)); setpoint = profile.calculate(0.02, setpoint, goal); } @@ -50,6 +51,6 @@ public void updateInputs(ArmIOInputs inputs) { } public void setVoltage(double voltage) { - armMotor.setVoltage(-voltage); + armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 33786ed..4b7d274 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -1,5 +1,9 @@ package frc.robot.subsystems.arm; +import java.util.Arrays; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; @@ -17,41 +21,42 @@ public class ArmIOSim implements ArmIO { ArmConstants.Sim.GRAVITY, ArmConstants.Sim.INIT_ANGLE ); - private double voltage = 0; - private final SimpleMotorFeedforward ffmodel = new SimpleMotorFeedforward(0.04, 1.65); - private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 2); + private final ArmFeedforward ffmodel = new ArmFeedforward( + ArmConstants.kS, + ArmConstants.kG, + ArmConstants.kV, + ArmConstants.kA + ); + private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.5, 1); private final PIDController controller = new PIDController(0.02, 0,0.00); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); - private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE + 0.5, 0); + private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - - public ArmIOSim() { - - } + private double voltage = 0; @Override public void setVoltage(double voltage) { - armSim.setInputVoltage(voltage); + armSim.setInputVoltage(MathUtil.clamp(voltage, -12, 12)); this.voltage = voltage; } @Override public void setGoal(double angle) { - goal = new TrapezoidProfile.State(angle + 0.5, 0); + goal = new TrapezoidProfile.State(angle, 0); } @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { - setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(armSim.getVelocityRadPerSec(), setpoint.velocity)); setpoint = profile.calculate(0.02, setpoint, goal); + setVoltage(ffmodel.calculate(inputs.angularPosition, setpoint.velocity)); } inputs.angularPosition = armSim.getAngleRads(); inputs.angularVelocity = armSim.getVelocityRadPerSec(); inputs.current = armSim.getCurrentDrawAmps(); inputs.voltage = voltage; - inputs.goalAngle = goal.position - 0.5; + inputs.goalAngle = goal.position; inputs.setpointVelocity = setpoint.velocity; armSim.update(0.02); } From 65dde4202fb6044db7f42b63839daabf3a724132 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Tue, 28 Jan 2025 16:24:14 -0500 Subject: [PATCH 046/110] Arm conversion factors --- .../java/frc/robot/subsystems/arm/ArmConstants.java | 2 +- .../java/frc/robot/subsystems/arm/ArmIOReal.java | 12 +++++++++--- src/main/java/frc/robot/subsystems/arm/ArmIOSim.java | 3 --- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 8ec28e0..9b585d5 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -3,7 +3,7 @@ public class ArmConstants { public static int CAN_ID = 10; public static int CURRENT_LIMIT = 40; - public static double kG = 1.338; + public static double kG = 1.02; public static double kV = 1.01; public static double kA = 0.05; public static double kS = 0.1; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 3a99897..e56f5ed 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.arm; +import java.util.concurrent.atomic.AtomicReferenceFieldUpdater; + import com.revrobotics.AbsoluteEncoder; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.PersistMode; @@ -16,7 +18,7 @@ public class ArmIOReal implements ArmIO { private final SparkFlex armMotor = new SparkFlex(ArmConstants.CAN_ID, MotorType.kBrushless); private SparkFlexConfig config = new SparkFlexConfig(); - private AbsoluteEncoder armEncoder = armMotor.getAbsoluteEncoder(); + private AbsoluteEncoder armEncoder; private final ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.kS, ArmConstants.kG, ArmConstants.kV, ArmConstants.kA); private final PIDController controller = new PIDController(0.04, 0,0.00); @@ -28,8 +30,12 @@ public class ArmIOReal implements ArmIO { public ArmIOReal(){ config.smartCurrentLimit(ArmConstants.CURRENT_LIMIT); config.idleMode(IdleMode.kBrake); + config.absoluteEncoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); + config.absoluteEncoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60); armMotor.clearFaults(); armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + armEncoder = armMotor.getAbsoluteEncoder(); } @Override @@ -44,8 +50,8 @@ public void updateInputs(ArmIOInputs inputs) { setpoint = profile.calculate(0.02, setpoint, goal); } - inputs.angularPosition = armEncoder.getPosition() * 2 * Math.PI; - inputs.angularVelocity = armEncoder.getVelocity() * 2 * Math.PI / 60; + inputs.angularPosition = armEncoder.getPosition(); + inputs.angularVelocity = armEncoder.getVelocity(); inputs.current = armMotor.getOutputCurrent(); inputs.voltage = armMotor.getAppliedOutput(); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 4b7d274..7e0f663 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -1,11 +1,8 @@ package frc.robot.subsystems.arm; -import java.util.Arrays; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; From 45ac3764e34316a9d568105e9efdc9809bff2109 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Tue, 28 Jan 2025 16:30:04 -0500 Subject: [PATCH 047/110] Zero encoder method --- src/main/java/frc/robot/subsystems/arm/Arm.java | 6 +++++- src/main/java/frc/robot/subsystems/arm/ArmIO.java | 4 ++++ src/main/java/frc/robot/subsystems/arm/ArmIOReal.java | 8 +++++++- src/main/java/frc/robot/subsystems/arm/ArmIOSim.java | 5 +++++ 4 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 3ede6f2..f1fcc53 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -35,8 +35,12 @@ public Command setGoal(double angle) { return Commands.runOnce(() -> setOpenLoop(false)).andThen(() -> io.setGoal(angle), this); } + public void resetAbsoluteEncoder() { + io.resetAbsoluteEncoder(); + } + public Command manualVoltage(double voltage) { - return Commands.startEnd(() -> io.setVoltage(voltage), () -> io.setVoltage(voltage), this); + return Commands.runOnce(() -> setOpenLoop(true)).andThen(Commands.startEnd(() -> io.setVoltage(voltage), () -> io.setVoltage(voltage), this)); } public void setOpenLoop(boolean openLoop) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 6dc6d1f..6507987 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -19,6 +19,10 @@ public static class ArmIOInputs{ public default void updateInputs(ArmIOInputs inputs) { } + + public default void resetAbsoluteEncoder() { + } + public default void setVoltage(double voltage) { } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index e56f5ed..2b5d2a8 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -3,6 +3,7 @@ import java.util.concurrent.atomic.AtomicReferenceFieldUpdater; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -18,7 +19,7 @@ public class ArmIOReal implements ArmIO { private final SparkFlex armMotor = new SparkFlex(ArmConstants.CAN_ID, MotorType.kBrushless); private SparkFlexConfig config = new SparkFlexConfig(); - private AbsoluteEncoder armEncoder; + private SparkAbsoluteEncoder armEncoder; private final ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.kS, ArmConstants.kG, ArmConstants.kV, ArmConstants.kA); private final PIDController controller = new PIDController(0.04, 0,0.00); @@ -43,6 +44,11 @@ public void setGoal(double angle) { goal = new TrapezoidProfile.State(angle, 0); } + @Override + public void resetAbsoluteEncoder() { + armMotor.getEncoder().setPosition(0); + } + @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 7e0f663..bcfcb85 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -37,6 +37,11 @@ public void setVoltage(double voltage) { this.voltage = voltage; } + @Override + public void resetAbsoluteEncoder() { + armSim.setState(0, armSim.getVelocityRadPerSec()); + } + @Override public void setGoal(double angle) { goal = new TrapezoidProfile.State(angle, 0); From cb6f31d455a3cd5260922916bae7ed9b832ee329 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Tue, 28 Jan 2025 16:38:42 -0500 Subject: [PATCH 048/110] arm CANID --- src/main/java/frc/robot/subsystems/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 9b585d5..d53df69 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.arm; public class ArmConstants { - public static int CAN_ID = 10; + public static int CAN_ID = 14; public static int CURRENT_LIMIT = 40; public static double kG = 1.02; public static double kV = 1.01; From 5a6c21c5a979267af205217a316757c349c620a9 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Tue, 28 Jan 2025 16:44:19 -0500 Subject: [PATCH 049/110] soft limits --- src/main/java/frc/robot/subsystems/arm/ArmConstants.java | 6 ++++-- src/main/java/frc/robot/subsystems/arm/ArmIOReal.java | 7 +++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index d53df69..b7c1068 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.arm; +import edu.wpi.first.math.util.Units; + public class ArmConstants { public static int CAN_ID = 14; public static int CURRENT_LIMIT = 40; @@ -12,8 +14,8 @@ public static class Sim { public static double GEARING = 60; public static double MOI = 2.09670337984; public static double LENGTH = 0.6604; - public static double MIN_ANGLE = -Math.PI / 6; - public static double MAX_ANGLE = 2 * Math.PI / 3; + public static double MIN_ANGLE = Units.degreesToRadians(-95); + public static double MAX_ANGLE = Units.degreesToRadians(55); public static boolean GRAVITY = true; public static double INIT_ANGLE = -Math.PI / 6; } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 2b5d2a8..03a5a76 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -33,10 +33,17 @@ public ArmIOReal(){ config.idleMode(IdleMode.kBrake); config.absoluteEncoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); config.absoluteEncoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60); + config.encoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); + config.encoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60); + config.softLimit.forwardSoftLimitEnabled(true); + config.softLimit.reverseSoftLimitEnabled(false); + config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); + config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); armMotor.clearFaults(); armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); armEncoder = armMotor.getAbsoluteEncoder(); + armMotor.getEncoder().setPosition(armEncoder.getPosition()); } @Override From b1213307a73448cf68bcaeb16835bfca7c789014 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Tue, 28 Jan 2025 16:56:57 -0500 Subject: [PATCH 050/110] pigeon CAN ID --- src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java index 3e48653..9833a62 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java @@ -12,7 +12,7 @@ import edu.wpi.first.units.measure.AngularVelocity; public class GyroIOPigeon implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(20); + private final Pigeon2 pigeon = new Pigeon2(9); private final StatusSignal yaw = pigeon.getYaw(); private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); From 9df7c95265bb67cf328299b707577a36e5f13501 Mon Sep 17 00:00:00 2001 From: GooseJuice898 Date: Tue, 28 Jan 2025 17:45:12 -0500 Subject: [PATCH 051/110] zeroing, soft limits, inverted follower motor, separately logs velocity and position, and conversion factors --- simgui-ds.json | 4 ++ src/main/java/frc/robot/RobotContainer.java | 4 +- .../robot/subsystems/elevator/Elevator.java | 2 + .../elevator/ElevatorConstants.java | 10 ++--- .../elevator/ElevatorController.java | 0 .../subsystems/elevator/ElevatorIOReal.java | 38 +++++++++++++++++-- .../subsystems/elevator/ElevatorIOSim.java | 3 +- 7 files changed, 50 insertions(+), 11 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorController.java diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..15cec71 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -92,6 +92,10 @@ "robotJoysticks": [ { "guid": "Keyboard0" + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 487bc56..35ed529 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,8 +36,8 @@ public RobotContainer() { private void configureBindings() { CommandXboxController xboxController = new CommandXboxController(1); - xboxController.leftTrigger().onTrue(Commands.run(() -> elevator.moveElevator(xboxController.getLeftTriggerAxis()/2))); - xboxController.leftTrigger().onTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis()/2))); + xboxController.leftTrigger().whileTrue(Commands.run(() -> elevator.moveElevator(xboxController.getLeftTriggerAxis()/2))); + xboxController.leftTrigger().whileTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis()/2))); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 232f1e9..12685c4 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -2,6 +2,7 @@ import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.controller.ElevatorFeedforward; @@ -36,6 +37,7 @@ public static Elevator getInstance(){ } public void periodic () { + Logger.processInputs(getName(), inputs); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 2881047..383685c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -10,17 +10,17 @@ public final class ElevatorConstants{ public static double kD = 0; //figure this out later public static double kI = 0; //figure this out later public static double kP = 1; //figure this out later - public static int rightDeviceID = 9; - public static int leftDeviceID = 10; + public static int rightDeviceID = 13; + public static int leftDeviceID = 12; public static double ks = 0; - public static double kg = 0; - public static double kv = 1; + public static double kg = 0.16; + public static double kv = 15.96; //IS THIS RIGHT?!?!? //change this later public static double conversionFactor = 0.1; - public static final int elevatorCurrentLimits = 40; //figure this out later + public static final int elevatorCurrentLimits = 60; //might need to adjust later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorController.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorController.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index c103bf8..44b47e1 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularVelocity; import com.revrobotics.spark.SparkBase; @@ -32,6 +33,7 @@ public class ElevatorIOReal implements ElevatorIO { private RelativeEncoder leftEncoder = leftElevatorMotor.getEncoder(); private RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder(); + private AbsoluteEncoder leftAbsoluteEncoder = leftElevatorMotor.getAbsoluteEncoder(); private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(0, 0, 0); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 1); @@ -40,6 +42,8 @@ public class ElevatorIOReal implements ElevatorIO { private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + private double minPositionMeters = 0.0; + private double maxPositionMeters = 0.7112; private SparkFlexConfig config = new SparkFlexConfig(); @@ -49,6 +53,13 @@ public class ElevatorIOReal implements ElevatorIO { private boolean isEnabled; private boolean hasPlayed = false; + + //zero stuff + public void zeroElevator(){ + double AbsolutePosition = leftAbsoluteEncoder.getPosition(); + leftEncoder.setPosition(0); + rightEncoder.setPosition(0); + } public ElevatorIOReal(){ @@ -56,6 +67,9 @@ public ElevatorIOReal(){ config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); config.follow(ElevatorConstants.rightDeviceID); + config.inverted(true); + config.absoluteEncoder.positionConversionFactor(Units.inchesToMeters(11 / 8)); + config.absoluteEncoder.velocityConversionFactor(Units.inchesToMeters(11 / 8) / 60); leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); leftElevatorMotor.clearFaults(); @@ -64,6 +78,12 @@ public ElevatorIOReal(){ @Override public void setVoltage(double voltage) { + double currentPosition = leftEncoder.getPosition(); // Relative encoder position + if (currentPosition <= minPositionMeters && voltage < 0) { + voltage = 0; // Prevent moving below the minimum position + } else if (currentPosition >= maxPositionMeters && voltage > 0) { + voltage = 0; // Prevent moving above the maximum position + } inputVolts = voltage; rightElevatorMotor.setVoltage(voltage); } @@ -80,8 +100,10 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); - inputs.elevatorMotorPositionMeters = (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2; - inputs.elevatorMotorVelocityMetersPerSecond = (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2; + //inputs.elevatorMotorPositionMeters = (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2; + inputs.elevatorMotorPositionMeters = leftAbsoluteEncoder.getPosition(); + //inputs.elevatorMotorVelocityMetersPerSecond = (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2; + inputs.elevatorMotorVelocityMetersPerSecond = leftAbsoluteEncoder.getVelocity(); inputs.elevatorInputVolts = inputVolts; Logger.recordOutput("Elevator/Left Motor", leftEncoder.getPosition()); @@ -92,8 +114,18 @@ public void updateInputs(ElevatorIOInputs inputs) { @Override public void moveElevator(double speed) { + + double currentPosition = leftEncoder.getPosition(); + if ((currentPosition <= minPositionMeters && speed < 0) || + (currentPosition >= maxPositionMeters && speed > 0)) { + speed = 0; + } rightElevatorMotor.set(speed); + } + public void setSoftLimits(double min, double max) { + this.minPositionMeters = min; + this.maxPositionMeters = max; + } } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 1ebce80..89c5af8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -45,7 +45,8 @@ public void updateInputs(ElevatorIOInputs inputs) { @Override public void moveElevator(double speed) { - elevatorAppliedVolts = MathUtil.clamp(speed, 0, 0); + System.out.println("hi"); + elevatorAppliedVolts = 12 * speed; elevatorSim.setInputVoltage(elevatorAppliedVolts); } From ec3561de09a4ad55f8373c6aad241fd7f211b8ab Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Thu, 30 Jan 2025 17:26:12 -0500 Subject: [PATCH 052/110] stuff --- .../java/frc/robot/subsystems/elevator/Elevator.java | 12 ++++++++++-- .../frc/robot/subsystems/elevator/ElevatorIO.java | 3 +++ .../robot/subsystems/elevator/ElevatorIOReal.java | 3 ++- .../frc/robot/subsystems/elevator/ElevatorIOSim.java | 4 +++- 4 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 12685c4..7a9cc4c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -8,6 +8,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public class Elevator extends SubsystemBase { @@ -60,8 +63,9 @@ private Elevator(ElevatorIO elevatorIO){ // } - public void setGoal(double goal){ - + public Commands setGoal(double goal){ + + return Commands.runOnce(() -> setOpenLoop(false)).andThen(() -> io.setGoal(goal), this); } public void moveElevator(double speed) { @@ -89,8 +93,12 @@ public void reset() { } + @Override + public void setGoal(double angle) { + goal = new TrapezoidProfile.State(goal, 0); } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 411c16b..83aa426 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -16,6 +16,9 @@ public static class ElevatorIOInputs { public boolean openLoop = false; } + + public + public default void updateInputs(ElevatorIOInputs inputs) { } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 44b47e1..48c285c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -40,6 +40,7 @@ public class ElevatorIOReal implements ElevatorIO { private final PIDController controller = new PIDController(1, 0, 0); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + private TrapezoidProfile.State goal = new TrapezoidProfile.State(); private double minPositionMeters = 0.0; @@ -95,7 +96,7 @@ public void updateInputs(ElevatorIOInputs inputs) { if(!inputs.openLoop){ setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - setpoint = profile.calculate(0.1, setpoint, setpoint); + setpoint = profile.calculate(0.1, setpoint, setGoal); } inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 89c5af8..ae68970 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -25,6 +25,8 @@ public class ElevatorIOSim implements ElevatorIO { private final PIDController controller = new PIDController(1, 0, 0); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); +private TrapezoidProfile.State goal = new TrapezoidProfile.State(); + @Override @@ -32,7 +34,7 @@ public void updateInputs(ElevatorIOInputs inputs) { if(!inputs.openLoop){ setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - setpoint = profile.calculate(0.1, setpoint, setpoint); + setpoint = profile.calculate(0.1, setpoint, setGoal); } elevatorSim.update(0.02); From fff5584d11e063a46e6950527157cc38eac0fff6 Mon Sep 17 00:00:00 2001 From: Arenacloserr <162641455+Arenacloserr@users.noreply.github.com> Date: Thu, 30 Jan 2025 17:31:30 -0500 Subject: [PATCH 053/110] Intake game piece detection --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/Main.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 10 ++++++++ .../frc/robot/subsystems/intake/Intake.java | 23 +++++++++++++++++++ 4 files changed, 36 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dd50fe3..df407bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,6 +6,7 @@ public final class Constants { public static final class IntakeSetpoints{ //placeholders public static final double intake = -0.85; + public static final double slow = -0.4; public static final double reverse = 0.7; } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fe215d7..234b9e2 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -9,6 +9,8 @@ public final class Main { private Main() {} + public String Bruh = "Bruh"; + public static void main(String... args) { RobotBase.startRobot(Robot::new); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 124f974..3bc1633 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,6 +42,16 @@ private void configureBindings() { ()->intake.setIntakeSpeed(0), intake)); m_driverController.b().whileTrue(Commands.startEnd(()->intake.setIntakeSpeed(-1), ()->intake.setIntakeSpeed(0), intake)); + + + m_driverController.leftBumper() + .whileTrue( + Commands.sequence( + intake.intake().until(() -> intake.GamePieceInitial()), + intake.intakeSlow().until(() -> intake.GamePeiceFinal()), + Commands.run(() -> intake.setIntakeSpeed(0)) + ) + ); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 403c0d1..6bc2e17 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,7 +1,9 @@ package frc.robot.subsystems.intake; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeSetpoints; import frc.robot.subsystems.intake.IntakeIO.IntakeIOInputs; @@ -35,13 +37,34 @@ public void setIntakeSpeed(double speed){ intakeSpeed = speed; } + /* + public Command intakeS() { + return startEnd(() -> setIntakeSpeed(IntakeSetpoints.intake), () -> setIntakeSpeed(0)).until(() -> GamePieceInitial()).andThen(() -> intakeSlow().until(() -> GamePeiceFinal())); + return startEnd(() -> setIntakeSpeed(IntakeSetpoints.intake), () -> setIntakeSpeed(0)).until(() -> GamePieceInitial()); + } + */ + public Command intake() { return startEnd(() -> setIntakeSpeed(IntakeSetpoints.intake), () -> setIntakeSpeed(0)); } + public Command intakeSlow() { + return startEnd(() -> setIntakeSpeed(IntakeSetpoints.slow), () -> setIntakeSpeed(0)); + } + public Command reverse() { return startEnd(() -> setIntakeSpeed(IntakeSetpoints.reverse), () -> setIntakeSpeed(0)); } + + + public boolean GamePieceInitial() { + return 0 >= inputs.currentAmps; + } + + public boolean GamePeiceFinal() { + return IntakeConstants.currentThreshold >= inputs.currentAmps; + } + public boolean hasGamepiece() { return io.hasGamepiece(); } From 30b6de15ade2dd62564aa4e34909be5fa87fb082 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Thu, 30 Jan 2025 17:38:08 -0500 Subject: [PATCH 054/110] stuff --- .../robot/subsystems/elevator/Elevator.java | 19 ++++++++++--------- .../robot/subsystems/elevator/ElevatorIO.java | 7 +++++-- .../subsystems/elevator/ElevatorIOSim.java | 7 +++---- 3 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 7a9cc4c..64eb33f 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -5,7 +5,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.controller.ElevatorFeedforward; - +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -39,11 +39,6 @@ public static Elevator getInstance(){ return instance; } - public void periodic () { - Logger.processInputs(getName(), inputs); - - } - public static Elevator initialize(ElevatorIO io){ if(instance == null){ instance = new Elevator(io); @@ -68,6 +63,9 @@ public Commands setGoal(double goal){ return Commands.runOnce(() -> setOpenLoop(false)).andThen(() -> io.setGoal(goal), this); } + + } + public void moveElevator(double speed) { io.moveElevator(speed); } @@ -92,10 +90,13 @@ public void reset() { this.setGoal(getPosition()); } - @Override - public void setGoal(double angle) { - goal = new TrapezoidProfile.State(goal, 0); + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); + + } + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 83aa426..3c607ed 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -17,11 +17,14 @@ public static class ElevatorIOInputs { } - public - + public default void updateInputs(ElevatorIOInputs inputs) { } + + public default void setGoal(double goal) { + + } public default void moveElevator(double speed) { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index ae68970..a8e080d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -15,12 +15,12 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class ElevatorIOSim implements ElevatorIO { - private ElevatorSim elevatorSim = new ElevatorSim(0, 0.1, DCMotor.getNeoVortex(2), 0, 0, false, 0); + private ElevatorSim elevatorSim = new ElevatorSim(ElevatorConstants.kv, 1, DCMotor.getNeoVortex(2), 0, 0.7112, true, 0); private double elevatorAppliedVolts = 0.0; -private final SimpleMotorFeedforward ffmodel = new SimpleMotorFeedforward(0, 0); +private final SimpleMotorFeedforward ffmodel = new SimpleMotorFeedforward(ElevatorConstants.ks, ElevatorConstants.kv); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 1); private final PIDController controller = new PIDController(1, 0, 0); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); @@ -37,17 +37,16 @@ public void updateInputs(ElevatorIOInputs inputs) { setpoint = profile.calculate(0.1, setpoint, setGoal); } - elevatorSim.update(0.02); inputs.elevatorPositionMetersPerSecond = elevatorSim.getPositionMeters(); inputs.elevatorMotorPositionMeters = elevatorSim.getPositionMeters(); inputs.elevatorMotorVelocityMetersPerSecond = elevatorSim.getVelocityMetersPerSecond(); inputs.elevatorCurrent = elevatorSim.getCurrentDrawAmps(); inputs.elevatorAppliedVolts = elevatorAppliedVolts; + elevatorSim.update(0.02); } @Override public void moveElevator(double speed) { - System.out.println("hi"); elevatorAppliedVolts = 12 * speed; elevatorSim.setInputVoltage(elevatorAppliedVolts); } From aaac26997f1817145fb6c10c3ba94e26bab3bf54 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Thu, 30 Jan 2025 18:01:35 -0500 Subject: [PATCH 055/110] stuff --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/elevator/Elevator.java | 11 +++++------ .../frc/robot/subsystems/elevator/ElevatorIO.java | 4 ++-- .../frc/robot/subsystems/elevator/ElevatorIOReal.java | 2 +- .../frc/robot/subsystems/elevator/ElevatorIOSim.java | 10 ++++++++-- 5 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 35ed529..1dc2a98 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,7 +37,7 @@ public RobotContainer() { private void configureBindings() { CommandXboxController xboxController = new CommandXboxController(1); xboxController.leftTrigger().whileTrue(Commands.run(() -> elevator.moveElevator(xboxController.getLeftTriggerAxis()/2))); - xboxController.leftTrigger().whileTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis()/2))); + xboxController.rightTrigger().whileTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis()/2))); } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 64eb33f..63320dc 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; public class Elevator extends SubsystemBase { @@ -58,15 +57,17 @@ private Elevator(ElevatorIO elevatorIO){ // } - public Commands setGoal(double goal){ + public Command setGoal(double goal){ return Commands.runOnce(() -> setOpenLoop(false)).andThen(() -> io.setGoal(goal), this); } - + public void setOpenLoop(boolean openLoop) { + inputs.openLoop = openLoop; } public void moveElevator(double speed) { + setOpenLoop(true); io.moveElevator(speed); } @@ -97,9 +98,7 @@ public void periodic() { } - } - -} + } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 3c607ed..26aebbb 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -13,11 +13,11 @@ public static class ElevatorIOInputs { public double elevatorMotorVelocityMetersPerSecond = 0.0; public double elevatorMotorPositionMeters = 0.0; - public boolean openLoop = false; + public boolean openLoop = true; } - + public default void updateInputs(ElevatorIOInputs inputs) { } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 48c285c..eb83267 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -96,7 +96,7 @@ public void updateInputs(ElevatorIOInputs inputs) { if(!inputs.openLoop){ setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - setpoint = profile.calculate(0.1, setpoint, setGoal); + setpoint = profile.calculate(0.1, setpoint, goal); } inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index a8e080d..65c5c45 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -34,7 +34,7 @@ public void updateInputs(ElevatorIOInputs inputs) { if(!inputs.openLoop){ setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - setpoint = profile.calculate(0.1, setpoint, setGoal); + setpoint = profile.calculate(0.02, setpoint, goal); } inputs.elevatorPositionMetersPerSecond = elevatorSim.getPositionMeters(); @@ -57,5 +57,11 @@ public void setVoltage(double volts){ // Logger.recordOutput("Setting output", volts); elevatorSim.setInputVoltage(volts); } + +@Override +public void setGoal(double height) { + goal = new TrapezoidProfile.State(height, 0.1); -} \ No newline at end of file +} + + } \ No newline at end of file From da5aa9246bac62558576e2f96388d9396c47703a Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Thu, 30 Jan 2025 18:23:22 -0500 Subject: [PATCH 056/110] more stuff --- .../java/frc/robot/subsystems/elevator/Elevator.java | 9 +++++++++ .../robot/subsystems/elevator/ElevatorIOReal.java | 12 ++++++++++-- .../frc/robot/subsystems/elevator/ElevatorIOSim.java | 1 + 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 63320dc..be4c9f2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.elevator; +import java.io.DataOutput; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -22,6 +24,13 @@ public class Elevator extends SubsystemBase { @AutoLogOutput private double rightMovingSpeed = 0.0; + @AutoLogOutput + private double setGoal = 0.0; + + @AutoLogOutput + private double setPoint = 0.0; + + private double maxDistance = ElevatorConstants.maxDistance; private static Elevator instance; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index eb83267..2d9ecfa 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -100,12 +100,20 @@ public void updateInputs(ElevatorIOInputs inputs) { } inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); + - //inputs.elevatorMotorPositionMeters = (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2; inputs.elevatorMotorPositionMeters = leftAbsoluteEncoder.getPosition(); - //inputs.elevatorMotorVelocityMetersPerSecond = (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2; inputs.elevatorMotorVelocityMetersPerSecond = leftAbsoluteEncoder.getVelocity(); inputs.elevatorInputVolts = inputVolts; + Logger.recordOutput("Elevator/Setpoint/Position", setpoint.position); + Logger.recordOutput("Elevator/Setpoint/Velocity", setpoint.velocity); + + setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); + setpoint = profile.calculate(0.1, setpoint, goal); + + // log goal after updating + Logger.recordOutput("Elevator/Goal/Position", goal.position); + Logger.recordOutput("Elevator/Goal/Velocity", goal.velocity); Logger.recordOutput("Elevator/Left Motor", leftEncoder.getPosition()); Logger.recordOutput("Elevator/Right Motor", rightEncoder.getPosition()); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 65c5c45..3f8b7d0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -43,6 +43,7 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.elevatorCurrent = elevatorSim.getCurrentDrawAmps(); inputs.elevatorAppliedVolts = elevatorAppliedVolts; elevatorSim.update(0.02); + } @Override From ccc7c0587fa713952bf15eedceec1bed45a906ad Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Sat, 1 Feb 2025 10:55:56 -0500 Subject: [PATCH 057/110] got rid of absolute encoder --- .../subsystems/elevator/ElevatorIOReal.java | 31 ++++++++++++------- 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 2d9ecfa..419af5a 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -33,7 +33,7 @@ public class ElevatorIOReal implements ElevatorIO { private RelativeEncoder leftEncoder = leftElevatorMotor.getEncoder(); private RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder(); - private AbsoluteEncoder leftAbsoluteEncoder = leftElevatorMotor.getAbsoluteEncoder(); + // private AbsoluteEncoder leftAbsoluteEncoder = leftElevatorMotor.getAbsoluteEncoder(); private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(0, 0, 0); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 1); @@ -57,7 +57,7 @@ public class ElevatorIOReal implements ElevatorIO { //zero stuff public void zeroElevator(){ - double AbsolutePosition = leftAbsoluteEncoder.getPosition(); + // double AbsolutePosition = leftAbsoluteEncoder.getPosition(); leftEncoder.setPosition(0); rightEncoder.setPosition(0); } @@ -67,10 +67,14 @@ public ElevatorIOReal(){ config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - config.follow(ElevatorConstants.rightDeviceID); + config.follow(ElevatorConstants.rightDeviceID, true); config.inverted(true); - config.absoluteEncoder.positionConversionFactor(Units.inchesToMeters(11 / 8)); - config.absoluteEncoder.velocityConversionFactor(Units.inchesToMeters(11 / 8) / 60); + + config.encoder.positionConversionFactor(Units.inchesToMeters(11/18)); + config.encoder.velocityConversionFactor(Units.inchesToMeters(11/18)/60); + + // config.absoluteEncoder.positionConversionFactor(Units.inchesToMeters(11 / 18)); use these later + // config.absoluteEncoder.velocityConversionFactor(Units.inchesToMeters(11 / 18) / 60); use these later leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); leftElevatorMotor.clearFaults(); @@ -81,9 +85,9 @@ public ElevatorIOReal(){ public void setVoltage(double voltage) { double currentPosition = leftEncoder.getPosition(); // Relative encoder position if (currentPosition <= minPositionMeters && voltage < 0) { - voltage = 0; // Prevent moving below the minimum position + voltage = 0; } else if (currentPosition >= maxPositionMeters && voltage > 0) { - voltage = 0; // Prevent moving above the maximum position + voltage = 0; } inputVolts = voltage; rightElevatorMotor.setVoltage(voltage); @@ -96,20 +100,23 @@ public void updateInputs(ElevatorIOInputs inputs) { if(!inputs.openLoop){ setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - setpoint = profile.calculate(0.1, setpoint, goal); + setpoint = profile.calculate(0.02, setpoint, goal); } inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); - - inputs.elevatorMotorPositionMeters = leftAbsoluteEncoder.getPosition(); - inputs.elevatorMotorVelocityMetersPerSecond = leftAbsoluteEncoder.getVelocity(); + inputs.elevatorMotorPositionMeters = leftEncoder.getPosition(); + inputs.elevatorMotorPositionMeters = rightEncoder.getPosition(); + inputs.elevatorMotorVelocityMetersPerSecond = leftEncoder.getVelocity(); + inputs.elevatorMotorVelocityMetersPerSecond = rightEncoder.getVelocity(); + // inputs.elevatorMotorPositionMeters = leftAbsoluteEncoder.getPosition(); + // inputs.elevatorMotorVelocityMetersPerSecond = leftAbsoluteEncoder.getVelocity(); will use absolute encoder later inputs.elevatorInputVolts = inputVolts; Logger.recordOutput("Elevator/Setpoint/Position", setpoint.position); Logger.recordOutput("Elevator/Setpoint/Velocity", setpoint.velocity); setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - setpoint = profile.calculate(0.1, setpoint, goal); + setpoint = profile.calculate(0.02, setpoint, goal); // log goal after updating Logger.recordOutput("Elevator/Goal/Position", goal.position); From 282a2d04fb5143b0c6899f927c18f77ff7ff225c Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Sat, 1 Feb 2025 10:56:31 -0500 Subject: [PATCH 058/110] Code cleanup and variables change --- .../robot/subsystems/drive/ModuleIOSim.java | 27 ++++++++++--------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 73c332e..a0ad003 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -8,34 +8,35 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; - //change gearbox argument - private DCMotorSim driveSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 0.025, DriveConstants.DriveGearing), DCMotor.getKrakenX60(4)); //needs to change - private DCMotorSim turnSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 0.004, DriveConstants.TurnGearing), DCMotor.getNeoVortex(4)); //needs to change - - private final Rotation2d turnAbsoluteInitPositon = new Rotation2d(Math.random()*2.0*Math.PI); + // change gearbox argument + private DCMotorSim driveSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 0.025, DriveConstants.DriveGearing), + DCMotor.getKrakenX60(4)); // needs to change + private DCMotorSim turnSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 0.004, DriveConstants.TurnGearing), + DCMotor.getNeoVortex(4)); // needs to change + + private final Rotation2d turnAbsoluteInitPositon = new Rotation2d(Math.random() * 2.0 * Math.PI); private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; @Override - public void updateInputs(ModuleIOInputs inputs){ + public void updateInputs(ModuleIOInputs inputs) { driveSim.update(LOOP_PERIOD_SECS); turnSim.update(LOOP_PERIOD_SECS); inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; - - - inputs.turnAbsolutePosition = - new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPositon); + inputs.driveCurrentAmps = new double[] { Math.abs(driveSim.getCurrentDrawAmps()) }; + inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPositon); inputs.turnPosition = new Rotation2d(turnSim.getAngularPosition()); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turnCurrentAmps = new double[] { Math.abs(turnSim.getCurrentDrawAmps()) }; } @Override - public void setDriveVoltage(double volts){ + public void setDriveVoltage(double volts) { driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); driveSim.setInputVoltage(driveAppliedVolts); } From 7f3f053a56d94957d695658526f1f2b4742eecd3 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Sat, 1 Feb 2025 11:37:30 -0500 Subject: [PATCH 059/110] binded buttons --- src/main/java/frc/robot/Constants.java | 4 +-- src/main/java/frc/robot/RobotContainer.java | 12 +++++++- .../elevator/ElevatorConstants.java | 29 +++++++++++++++++++ .../frc/robot/subsystems/intake/Intake.java | 4 +-- .../subsystems/intake/IntakeConstants.java | 2 ++ 5 files changed, 46 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index df407bb..b42f957 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,8 +5,8 @@ public final class Constants { public static final class IntakeSetpoints{ //placeholders - public static final double intake = -0.85; - public static final double slow = -0.4; + public static final double intake = -0.6; + public static final double slow = -0.3; public static final double reverse = 0.7; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3bc1633..240472d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,8 @@ public class RobotContainer { private final CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); + + private final CommandXboxController m_operatorController = new CommandXboxController(1); public RobotContainer() { @@ -44,7 +46,7 @@ private void configureBindings() { ()->intake.setIntakeSpeed(0), intake)); - m_driverController.leftBumper() + m_operatorController.povUp() .whileTrue( Commands.sequence( intake.intake().until(() -> intake.GamePieceInitial()), @@ -52,6 +54,14 @@ private void configureBindings() { Commands.run(() -> intake.setIntakeSpeed(0)) ) ); + + m_operatorController.x().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(-0.5), + () -> intake.setIntakeSpeed(0), intake)); + + m_operatorController.a().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(0.5), + () -> intake.setIntakeSpeed(0), intake)); + + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..5915802 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; + +public final class ElevatorConstants{ + public static final double maxDistance = 0.80; + // public static double constraints = 0.2; //figure this out later + public static final Constraints constraints = new Constraints(4, 5); //ch e later + + public static double kD = 0; //figure this out later + public static double kI = 0; //figure this out later + public static double kP = 1; //figure this out later + public static int rightDeviceID = 13; + public static int leftDeviceID = 12; + public static double ks = 0; + public static double kg = 0.16; + public static double kv = 15.96; //IS THIS RIGHT?!?!? + + //change this later + public static double conversionFactor = 0.1; + + + public static final int elevatorCurrentLimits = 60; //might need to adjust later + + + + + +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 6bc2e17..25ebce4 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -58,11 +58,11 @@ public Command reverse() { public boolean GamePieceInitial() { - return 0 >= inputs.currentAmps; + return IntakeConstants.initialThreshold <= inputs.currentAmps; } public boolean GamePeiceFinal() { - return IntakeConstants.currentThreshold >= inputs.currentAmps; + return IntakeConstants.currentThreshold <= inputs.currentAmps; } public boolean hasGamepiece() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f48a53f..1fca7c3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -12,4 +12,6 @@ public final class IntakeConstants { public static final double intakeCurrent = 0; public static final int currentLimit = 30; public static final double currentThreshold = 20; //change later based on akit numbers for gamepiece + + public static final double initialThreshold = 10; } From adf024c3541ebc26a0d4238f52fa14053c44eaac Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sat, 1 Feb 2025 13:16:22 -0500 Subject: [PATCH 060/110] Switched to void methods instead of commands in Arm.java --- src/main/java/frc/robot/RobotContainer.java | 24 ++++++++++++++----- .../java/frc/robot/subsystems/arm/Arm.java | 10 ++++---- .../java/frc/robot/subsystems/arm/ArmIO.java | 1 - .../frc/robot/subsystems/arm/ArmIOReal.java | 3 ++- 4 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 253c50e..2ddb4bf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,14 @@ package frc.robot; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmIOReal; import frc.robot.subsystems.arm.ArmIOSim; +import frc.robot.subsystems.arm.ArmConstants; public class RobotContainer { public RobotContainer() { @@ -30,13 +31,24 @@ public RobotContainer() { } private void configureBindings() { - if(OIConstants.driverController.isConnected()) { - OIConstants.driverController.povUp().onTrue(Arm.getInstance().setGoal(Math.PI / 2)); - OIConstants.driverController.povDown().onTrue(Arm.getInstance().setGoal(0)); - } + OIConstants.driverController.povUp().whileTrue( + Commands.startEnd( + () -> Arm.getInstance().manualVoltage(6), + () -> Arm.getInstance().manualVoltage(0), + Arm.getInstance() + ) + ); + + OIConstants.driverController.povDown().whileTrue( + Commands.startEnd( + () -> Arm.getInstance().manualVoltage(-6), + () -> Arm.getInstance().manualVoltage(0), + Arm.getInstance() + ) + ); } public Command getAutonomousCommand() { - return Arm.getInstance().setGoal(Math.PI / 2); + return Commands.runOnce(() -> Arm.getInstance().manualVoltage(ArmConstants.kG)); } } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index f1fcc53..978bfec 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -31,16 +31,18 @@ public static Arm getInstance() { return instance; } - public Command setGoal(double angle) { - return Commands.runOnce(() -> setOpenLoop(false)).andThen(() -> io.setGoal(angle), this); + public void setGoal(double angle) { + setOpenLoop(false); + io.setGoal(angle); } public void resetAbsoluteEncoder() { io.resetAbsoluteEncoder(); } - public Command manualVoltage(double voltage) { - return Commands.runOnce(() -> setOpenLoop(true)).andThen(Commands.startEnd(() -> io.setVoltage(voltage), () -> io.setVoltage(voltage), this)); + public void manualVoltage(double voltage) { + setOpenLoop(true); + io.setVoltage(voltage); } public void setOpenLoop(boolean openLoop) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 6507987..892cdc8 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -3,7 +3,6 @@ import java.util.Arrays; import org.littletonrobotics.junction.AutoLog; - public interface ArmIO { @AutoLog public static class ArmIOInputs{ diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 03a5a76..b636925 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -33,6 +33,7 @@ public ArmIOReal(){ config.idleMode(IdleMode.kBrake); config.absoluteEncoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); config.absoluteEncoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60); + config.absoluteEncoder.inverted(true); config.encoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); config.encoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60); config.softLimit.forwardSoftLimitEnabled(true); @@ -59,7 +60,7 @@ public void resetAbsoluteEncoder() { @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { - setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(inputs.angularVelocity, setpoint.velocity)); + setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position)); setpoint = profile.calculate(0.02, setpoint, goal); } From 692047a1f35de0cc0a43771d3fb888ed9099f743 Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Sun, 2 Feb 2025 13:29:39 -0500 Subject: [PATCH 061/110] merge clean up --- src/main/java/frc/robot/RobotContainer.java | 108 +++++++++----------- 1 file changed, 47 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d7fd153..9fc753b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,85 +27,71 @@ public class RobotContainer { Elevator elevator; - - private Drive drive; - - public RobotContainer() throws IOException, ParseException { - switch (Constants.currentMode){ + private Drive drive; + + public RobotContainer() throws IOException, ParseException { + switch (Constants.currentMode) { case REAL: - elevator = Elevator.initialize(new ElevatorIOReal()); - break; - case SIM: - elevator = Elevator.initialize(new ElevatorIOSim()); - break; - case REPLAY: - elevator = Elevator.initialize(new ElevatorIOReal()); - break; - default: - } - - switch (Constants.currentMode) { - case REAL: - - drive = - new Drive( - new GyroIOPigeon(), + elevator = Elevator.initialize(new ElevatorIOReal()); + + drive = new Drive( + new GyroIOPigeon(), new ModuleIOTalonFX(0), - new ModuleIOTalonFX(1), - new ModuleIOTalonFX(2), + new ModuleIOTalonFX(1), + new ModuleIOTalonFX(2), new ModuleIOTalonFX(3)); - break; - + + break; case SIM: - - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - - default: - - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - + elevator = Elevator.initialize(new ElevatorIOSim()); + + drive = new Drive( + new GyroIO() { + }, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + break; - } + case REPLAY: + elevator = Elevator.initialize(new ElevatorIOReal()); + break; + default: + elevator = Elevator.initialize(new ElevatorIOSim()); + + drive = new Drive( + new GyroIO() { + }, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + } configureBindings(); } private void configureBindings() { drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> Constants.OIConstants.driverController.getLeftY(), - () -> Constants.OIConstants.driverController.getLeftX(), - () -> -Constants.OIConstants.driverController.getRightX() - )); - - Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(()-> drive.zeroHeading(), drive)); + DriveCommands.joystickDrive( + drive, + () -> Constants.OIConstants.driverController.getLeftY(), + () -> Constants.OIConstants.driverController.getLeftX(), + () -> -Constants.OIConstants.driverController.getRightX())); - } + Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); - - - private void configureBindings() { CommandXboxController xboxController = new CommandXboxController(1); - xboxController.leftTrigger().whileTrue(Commands.run(() -> elevator.moveElevator(xboxController.getLeftTriggerAxis()/2))); - xboxController.rightTrigger().whileTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis()/2))); - + xboxController.leftTrigger() + .whileTrue(Commands.run(() -> elevator.moveElevator(xboxController.getLeftTriggerAxis() / 2))); + xboxController.rightTrigger() + .whileTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis() / 2))); } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } From de5ddc4bcb8009e36c11e324bd198382be1c3c76 Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Sun, 2 Feb 2025 13:59:45 -0500 Subject: [PATCH 062/110] forgot to commit arm commands --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ebdece..68a400f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.subsystems.elevator.ElevatorIO; import frc.robot.subsystems.elevator.ElevatorIOReal; import frc.robot.subsystems.elevator.ElevatorIOSim; +import frc.robot.Constants.OIConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIOReal; @@ -140,6 +141,21 @@ private void configureBindings() { Constants.OIConstants.operatorController.a().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(0.5), () -> intake.setIntakeSpeed(0), intake)); + OIConstants.driverController.povUp().whileTrue( + Commands.startEnd( + () -> Arm.getInstance().manualVoltage(6), + () -> Arm.getInstance().manualVoltage(0), + Arm.getInstance() + ) + ); + + OIConstants.driverController.povDown().whileTrue( + Commands.startEnd( + () -> Arm.getInstance().manualVoltage(-6), + () -> Arm.getInstance().manualVoltage(0), + Arm.getInstance() + ) + ); } public Command getAutonomousCommand() { From be8c786791dc632d1b66f50222be51e7542f18e7 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Mon, 3 Feb 2025 10:12:57 -0500 Subject: [PATCH 063/110] Constants change --- .../subsystems/drive/DriveConstants.java | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 7eaae0e..402e4a4 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -3,22 +3,26 @@ import edu.wpi.first.math.util.Units; public class DriveConstants { + public static final double MAX_LINEAR_SPEED = Units.metersToFeet(5); public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); - public static final double DRIVE_BASE_RADIUS = - Math.hypot(DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0); - public static final double MAX_ANGULAR_SPEED = Units.feetToMeters(DriveConstants.MAX_LINEAR_SPEED) / DRIVE_BASE_RADIUS; - public static final double TranslationKP = 5.0; + public static final double DRIVE_BASE_RADIUS = Math.hypot(DriveConstants.TRACK_LENGTH / 2.0, + DriveConstants.TRACK_WIDTH / 2.0); + public static final double MAX_ANGULAR_SPEED = Units.feetToMeters(DriveConstants.MAX_LINEAR_SPEED) + / DRIVE_BASE_RADIUS; + public static final double TranslationKS = 0.014; + public static final double TranslationKV = 0.134; + public static final double TranslationKP = 0.1; public static final double TranslationKI = 0.0; public static final double TranslationKD = 0.0; - public static final double RotationKP = 5.0; + public static final double RotationKP = 10.0; public static final double RotationKI = 0.0; public static final double RotationKD = 0.0; public static final double WHEEL_RADIUS = Units.inchesToMeters(1.5); - public static final double DriveGearing = 13.371; - public static final double TurnGearing = 6.75; - public static final int turnCurrentLimit = 60; //May need to change later -} + public static final double TurnGearing = 13.3714; + public static final double DriveGearing = 6.11; + public static final int turnCurrentLimit = 60; // May need to change later + public static final int driveCurrentLimit = 60; - +} From 0a80f3b78ca838812bffb2e55381f870e53f19ca Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Mon, 3 Feb 2025 11:06:16 -0500 Subject: [PATCH 064/110] Switch ffmodel based on gamepiece --- src/main/java/frc/robot/Constants.java | 6 +++++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/arm/Arm.java | 15 +++++++++++++ .../robot/subsystems/arm/ArmConstants.java | 22 +++++++++++++++---- .../java/frc/robot/subsystems/arm/ArmIO.java | 4 ++++ .../frc/robot/subsystems/arm/ArmIOReal.java | 8 ++++++- .../frc/robot/subsystems/arm/ArmIOSim.java | 12 +++++----- 7 files changed, 57 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 96b7af7..270d4ab 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,4 +13,10 @@ public static enum Mode { public static class OIConstants { public static final CommandXboxController driverController = new CommandXboxController(0); } + + public static enum Gamepiece { + ALGAE, + CORAL, + NONE, + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2ddb4bf..267ae3c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,6 +49,6 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return Commands.runOnce(() -> Arm.getInstance().manualVoltage(ArmConstants.kG)); + return Commands.runOnce(() -> Arm.getInstance().manualVoltage(ArmConstants.DEFAULTkG)); } } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 978bfec..07ab2ac 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.Gamepiece; public class Arm extends SubsystemBase { private ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); @@ -56,6 +57,20 @@ public Pose3d getArmPose(){ ); } + public void setFFMode(Gamepiece gamepieceType) { + switch (gamepieceType) { + case CORAL: + io.setFFValues(ArmConstants.CORALkS, ArmConstants.CORALkG, ArmConstants.CORALkV, ArmConstants.CORALkA); + break; + case ALGAE: + io.setFFValues(ArmConstants.ALGAEkS, ArmConstants.ALGAEkG, ArmConstants.CORALkV, ArmConstants.CORALkA); + break; + case NONE: + io.setFFValues(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); + break; + } + } + @Override public void periodic() { Logger.recordOutput(getName() + "/Pose", getArmPose()); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index b7c1068..6b8964b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -5,10 +5,24 @@ public class ArmConstants { public static int CAN_ID = 14; public static int CURRENT_LIMIT = 40; - public static double kG = 1.02; - public static double kV = 1.01; - public static double kA = 0.05; - public static double kS = 0.1; + + // Default FF + public static double DEFAULTkG = 1.02; + public static double DEFAULTkV = 1.01; + public static double DEFAULTkA = 0.05; + public static double DEFAULTkS = 0.1; + + // Algae FF + public static double ALGAEkG = 1.02; + public static double ALGAEkV = 1.01; + public static double ALGAEkA = 0.05; + public static double ALGAEkS = 0.1; + + // Default FF + public static double CORALkG = 1.02; + public static double CORALkV = 1.01; + public static double CORALkA = 0.05; + public static double CORALkS = 0.1; public static class Sim { public static double GEARING = 60; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 892cdc8..0e25798 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -28,4 +28,8 @@ public default void setVoltage(double voltage) { public default void setGoal(double angle) { } + + public default void setFFValues(double kS, double kG, double kV, double kA) { + + } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index b636925..a2754e0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -21,7 +21,7 @@ public class ArmIOReal implements ArmIO { private SparkFlexConfig config = new SparkFlexConfig(); private SparkAbsoluteEncoder armEncoder; - private final ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.kS, ArmConstants.kG, ArmConstants.kV, ArmConstants.kA); + private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); private final PIDController controller = new PIDController(0.04, 0,0.00); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 2); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); @@ -52,6 +52,11 @@ public void setGoal(double angle) { goal = new TrapezoidProfile.State(angle, 0); } + @Override + public void setFFValues(double kS, double kG, double kV, double kA) { + ffmodel = new ArmFeedforward(kS, kG, kV); + } + @Override public void resetAbsoluteEncoder() { armMotor.getEncoder().setPosition(0); @@ -60,6 +65,7 @@ public void resetAbsoluteEncoder() { @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { + // setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(inputs.angularVelocity, setpoint.velocity)); setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position)); setpoint = profile.calculate(0.02, setpoint, goal); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index bcfcb85..55db7e3 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -18,12 +18,7 @@ public class ArmIOSim implements ArmIO { ArmConstants.Sim.GRAVITY, ArmConstants.Sim.INIT_ANGLE ); - private final ArmFeedforward ffmodel = new ArmFeedforward( - ArmConstants.kS, - ArmConstants.kG, - ArmConstants.kV, - ArmConstants.kA - ); + private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.5, 1); private final PIDController controller = new PIDController(0.02, 0,0.00); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); @@ -47,6 +42,11 @@ public void setGoal(double angle) { goal = new TrapezoidProfile.State(angle, 0); } + @Override + public void setFFValues(double kS, double kG, double kV, double kA) { + ffmodel = new ArmFeedforward(kS, kG, kV, kA); + } + @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { From 381f5439501ebfecc278897b4ae776dcf78e9463 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Mon, 3 Feb 2025 11:18:37 -0500 Subject: [PATCH 065/110] fixed binding conflicts --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 778f455..7f25608 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -114,8 +114,9 @@ private void configureBindings() { .whileTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis() / 2))); - Constants.OIConstants.driverController.a().whileTrue(Commands.startEnd(()->intake.setIntakeSpeed(1), + Constants.OIConstants.driverController.x().whileTrue(Commands.startEnd(()->intake.setIntakeSpeed(1), ()->intake.setIntakeSpeed(0), intake)); + OIConstants.driverController.b().whileTrue(Commands.startEnd(()->intake.setIntakeSpeed(-1), ()->intake.setIntakeSpeed(0), intake)); From b51bcc240fa68ddd79070aee1f305715fb6c80cf Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Mon, 3 Feb 2025 12:30:02 -0500 Subject: [PATCH 066/110] Module adjustments --- .../subsystems/drive/ModuleIOTalonFX.java | 117 +++++++++--------- 1 file changed, 57 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 3221bdb..38cfb4e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -32,24 +32,28 @@ import edu.wpi.first.math.util.Units; /** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO + * Module IO implementation for SparkMax drive motor controller, SparkMax turn + * motor controller (NEO * or NEO 550), and analog absolute encoder connected to the RIO * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware + *

+ * NOTE: This implementation should be used as a starting point and adapted to + * different hardware * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the + *

+ * To calibrate the absolute encoder offsets, point the modules straight (such + * that forward + * motion on the drive motor will propel the robot forward) and copy the + * reported values from the * absolute encoders using AdvantageScope. These values are logged under * "/Drive/ModuleX/TurnAbsolutePositionRad" */ public class ModuleIOTalonFX implements ModuleIO { final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); - - private final TalonFX driveTalonFX; - private final SparkFlex turnSparkMax; + private final SparkFlex turnSparkFlex; private final RelativeEncoder turnRelativeEncoder; private final AbsoluteEncoder turnAbsoluteEncoder; @@ -57,47 +61,48 @@ public class ModuleIOTalonFX implements ModuleIO { private final boolean isTurnMotorInverted = true; public ModuleIOTalonFX(int index) { - + switch (index) { case 0: // Front left driveTalonFX = new TalonFX(1); - turnSparkMax = new SparkFlex(2, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + turnSparkFlex = new SparkFlex(2, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); driveTalonFX.setInverted(false); break; case 1: // Front right - driveTalonFX = new TalonFX(3); - turnSparkMax = new SparkFlex(4, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + driveTalonFX = new TalonFX(10); + turnSparkFlex = new SparkFlex(9, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); break; case 2: // Back left - driveTalonFX = new TalonFX(8); - turnSparkMax = new SparkFlex(7, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + driveTalonFX = new TalonFX(4); + turnSparkFlex = new SparkFlex(6, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); driveTalonFX.setInverted(false); break; case 3: // Back right - driveTalonFX = new TalonFX(6); - turnSparkMax = new SparkFlex(5, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); + driveTalonFX = new TalonFX(7); + turnSparkFlex = new SparkFlex(8, MotorType.kBrushless); + turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); break; default: throw new RuntimeException("Invalid module index"); } - - final TalonFXConfiguration drivingConfig = new TalonFXConfiguration(); - //turnSparkMax.restoreFactoryDefaults(); - //turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians - //turnAbsoluteEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); // Radians per second + // final TalonFXConfiguration drivingConfig = new TalonFXConfiguration(); + // turnSparkFlex.restoreFactoryDefaults(); + + // turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians + // turnAbsoluteEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); // + // Radians per second - turnSparkMax.setCANTimeout(250); + turnSparkFlex.setCANTimeout(250); - turnRelativeEncoder = turnSparkMax.getEncoder(); // ??? + turnRelativeEncoder = turnSparkFlex.getEncoder(); // ??? final ExternalEncoderConfig encoderconfig = new ExternalEncoderConfig(); encoderconfig.measurementPeriod(10); @@ -105,18 +110,16 @@ public ModuleIOTalonFX(int index) { encoderconfig.measurementPeriod(10); encoderconfig.averageDepth(2); - turnSparkMax.setInverted(isTurnMotorInverted); + turnSparkFlex.setInverted(isTurnMotorInverted); - driveTalonFX.setInverted(true); driveTalonFX.setPosition(0); + driveTalonFX.setInverted(true); - turnRelativeEncoder.setPosition(0.0); - encoderconfig.measurementPeriod(10); - encoderconfig.averageDepth(2); + - turnSparkMax.setCANTimeout(0); + turnSparkFlex.setCANTimeout(0); - //turnSparkMax.burnFlash(); + // turnSparkFlex.burnFlash(); var config = config(); var configSpark = configSpark(null); @@ -129,8 +132,9 @@ public ModuleIOTalonFX(int index) { REVLibError status2 = REVLibError.kError; // not work maybe for (int i = 0; i < 5; i++) { status = driveTalonFX.getConfigurator().apply(config); - turnSparkMax.configure(configSpark, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - if (status.isOK()) break; + turnSparkFlex.configure(configSpark, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + if (status.isOK()) + break; } if (!status.isOK()) { @@ -144,22 +148,20 @@ public ModuleIOTalonFX(int index) { @Override public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveTalonFX.getPosition().getValueAsDouble()) / DriveConstants.DriveGearing; - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble()) - / DriveConstants.DriveGearing; + inputs.drivePositionRad = Units.rotationsToRadians(driveTalonFX.getPosition().getValueAsDouble()) + / DriveConstants.DriveGearing; + inputs.driveVelocityRadPerSec = Units + .rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble()) + / DriveConstants.DriveGearing; inputs.driveAppliedVolts = driveTalonFX.getMotorVoltage().getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveTalonFX.getSupplyCurrent().getValueAsDouble()}; + inputs.driveCurrentAmps = new double[] { driveTalonFX.getSupplyCurrent().getValueAsDouble() }; inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition()); - inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / DriveConstants.TurnGearing); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / DriveConstants.TurnGearing; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + inputs.turnPosition = Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / DriveConstants.TurnGearing); + inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / DriveConstants.TurnGearing; + inputs.turnAppliedVolts = turnSparkFlex.getAppliedOutput() * turnSparkFlex.getBusVoltage(); + inputs.turnCurrentAmps = new double[] { turnSparkFlex.getOutputCurrent() }; } private SparkFlexConfig configSpark(IdleMode idle) { @@ -185,7 +187,7 @@ private TalonFXConfiguration config() { talonFXConfig.Slot0.kV = 0; talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfig.CurrentLimits.StatorCurrentLimit = 40; // CurrentLimits.drive; + talonFXConfig.CurrentLimits.StatorCurrentLimit = DriveConstants.driveCurrentLimit; // CurrentLimits.drive; talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -199,24 +201,19 @@ public void setDriveVoltage(double volts) { driveTalonFX.setVoltage(volts); } - @Override - public void runTalonPID(double desiredStateRotPerSecond) { - driveTalonFX.setControl(m_request.withVelocity(desiredStateRotPerSecond)); - } - @Override public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); + turnSparkFlex.setVoltage(volts); } - // @Override - // public void setDriveBrakeMode(boolean enable) { - // driveTalonFX.setBrakeMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - // } + // @Override + // public void setDriveBrakeMode(boolean enable) { + // driveTalonFX.setBrakeMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + // } @Override public void setTurnBrakeMode(boolean enable) { - // turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + // turnSparkFlex.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); configSpark(enable ? IdleMode.kBrake : IdleMode.kCoast); } } \ No newline at end of file From bdf7db713f57ceeb6b66e75e48a5492d2dfa2980 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Mon, 3 Feb 2025 13:51:59 -0500 Subject: [PATCH 067/110] Formatting --- .../frc/robot/subsystems/drive/Drive.java | 559 +++++++++--------- 1 file changed, 271 insertions(+), 288 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f62cf80..745795d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -39,7 +39,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; - public class Drive extends SubsystemBase { private static Drive instance; private static final SwerveDriveKinematics K_DRIVE_KINEMATICS = new SwerveDriveKinematics(getModuleTranslation2d()); @@ -48,28 +47,29 @@ public class Drive extends SubsystemBase { private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final Module[] modules = new Module[4]; private static SysIdRoutine sysId; - private final MutableMeasure m_appliedVoltage = new MutVoltage(0, 0, Volt); - private final MutableMeasure m_position = new MutAngle(0,0, Radian); - private final MutableMeasure m_velocity = new MutAngularVelocity(0, 0, RadiansPerSecond); + private final MutableMeasure m_appliedVoltage = new MutVoltage(0, 0, Volt); + private final MutableMeasure m_position = new MutAngle(0, 0, Radian); + private final MutableMeasure m_velocity = new MutAngularVelocity( + 0, 0, RadiansPerSecond); private static SwerveDriveKinematics Modulekinematics = new SwerveDriveKinematics(getModuleTranslation2d()); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslation2d()); private Rotation2d rawGyroRotation2d = new Rotation2d(); - private SwerveModulePosition[] lastModulePositions = - new SwerveModulePosition[] { + private SwerveModulePosition[] lastModulePositions = new SwerveModulePosition[] { new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition() - }; + }; - private SwerveDrivePoseEstimator poseEstimator = - new SwerveDrivePoseEstimator(kinematics, rawGyroRotation2d, lastModulePositions, new Pose2d()); - - public static Drive getInstance(){ + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation2d, + lastModulePositions, new Pose2d()); + + public static Drive getInstance() { return instance; } - public static Drive initialize (GyroIO gyro, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) throws IOException, ParseException { + public static Drive initialize(GyroIO gyro, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) + throws IOException, ParseException { if (instance == null) { instance = new Drive(gyro, fl, fr, bl, br); } @@ -77,361 +77,344 @@ public static Drive initialize (GyroIO gyro, ModuleIO fl, ModuleIO fr, ModuleIO } public Drive( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO) throws IOException, ParseException { - this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); - - RobotConfig config; - + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) throws IOException, ParseException { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + RobotConfig config; + + config = RobotConfig.fromGUISettings(); + + try { config = RobotConfig.fromGUISettings(); - - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); } - // } finally { - // config = null; // if something went horribly wrong - // } - - // Configure AutoBuilder last - AutoBuilder.configure( - this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD), // Translation PID constants - new PIDConstants(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD) // Rotation PID constants - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + } + // } finally { + // config = null; // if something went horribly wrong + // } + + // Configure AutoBuilder last + AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also + // optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for + // holonomic drive trains + new PIDConstants(DriveConstants.TranslationKP, DriveConstants.TranslationKI, + DriveConstants.TranslationKD), // Translation PID constants + new PIDConstants(DriveConstants.RotationKP, DriveConstants.RotationKI, + DriveConstants.RotationKD) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - } - - - - public void robotInit(){ - - - - - - //Pathfinding.setPathfinder(new LocalADStarAK()); // AK code needs to be added - PathPlannerLogging.setLogActivePathCallback( + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } + + public void robotInit() { + + // Pathfinding.setPathfinder(new LocalADStarAK()); // AK code needs to be added + PathPlannerLogging.setLogActivePathCallback( (activePath) -> { Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); }); - PathPlannerLogging.setLogTargetPoseCallback( + PathPlannerLogging.setLogTargetPoseCallback( (targetPose) -> { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); }); - sysId = - new SysIdRoutine( - new SysIdRoutine.Config(), - new SysIdRoutine.Mechanism( + sysId = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( (voltage) -> { - for (int i = 0; i < 4; i++){ + for (int i = 0; i < 4; i++) { modules[i].runCharacterization(voltage.in(Volts)); } }, log -> { log.motor("driveSparkMax") - .voltage(m_appliedVoltage.mut_replace(inputs.driveAppliedVolts, Volts)) - .angularPosition(m_position.mut_replace(inputs.drivePositionRad, Radians)) - .angularVelocity( - m_velocity.mut_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond)); - + .voltage(m_appliedVoltage.mut_replace(inputs.driveAppliedVolts, Volts)) + .angularPosition(m_position.mut_replace(inputs.drivePositionRad, Radians)) + .angularVelocity( + m_velocity.mut_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond)); + }, - this)); - } - - public void periodic() { - gyroIO.updateInputs(gyroInputs);; - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); - } + this)); + } + public void periodic() { + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); - if (DriverStation.isDisabled()){ - for (var module : modules){ + } + + if (DriverStation.isDisabled()) { + for (var module : modules) { module.stop(); } } - - if (DriverStation.isDisabled()){ - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - } - SwerveModulePosition[] modulePositions = getModulePositions(); - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex ++){ - moduleDeltas [moduleIndex] = - new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - } - - if (gyroInputs.connected) { - - rawGyroRotation2d = gyroInputs.yawPosition; - } else { + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } - Twist2d twist = K_DRIVE_KINEMATICS.toTwist2d(moduleDeltas); - rawGyroRotation2d = rawGyroRotation2d.plus(new Rotation2d(twist.dtheta)); - } - poseEstimator.update(rawGyroRotation2d, modulePositions); + SwerveModulePosition[] modulePositions = getModulePositions(); + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } + if (gyroInputs.connected) { -public void runVelocity(ChassisSpeeds speeds){ - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates,DriveConstants.MAX_LINEAR_SPEED); + rawGyroRotation2d = gyroInputs.yawPosition; + } else { - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + Twist2d twist = K_DRIVE_KINEMATICS.toTwist2d(moduleDeltas); + rawGyroRotation2d = rawGyroRotation2d.plus(new Rotation2d(twist.dtheta)); + } + poseEstimator.update(rawGyroRotation2d, modulePositions); } - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); -} - -public void stop(){ - runVelocity(new ChassisSpeeds()); -} + public void runVelocity(ChassisSpeeds speeds) { + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.MAX_LINEAR_SPEED); + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + public void stop() { + runVelocity(new ChassisSpeeds()); + } -public void stopWithx() { - Rotation2d[] headings = new Rotation2d[4]; - for (int i = 0; i < 4; i++) { - headings[i] = getModuleTranslation2d()[i].getAngle(); + public void stopWithx() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslation2d()[i].getAngle(); + } + Modulekinematics.resetHeadings(); + stop(); } - Modulekinematics.resetHeadings(); - stop(); -} public void zeroHeading() { - if (Constants.currentMode == frc.robot.Constants.Mode.SIM){ + if (Constants.currentMode == frc.robot.Constants.Mode.SIM) { poseEstimator.resetPosition( - new Rotation2d(), - new SwerveModulePosition[]{ - modules[0].getPosition(), - modules[1].getPosition(), - modules[2].getPosition(), - modules[3].getPosition(), - }, - getPose()); + new Rotation2d(), + new SwerveModulePosition[] { + modules[0].getPosition(), + modules[1].getPosition(), + modules[2].getPosition(), + modules[3].getPosition(), + }, + getPose()); } gyroIO.reset(); } - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } - @AutoLogOutput (key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates(){ - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++){ - states[i] = modules[i].getState(); - } - return states; + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); } + return states; + } - - private SwerveModulePosition[] getModulePositions(){ - SwerveModulePosition[] states = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++){ - states[i] = modules[i].getPosition(); - } - return states; - } - - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, 4.8); // DriveConstants.kMaxSpeedMetersPerSecond - for (int i = 0; i < 4; i++) { - modules[i].setDesiredState(desiredStates[i]); - } + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); } + return states; + } - @AutoLogOutput(key = "odometry/Robot") - public Pose2d getPose(){ - return poseEstimator.getEstimatedPosition(); + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, 4.8); // DriveConstants.kMaxSpeedMetersPerSecond + for (int i = 0; i < 4; i++) { + modules[i].setDesiredState(desiredStates[i]); } + } - public Rotation2d getRotation() { - return getPose().getRotation(); - } - + @AutoLogOutput(key = "odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation2d, getModulePositions(),pose); - } - - private ChassisSpeeds getRobotRelativeSpeeds(){ - return K_DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates()); - - } + public Rotation2d getRotation() { + return getPose().getRotation(); + } - public void driveRobotRelative(ChassisSpeeds speeds){ - SwerveModuleState[] moduleStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(speeds); - setModuleStates(moduleStates); - } + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation2d, getModulePositions(), pose); + } + private ChassisSpeeds getRobotRelativeSpeeds() { + return K_DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates()); - public void addVisionMeasurement(Pose2d visionPose, double timestamp){ - poseEstimator.addVisionMeasurement(visionPose, timestamp); - } + } - - - public double getMaxLinearSpeedMetersPerSec(){ - return DriveConstants.MAX_LINEAR_SPEED; - } + public void driveRobotRelative(ChassisSpeeds speeds) { + SwerveModuleState[] moduleStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(speeds); + setModuleStates(moduleStates); + } + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } - public double getMaxAngularSpeedRadPerSec(){ - return DriveConstants.MAX_ANGULAR_SPEED; - } + public double getMaxLinearSpeedMetersPerSec() { + return DriveConstants.MAX_LINEAR_SPEED; + } + public double getMaxAngularSpeedRadPerSec() { + return DriveConstants.MAX_ANGULAR_SPEED; + } - public static Translation2d[] getModuleTranslation2d(){ - return new Translation2d[]{ - new Translation2d(DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), - new Translation2d(DriveConstants.TRACK_LENGTH/2.0, -DriveConstants.TRACK_WIDTH/2.0), - new Translation2d(-DriveConstants.TRACK_LENGTH/2.0, DriveConstants.TRACK_WIDTH/2.0), - new Translation2d(-DriveConstants.TRACK_LENGTH/2.0, -DriveConstants.TRACK_WIDTH/2.0), - }; - } + public static Translation2d[] getModuleTranslation2d() { + return new Translation2d[] { + new Translation2d(DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0), + new Translation2d(DriveConstants.TRACK_LENGTH / 2.0, -DriveConstants.TRACK_WIDTH / 2.0), + new Translation2d(-DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0), + new Translation2d(-DriveConstants.TRACK_LENGTH / 2.0, -DriveConstants.TRACK_WIDTH / 2.0), + }; + } - public static void setInstance(Drive instance) { - Drive.instance = instance; - } + public static void setInstance(Drive instance) { + Drive.instance = instance; + } - public static double getMaxLinearSpeed() { - return DriveConstants.MAX_LINEAR_SPEED; - } + public static double getMaxLinearSpeed() { + return DriveConstants.MAX_LINEAR_SPEED; + } - public static double getTrackWidthX() { - return DriveConstants.TRACK_LENGTH; - } + public static double getTrackWidthX() { + return DriveConstants.TRACK_LENGTH; + } - public static double getTrackWidthY() { - return DriveConstants.TRACK_WIDTH; - } + public static double getTrackWidthY() { + return DriveConstants.TRACK_WIDTH; + } - public static double getDriveBaseRadius() { - return DriveConstants.DRIVE_BASE_RADIUS; - } + public static double getDriveBaseRadius() { + return DriveConstants.DRIVE_BASE_RADIUS; + } - public static double getMaxAngularSpeed() { - return DriveConstants.MAX_ANGULAR_SPEED; - } + public static double getMaxAngularSpeed() { + return DriveConstants.MAX_ANGULAR_SPEED; + } - public static SwerveDriveKinematics getkDriveKinematics() { - return K_DRIVE_KINEMATICS; - } + public static SwerveDriveKinematics getkDriveKinematics() { + return K_DRIVE_KINEMATICS; + } - public GyroIO getGyroIO() { - return gyroIO; - } + public GyroIO getGyroIO() { + return gyroIO; + } - public GyroIOInputsAutoLogged getGyroIOInputs() { - return gyroInputs; - } + public GyroIOInputsAutoLogged getGyroIOInputs() { + return gyroInputs; + } - public ModuleIOInputsAutoLogged getInputs() { - return inputs; - } + public ModuleIOInputsAutoLogged getInputs() { + return inputs; + } - public Module[] getModules() { - return modules; - } + public Module[] getModules() { + return modules; + } - public SysIdRoutine getSysId() { - return sysId; - } + public SysIdRoutine getSysId() { + return sysId; + } - public MutableMeasure getM_appliedVoltage() { - return m_appliedVoltage; - } + public MutableMeasure getM_appliedVoltage() { + return m_appliedVoltage; + } - public MutableMeasure getM_position() { - return m_position; - } + public MutableMeasure getM_position() { + return m_position; + } - public MutableMeasure getM_velocity() { - return m_velocity; - } + public MutableMeasure getM_velocity() { + return m_velocity; + } - public static SwerveDriveKinematics getModulekinematics() { - return Modulekinematics; - } + public static SwerveDriveKinematics getModulekinematics() { + return Modulekinematics; + } - public static void setModulekinematics(SwerveDriveKinematics modulekinematics) { - Modulekinematics = modulekinematics; - } + public static void setModulekinematics(SwerveDriveKinematics modulekinematics) { + Modulekinematics = modulekinematics; + } - public SwerveDriveKinematics getKinematics() { - return kinematics; - } + public SwerveDriveKinematics getKinematics() { + return kinematics; + } - public void setKinematics(SwerveDriveKinematics kinematics) { - this.kinematics = kinematics; - } + public void setKinematics(SwerveDriveKinematics kinematics) { + this.kinematics = kinematics; + } - public Rotation2d getRawGyroRotation2d() { - return rawGyroRotation2d; - } + public Rotation2d getRawGyroRotation2d() { + return rawGyroRotation2d; + } - public void setRawGyroRotation2d(Rotation2d rawGyroRotation2d) { - this.rawGyroRotation2d = rawGyroRotation2d; - } + public void setRawGyroRotation2d(Rotation2d rawGyroRotation2d) { + this.rawGyroRotation2d = rawGyroRotation2d; + } - public SwerveModulePosition[] getLastModulePositions() { - return lastModulePositions; - } + public SwerveModulePosition[] getLastModulePositions() { + return lastModulePositions; + } - public void setLastModulePositions(SwerveModulePosition[] lastModulePositions) { - this.lastModulePositions = lastModulePositions; - } + public void setLastModulePositions(SwerveModulePosition[] lastModulePositions) { + this.lastModulePositions = lastModulePositions; + } - public SwerveDrivePoseEstimator getPoseEstimator() { - return poseEstimator; - } + public SwerveDrivePoseEstimator getPoseEstimator() { + return poseEstimator; + } - public void setPoseEstimator(SwerveDrivePoseEstimator poseEstimator) { - this.poseEstimator = poseEstimator; - } + public void setPoseEstimator(SwerveDrivePoseEstimator poseEstimator) { + this.poseEstimator = poseEstimator; } - - - - \ No newline at end of file +} From 7b68c62aa079279360e2eadc42eb49efc2f215ab Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Mon, 3 Feb 2025 15:19:51 -0500 Subject: [PATCH 068/110] Code Cleanup --- .../subsystems/drive/DriveConstants.java | 1 + .../frc/robot/subsystems/drive/GyroIO.java | 22 +++---- .../robot/subsystems/drive/GyroIOPigeon.java | 26 ++++----- .../frc/robot/subsystems/drive/Module.java | 58 +++++++------------ .../frc/robot/subsystems/drive/ModuleIO.java | 26 +++++---- .../subsystems/drive/ModuleIOSparkFlex.Java | 3 +- 6 files changed, 62 insertions(+), 74 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 402e4a4..5e07060 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -24,5 +24,6 @@ public class DriveConstants { public static final double DriveGearing = 6.11; public static final int turnCurrentLimit = 60; // May need to change later public static final int driveCurrentLimit = 60; + public static final int pigeonID = 20; } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 484f8ef..c3dd8d8 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -4,17 +4,17 @@ import edu.wpi.first.math.geometry.Rotation2d; - public interface GyroIO { @AutoLog -public static class GyroIOInputs{ - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public double yawVelocityRadPerSec = 0.0; -} - -public default void reset() {} + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + } -public default void updateInputs(GyroIOInputs inputs) {} -} - + public default void reset() { + } + + public default void updateInputs(GyroIOInputs inputs) { + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java index 9833a62..1be99c3 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java @@ -12,27 +12,27 @@ import edu.wpi.first.units.measure.AngularVelocity; public class GyroIOPigeon implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(9); + private final Pigeon2 pigeon = new Pigeon2(DriveConstants.pigeonID); private final StatusSignal yaw = pigeon.getYaw(); private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - + public GyroIOPigeon() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(100); yawVelocity.setUpdateFrequency(100); pigeon.optimizeBusUtilization(); - } + } -@Override -public void reset(){ - pigeon.reset(); -} + @Override + public void reset() { + pigeon.reset(); + } -@Override -public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); -} + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 49710a7..72cab4d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -9,14 +9,11 @@ import org.littletonrobotics.junction.Logger; - public class Module { - private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; - private final SimpleMotorFeedforward drivFeedforward; private final PIDController driveFeddbackPidController; private final PIDController turnFeePidController; @@ -24,21 +21,21 @@ public class Module { private Double speedSetPoint = null; private Rotation2d turnRelativeOffset = null; - public Module (ModuleIO io, int index) { + public Module(ModuleIO io, int index) { this.io = io; this.index = index; switch (Constants.currentMode) { case REAL: - drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); - turnFeePidController = new PIDController(3.5, 0.0, 0.0); + drivFeedforward = new SimpleMotorFeedforward(DriveConstants.TranslationKS, DriveConstants.TranslationKV); + driveFeddbackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD); + turnFeePidController = new PIDController(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD); break; case REPLAY: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); turnFeePidController = new PIDController(7, 0.0, 0.0); - break; + break; case SIM: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); @@ -50,39 +47,33 @@ public Module (ModuleIO io, int index) { turnFeePidController = new PIDController(0.0, 0.0, 0.0); break; } - - turnFeePidController.enableContinuousInput(-Math.PI,Math.PI); - setBrakeMode(true); + turnFeePidController.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); } + public void periodic() { io.updateInputs(inputs); - Logger.processInputs("Drive/Module"+ Integer.toString(index), inputs); - - + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); } - if (angleSetpoint != null) { - io.setTurnVoltage(turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + io.setTurnVoltage(turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); } - if (speedSetPoint != null) { - double adjustSpeedSetpoint = speedSetPoint*Math.cos(turnFeePidController.getPositionError()); + double adjustSpeedSetpoint = speedSetPoint * Math.cos(turnFeePidController.getPositionError()); double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS; io.setDriveVoltage( - drivFeedforward.calculate(velocityRadPerSec) - + driveFeddbackPidController.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + drivFeedforward.calculate(velocityRadPerSec) + + driveFeddbackPidController.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); } } - - public SwerveModuleState runSetpoint(SwerveModuleState state) { state.optimize(getAngle()); angleSetpoint = state.angle; @@ -94,56 +85,47 @@ public void runCharacterization(double volts) { angleSetpoint = new Rotation2d(); - io.setDriveVoltage(volts); speedSetPoint = null; } - - public void stop(){ + public void stop() { io.setTurnVoltage(0.0); io.setDriveVoltage(0.0); - angleSetpoint = null; speedSetPoint = null; } - public void setBrakeMode(boolean enabled) { io.setDriveBrakeMode(enabled); io.setTurnBrakeMode(enabled); } - - public Rotation2d getAngle (){ - if (turnRelativeOffset == null){ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { return new Rotation2d(); } else { return inputs.turnPosition.plus(turnRelativeOffset); } } - - public double getVelocityRadPerSec(){ - return inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS; + public double getVelocityRadPerSec() { + return inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS; } - public double getPositionMeters() { - return inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS; + return inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS; } public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } - - public SwerveModuleState getState () { + public SwerveModuleState getState() { return new SwerveModuleState(getVelocityRadPerSec(), getAngle()); } - public double getCharacterizationVelocity() { return inputs.driveVelocityRadPerSec; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 1f267a1..a562471 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -13,26 +13,32 @@ public static class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double[] driveCurrentAmps = new double[] {}; - public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); public double turnAppliedVolts = 0.0; public double turnVelocityRadPerSec = 0.0; public double turnAppliedRadPerSec = 0.0; public double[] turnCurrentAmps = new double[] {}; - } + } - public default void updateInputs(ModuleIOInputs inputs) {} + public default void updateInputs(ModuleIOInputs inputs) { + } - public default void setDriveVoltage(double volts) {} + public default void setDriveVoltage(double volts) { + } - public default void runTalonPID (double desiredStateRotPerSecond) {} + public default void runMovementPID(double desiredStateRotPerSecond, double rotationDesiredPosition) { + } - public default void setDesiredState(SwerveModuleState state) {} + public default void setDesiredState(SwerveModuleState state) { + } - public default void setTurnVoltage (double volts) {} + public default void setTurnVoltage(double volts) { + } - public default void setDriveBrakeMode(boolean enable) {} + public default void setDriveBrakeMode(boolean enable) { + } - public default void setTurnBrakeMode(boolean enable) {} -} + public default void setTurnBrakeMode(boolean enable) { + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java index 2468259..f002987 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java @@ -91,8 +91,7 @@ public class ModuleIOSparkMax implements ModuleIO { turnSparkMax.setInverted(isTurnMotorInverted); driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.setSmartCurrentLimit(30); driveSparkMax.enableVoltageCompensation(12.0); turnSparkMax.enableVoltageCompensation(12.0); driveEncoder.setPosition(0.0); From 79de6892b3d032f723b03077e2d3d73fbfff29ce Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Mon, 3 Feb 2025 15:29:13 -0500 Subject: [PATCH 069/110] mode real and button binding additions for arm for mor delicate control --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 36 ++++++++++++--------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9c4e47b..ee96557 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,7 +8,7 @@ public static final class IntakeSetpoints{ public static final double slow = -0.3; public static final double reverse = 0.7; } - public static Mode currentMode = Mode.SIM; + public static Mode currentMode = Mode.REAL; public static enum Mode { REAL, SIM, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7f25608..7b9fcd8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -51,7 +51,6 @@ public RobotContainer() throws IOException, ParseException { elevator = Elevator.initialize(new ElevatorIOReal()); intake = Intake.initialize(new IntakeIORealVortex()); Arm.initialize(new ArmIOReal()); -; drive = new Drive( new GyroIOPigeon(), @@ -66,7 +65,6 @@ public RobotContainer() throws IOException, ParseException { intake = Intake.initialize(new IntakeIOSim()); Arm.initialize(new ArmIOSim()); - drive = new Drive( new GyroIO() { }, @@ -83,7 +81,6 @@ public RobotContainer() throws IOException, ParseException { intake = Intake.initialize(new IntakeIOSim()); Arm.initialize(new ArmIOSim()); - drive = new Drive( new GyroIO() { }, @@ -107,19 +104,10 @@ private void configureBindings() { Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); - CommandXboxController xboxController = new CommandXboxController(1); - xboxController.leftTrigger() - .whileTrue(Commands.run(() -> elevator.moveElevator(xboxController.getLeftTriggerAxis() / 2))); - xboxController.rightTrigger() - .whileTrue(Commands.run(() -> elevator.moveElevator(-xboxController.getRightTriggerAxis() / 2))); - - - Constants.OIConstants.driverController.x().whileTrue(Commands.startEnd(()->intake.setIntakeSpeed(1), - ()->intake.setIntakeSpeed(0), intake)); - - OIConstants.driverController.b().whileTrue(Commands.startEnd(()->intake.setIntakeSpeed(-1), - ()->intake.setIntakeSpeed(0), intake)); - + OIConstants.operatorController.leftTrigger() + .whileTrue(Commands.run(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 2))); + OIConstants.operatorController.rightTrigger() + .whileTrue(Commands.run(() -> elevator.moveElevator(-OIConstants.operatorController.getRightTriggerAxis() / 2))); Constants.OIConstants.operatorController.povUp() .whileTrue( @@ -151,6 +139,22 @@ private void configureBindings() { Arm.getInstance() ) ); + + + OIConstants.driverController.leftTrigger().whileTrue( + Commands.startEnd( + () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), + () -> Arm.getInstance().manualVoltage(0), + Arm.getInstance() + )); + + + OIConstants.driverController.rightTrigger().whileTrue( + Commands.startEnd( + () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), + () -> Arm.getInstance().manualVoltage(0), + Arm.getInstance() + )); } public Command getAutonomousCommand() { From 910bf09e0a8bfe0decee12083c13b7d14fd226a6 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Mon, 3 Feb 2025 18:06:52 -0500 Subject: [PATCH 070/110] drive testing --- src/main/java/frc/robot/Constants.java | 2 +- .../subsystems/drive/DriveConstants.java | 4 ++-- .../frc/robot/subsystems/drive/Module.java | 2 +- .../subsystems/drive/ModuleIOTalonFX.java | 18 +++++++++-------- .../elevator/ElevatorConstants.java | 4 ++-- .../subsystems/elevator/ElevatorIOReal.java | 20 +++++++++---------- 6 files changed, 26 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ee96557..b1e8803 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,8 +10,8 @@ public static final class IntakeSetpoints{ } public static Mode currentMode = Mode.REAL; public static enum Mode { - REAL, SIM, + REAL, REPLAY } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 5e07060..5b9ec71 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -16,11 +16,11 @@ public class DriveConstants { public static final double TranslationKP = 0.1; public static final double TranslationKI = 0.0; public static final double TranslationKD = 0.0; - public static final double RotationKP = 10.0; + public static final double RotationKP = 5.0; public static final double RotationKI = 0.0; public static final double RotationKD = 0.0; public static final double WHEEL_RADIUS = Units.inchesToMeters(1.5); - public static final double TurnGearing = 13.3714; + public static final double TurnGearing = 12.155818; public static final double DriveGearing = 6.11; public static final int turnCurrentLimit = 60; // May need to change later public static final int driveCurrentLimit = 60; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 72cab4d..fdb262a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -106,7 +106,7 @@ public Rotation2d getAngle() { if (turnRelativeOffset == null) { return new Rotation2d(); } else { - return inputs.turnPosition.plus(turnRelativeOffset); + return inputs.turnAbsolutePosition; } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 38cfb4e..e12e1b2 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -72,21 +72,21 @@ public ModuleIOTalonFX(int index) { break; case 1: // Front right - driveTalonFX = new TalonFX(10); - turnSparkFlex = new SparkFlex(9, MotorType.kBrushless); + driveTalonFX = new TalonFX(3); + turnSparkFlex = new SparkFlex(4, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); break; case 2: // Back left - driveTalonFX = new TalonFX(4); - turnSparkFlex = new SparkFlex(6, MotorType.kBrushless); + driveTalonFX = new TalonFX(8); + turnSparkFlex = new SparkFlex(7, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); driveTalonFX.setInverted(false); break; case 3: // Back right - driveTalonFX = new TalonFX(7); - turnSparkFlex = new SparkFlex(8, MotorType.kBrushless); + driveTalonFX = new TalonFX(6); + turnSparkFlex = new SparkFlex(5, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); break; default: @@ -115,14 +115,16 @@ public ModuleIOTalonFX(int index) { driveTalonFX.setPosition(0); driveTalonFX.setInverted(true); - + encoderconfig.positionConversionFactor(2 * Math.PI); + encoderconfig.velocityConversionFactor(2 * Math.PI / 60); turnSparkFlex.setCANTimeout(0); // turnSparkFlex.burnFlash(); var config = config(); - var configSpark = configSpark(null); + var configSpark = configSpark(IdleMode.kBrake); + configSpark.apply(encoderconfig); driveTalonFX.optimizeBusUtilization(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 383685c..483e8b7 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -10,8 +10,8 @@ public final class ElevatorConstants{ public static double kD = 0; //figure this out later public static double kI = 0; //figure this out later public static double kP = 1; //figure this out later - public static int rightDeviceID = 13; - public static int leftDeviceID = 12; + public static int rightDeviceID = 12; + public static int leftDeviceID = 13; public static double ks = 0; public static double kg = 0.16; public static double kv = 15.96; //IS THIS RIGHT?!?!? diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 419af5a..a752ae2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -64,21 +64,21 @@ public void zeroElevator(){ public ElevatorIOReal(){ - - config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); - rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - config.follow(ElevatorConstants.rightDeviceID, true); - config.inverted(true); + leftElevatorMotor.clearFaults(); + rightElevatorMotor.clearFaults(); + // config.encoder.positionConversionFactor(Units.inchesToMeters(11/18)); + // config.encoder.velocityConversionFactor(Units.inchesToMeters(11/18)/60); + config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); - config.encoder.positionConversionFactor(Units.inchesToMeters(11/18)); - config.encoder.velocityConversionFactor(Units.inchesToMeters(11/18)/60); - + rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + config.follow(ElevatorConstants.rightDeviceID, true); + + // config.absoluteEncoder.positionConversionFactor(Units.inchesToMeters(11 / 18)); use these later // config.absoluteEncoder.velocityConversionFactor(Units.inchesToMeters(11 / 18) / 60); use these later leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - leftElevatorMotor.clearFaults(); - rightElevatorMotor.clearFaults(); + } @Override From 7af9661e9060e3749100a3706c41bc96563c6753 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Mon, 3 Feb 2025 18:27:52 -0500 Subject: [PATCH 071/110] drive changes --- .../java/frc/robot/subsystems/elevator/ElevatorConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 483e8b7..55a6f99 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -20,7 +20,7 @@ public final class ElevatorConstants{ public static double conversionFactor = 0.1; - public static final int elevatorCurrentLimits = 60; //might need to adjust later + public static final int elevatorCurrentLimits = 45; //might need to adjust later From 4822be63d73a3af1ad030ff3ae8afdf4246ec5da Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Tue, 4 Feb 2025 14:27:10 -0500 Subject: [PATCH 072/110] added onboard PID control and conversion factors --- .../subsystems/drive/DriveConstants.java | 1 + .../subsystems/drive/ModuleIOTalonFX.java | 33 +++++++++++++------ 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 5e07060..0235f24 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -25,5 +25,6 @@ public class DriveConstants { public static final int turnCurrentLimit = 60; // May need to change later public static final int driveCurrentLimit = 60; public static final int pigeonID = 20; + public static final double turningFactor = 2*Math.PI; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 38cfb4e..38eccbf 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -24,12 +24,14 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.ExternalEncoderConfig; import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import frc.robot.commands.DriveCommands; /** * Module IO implementation for SparkMax drive motor controller, SparkMax turn @@ -59,7 +61,8 @@ public class ModuleIOTalonFX implements ModuleIO { private final AbsoluteEncoder turnAbsoluteEncoder; private final boolean isTurnMotorInverted = true; - + public static final SparkFlexConfig turningConfig = new SparkFlexConfig(); + public ModuleIOTalonFX(int index) { switch (index) { @@ -93,12 +96,21 @@ public ModuleIOTalonFX(int index) { throw new RuntimeException("Invalid module index"); } - // final TalonFXConfiguration drivingConfig = new TalonFXConfiguration(); - // turnSparkFlex.restoreFactoryDefaults(); - - // turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians - // turnAbsoluteEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); // - // Radians per second + + turningConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(DriveConstants.turnCurrentLimit); + turningConfig.absoluteEncoder + .inverted(true) + .positionConversionFactor(DriveConstants.turningFactor) + .velocityConversionFactor(DriveConstants.turningFactor/60); + turningConfig.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .pid(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD) + .outputRange(-1, 1) + .positionWrappingEnabled(true) + .positionWrappingInputRange(0, DriveConstants.turningFactor); + turnSparkFlex.setCANTimeout(250); @@ -119,8 +131,7 @@ public ModuleIOTalonFX(int index) { turnSparkFlex.setCANTimeout(0); - // turnSparkFlex.burnFlash(); - + var config = config(); var configSpark = configSpark(null); @@ -168,7 +179,7 @@ private SparkFlexConfig configSpark(IdleMode idle) { var turningConfig = new SparkFlexConfig(); turningConfig.smartCurrentLimit(DriveConstants.turnCurrentLimit); - // turningConfig.voltageCompensation(12.0); + turningConfig.voltageCompensation(12.0); if (idle != null) { turningConfig.idleMode(idle); @@ -216,4 +227,6 @@ public void setTurnBrakeMode(boolean enable) { // turnSparkFlex.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); configSpark(enable ? IdleMode.kBrake : IdleMode.kCoast); } + + } \ No newline at end of file From 752f50e09bd9f9158af476483019453e61d617f3 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Tue, 4 Feb 2025 18:18:06 -0500 Subject: [PATCH 073/110] robot scores --- src/main/java/frc/robot/RobotContainer.java | 39 ++++++++++++++----- .../java/frc/robot/subsystems/arm/Arm.java | 4 ++ .../robot/subsystems/arm/ArmConstants.java | 6 +-- .../frc/robot/subsystems/arm/ArmIOReal.java | 18 +++++---- .../robot/subsystems/elevator/Elevator.java | 7 ++++ .../elevator/ElevatorConstants.java | 4 +- .../subsystems/elevator/ElevatorIOReal.java | 35 ++++++++--------- .../frc/robot/subsystems/intake/Intake.java | 2 +- .../subsystems/intake/IntakeConstants.java | 4 +- .../subsystems/intake/IntakeIORealVortex.java | 1 + 10 files changed, 75 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b9fcd8..4c29137 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,9 +19,11 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorConstants; import frc.robot.subsystems.elevator.ElevatorIO; import frc.robot.subsystems.elevator.ElevatorIOReal; import frc.robot.subsystems.elevator.ElevatorIOSim; +import frc.robot.Constants.Gamepiece; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIOReal; @@ -91,6 +93,7 @@ public RobotContainer() throws IOException, ParseException { } + Arm.getInstance().setFFMode(Gamepiece.CORAL); configureBindings(); } @@ -104,16 +107,32 @@ private void configureBindings() { Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); - OIConstants.operatorController.leftTrigger() - .whileTrue(Commands.run(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 2))); - OIConstants.operatorController.rightTrigger() - .whileTrue(Commands.run(() -> elevator.moveElevator(-OIConstants.operatorController.getRightTriggerAxis() / 2))); + // OIConstants.operatorController.leftTrigger() + // .whileTrue(Commands.startEnd(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 1.5), + // () -> elevator.moveElevator(0), + // elevator)); + + // OIConstants.operatorController.rightTrigger() + // .whileTrue(Commands.startEnd(() -> elevator.moveElevator(-OIConstants.operatorController.getRightTriggerAxis() / 1.5), + // () -> elevator.moveElevator(0), + // elevator)); + + OIConstants.operatorController.rightBumper() + .whileTrue(Commands.startEnd(() -> elevator.setVoltage(4), + () -> elevator.setVoltage(-ElevatorConstants.kg), + elevator)); + + OIConstants.operatorController.leftBumper() + .whileTrue(Commands.startEnd(() -> elevator.setVoltage(-4), + () -> elevator.setVoltage(-ElevatorConstants.kg), + elevator)); + Constants.OIConstants.operatorController.povUp() .whileTrue( Commands.sequence( intake.intake().until(() -> intake.GamePieceInitial()), - intake.intakeSlow().until(() -> intake.GamePeiceFinal()), + intake.intakeSlow().until(() -> intake.GamePieceFinal()), Commands.run(() -> intake.setIntakeSpeed(0)) ) ); @@ -126,16 +145,16 @@ private void configureBindings() { OIConstants.driverController.povUp().whileTrue( Commands.startEnd( - () -> Arm.getInstance().manualVoltage(6), - () -> Arm.getInstance().manualVoltage(0), + () -> Arm.getInstance().manualVoltage(9), + () -> Arm.getInstance().manualVoltage(ArmConstants.DEFAULTkG * Math.cos(Arm.getInstance().getAngle() - Math.PI/2)), Arm.getInstance() ) ); OIConstants.driverController.povDown().whileTrue( Commands.startEnd( - () -> Arm.getInstance().manualVoltage(-6), - () -> Arm.getInstance().manualVoltage(0), + () -> Arm.getInstance().manualVoltage(-9), + () -> Arm.getInstance().manualVoltage(ArmConstants.DEFAULTkG * Math.cos(Arm.getInstance().getAngle() - Math.PI/2)), Arm.getInstance() ) ); @@ -158,6 +177,6 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return Commands.runOnce(() -> Arm.getInstance().manualVoltage(ArmConstants.DEFAULTkG)); + return Commands.runOnce(() -> elevator.setVoltage(ElevatorConstants.kg)); } } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 07ab2ac..a8c6119 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -24,6 +24,10 @@ private Arm(ArmIO io) { this.io = io; } + public double getAngle() { + return inputs.angularPosition; + } + public static void initialize(ArmIO io) { instance = new Arm(io); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 6b8964b..c3dc333 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -4,7 +4,7 @@ public class ArmConstants { public static int CAN_ID = 14; - public static int CURRENT_LIMIT = 40; + public static int CURRENT_LIMIT = 50; // Default FF public static double DEFAULTkG = 1.02; @@ -19,13 +19,13 @@ public class ArmConstants { public static double ALGAEkS = 0.1; // Default FF - public static double CORALkG = 1.02; + public static double CORALkG = 1.09; public static double CORALkV = 1.01; public static double CORALkA = 0.05; public static double CORALkS = 0.1; public static class Sim { - public static double GEARING = 60; + public static double GEARING = 40; public static double MOI = 2.09670337984; public static double LENGTH = 0.6604; public static double MIN_ANGLE = Units.degreesToRadians(-95); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index a2754e0..91302f8 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -23,19 +23,20 @@ public class ArmIOReal implements ArmIO { private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); private final PIDController controller = new PIDController(0.04, 0,0.00); - private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 2); + private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 2); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); - private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0); + private TrapezoidProfile.State goal = new TrapezoidProfile.State(0, 0); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); public ArmIOReal(){ config.smartCurrentLimit(ArmConstants.CURRENT_LIMIT); config.idleMode(IdleMode.kBrake); - config.absoluteEncoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); - config.absoluteEncoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60); - config.absoluteEncoder.inverted(true); - config.encoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); - config.encoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60); + config.absoluteEncoder.positionConversionFactor(1); + config.absoluteEncoder.positionConversionFactor(Math.PI); + config.absoluteEncoder.velocityConversionFactor(Math.PI / 60); + config.absoluteEncoder.inverted(false); + config.encoder.positionConversionFactor(Math.PI); + config.encoder.velocityConversionFactor(Math.PI / 60); config.softLimit.forwardSoftLimitEnabled(true); config.softLimit.reverseSoftLimitEnabled(false); config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); @@ -70,10 +71,11 @@ public void updateInputs(ArmIOInputs inputs) { setpoint = profile.calculate(0.02, setpoint, goal); } - inputs.angularPosition = armEncoder.getPosition(); + inputs.angularPosition = armMotor.getAbsoluteEncoder().getPosition(); inputs.angularVelocity = armEncoder.getVelocity(); inputs.current = armMotor.getOutputCurrent(); inputs.voltage = armMotor.getAppliedOutput(); + inputs.setpointVelocity = setpoint.velocity; } public void setVoltage(double voltage) { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index be4c9f2..12c5a9d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -17,6 +17,7 @@ public class Elevator extends SubsystemBase { private ElevatorIO io; private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + private double voltage = 0; @AutoLogOutput private double leftMovingSpeed = 0.0; @@ -100,10 +101,16 @@ public void reset() { this.setGoal(getPosition()); } + public void setVoltage(double volts) { + io.setVoltage(volts); + voltage = volts; + } + @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs(getName(), inputs); + io.setVoltage(voltage); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 55a6f99..e5752e5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -13,14 +13,14 @@ public final class ElevatorConstants{ public static int rightDeviceID = 12; public static int leftDeviceID = 13; public static double ks = 0; - public static double kg = 0.16; + public static double kg = 0.3; public static double kv = 15.96; //IS THIS RIGHT?!?!? //change this later public static double conversionFactor = 0.1; - public static final int elevatorCurrentLimits = 45; //might need to adjust later + public static final int elevatorCurrentLimits = 70; //might need to adjust later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index a752ae2..359a964 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -21,6 +21,8 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.motorcontrol.Spark; @@ -66,11 +68,12 @@ public void zeroElevator(){ public ElevatorIOReal(){ leftElevatorMotor.clearFaults(); rightElevatorMotor.clearFaults(); - // config.encoder.positionConversionFactor(Units.inchesToMeters(11/18)); - // config.encoder.velocityConversionFactor(Units.inchesToMeters(11/18)/60); + // config.encoder.positionConversionFactor(Units.inchesToMeters(11.0/18.0)); + // config.encoder.velocityConversionFactor(Units.inchesToMeters(11.0/18)/60.0); config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); + config.idleMode(IdleMode.kBrake); - rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); config.follow(ElevatorConstants.rightDeviceID, true); @@ -83,12 +86,12 @@ public ElevatorIOReal(){ @Override public void setVoltage(double voltage) { - double currentPosition = leftEncoder.getPosition(); // Relative encoder position - if (currentPosition <= minPositionMeters && voltage < 0) { - voltage = 0; - } else if (currentPosition >= maxPositionMeters && voltage > 0) { - voltage = 0; - } + // double currentPosition = leftEncoder.getPosition(); // Relative encoder position + // if (currentPosition <= minPositionMeters && voltage < 0) { + // voltage = 0; + // } else if (currentPosition >= maxPositionMeters && voltage > 0) { + // voltage = 0; + // } inputVolts = voltage; rightElevatorMotor.setVoltage(voltage); } @@ -98,10 +101,10 @@ public void setVoltage(double voltage) { public void updateInputs(ElevatorIOInputs inputs) { //might want to separate by motor or you can average if they don't need to be ran in reverse - if(!inputs.openLoop){ - setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - setpoint = profile.calculate(0.02, setpoint, goal); - } + // if(!inputs.openLoop){ + // setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); + // setpoint = profile.calculate(0.02, setpoint, goal); + // } inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); @@ -130,12 +133,6 @@ public void updateInputs(ElevatorIOInputs inputs) { @Override public void moveElevator(double speed) { - - double currentPosition = leftEncoder.getPosition(); - if ((currentPosition <= minPositionMeters && speed < 0) || - (currentPosition >= maxPositionMeters && speed > 0)) { - speed = 0; - } rightElevatorMotor.set(speed); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 25ebce4..fa4face 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -61,7 +61,7 @@ public boolean GamePieceInitial() { return IntakeConstants.initialThreshold <= inputs.currentAmps; } - public boolean GamePeiceFinal() { + public boolean GamePieceFinal() { return IntakeConstants.currentThreshold <= inputs.currentAmps; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 1fca7c3..0be4950 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -11,7 +11,7 @@ public final class IntakeConstants { public static final double intakeCurrent = 0; public static final int currentLimit = 30; - public static final double currentThreshold = 20; //change later based on akit numbers for gamepiece + public static final double currentThreshold = 25; //change later based on akit numbers for gamepiece - public static final double initialThreshold = 10; + public static final double initialThreshold = 25; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java b/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java index 6f42156..4121e2e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java @@ -29,6 +29,7 @@ public class IntakeIORealVortex implements IntakeIO { private SparkFlexConfig config = new SparkFlexConfig(); public IntakeIORealVortex() { + intakeMotor.clearFaults(); config.smartCurrentLimit(IntakeConstants.currentLimit); config.idleMode(IdleMode.kCoast); From 61499a8cff5b7e7b7556ef6af5f03cb8e7460928 Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Wed, 5 Feb 2025 00:08:35 -0500 Subject: [PATCH 074/110] FF parameter changes for arm and elevator --- src/main/java/frc/robot/subsystems/arm/ArmConstants.java | 1 + src/main/java/frc/robot/subsystems/arm/ArmIOReal.java | 4 ++-- .../java/frc/robot/subsystems/elevator/ElevatorConstants.java | 2 ++ .../java/frc/robot/subsystems/elevator/ElevatorIOReal.java | 4 ++-- 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index c3dc333..30d0e13 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -5,6 +5,7 @@ public class ArmConstants { public static int CAN_ID = 14; public static int CURRENT_LIMIT = 50; + public static final double ARM_OFFSET = Math.PI/2; // Default FF public static double DEFAULTkG = 1.02; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 91302f8..7f35b3c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -66,8 +66,8 @@ public void resetAbsoluteEncoder() { @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { - // setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(inputs.angularVelocity, setpoint.velocity)); - setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position)); + // setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity) + controller.calculate(inputs.angularVelocity, setpoint.velocity)); + setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity + ArmConstants.ARM_OFFSET)); setpoint = profile.calculate(0.02, setpoint, goal); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index e5752e5..f4e0464 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -15,6 +15,8 @@ public final class ElevatorConstants{ public static double ks = 0; public static double kg = 0.3; public static double kv = 15.96; //IS THIS RIGHT?!?!? + public static final double MAX_ACCELERATION = 1; + public static final double MAX_VELOCITY = 1; //change this later public static double conversionFactor = 0.1; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 359a964..2d440ef 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -38,7 +38,7 @@ public class ElevatorIOReal implements ElevatorIO { // private AbsoluteEncoder leftAbsoluteEncoder = leftElevatorMotor.getAbsoluteEncoder(); private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(0, 0, 0); - private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 1); + private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION); private final PIDController controller = new PIDController(1, 0, 0); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); @@ -118,7 +118,7 @@ public void updateInputs(ElevatorIOInputs inputs) { Logger.recordOutput("Elevator/Setpoint/Position", setpoint.position); Logger.recordOutput("Elevator/Setpoint/Velocity", setpoint.velocity); - setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); + setVoltage(ffmodel.calculate(setpoint.velocity, ElevatorConstants.MAX_ACCELERATION) + controller.calculate(setpoint.velocity)); //originally poassing in position, changed to max acceleration not sure if right setpoint = profile.calculate(0.02, setpoint, goal); // log goal after updating From aa180ea835fcb8b0d88c05e2b5905d88e5b85c47 Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Wed, 5 Feb 2025 11:01:10 -0500 Subject: [PATCH 075/110] made constants final, changed elevator ff to use most recent version --- .../robot/subsystems/arm/ArmConstants.java | 28 +++++++++---------- .../robot/subsystems/elevator/Elevator.java | 1 - .../elevator/ElevatorConstants.java | 20 ++++++------- .../subsystems/elevator/ElevatorIOReal.java | 2 +- 4 files changed, 25 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 30d0e13..eb51449 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -3,27 +3,27 @@ import edu.wpi.first.math.util.Units; public class ArmConstants { - public static int CAN_ID = 14; - public static int CURRENT_LIMIT = 50; + public static final int CAN_ID = 14; + public static final int CURRENT_LIMIT = 50; public static final double ARM_OFFSET = Math.PI/2; // Default FF - public static double DEFAULTkG = 1.02; - public static double DEFAULTkV = 1.01; - public static double DEFAULTkA = 0.05; - public static double DEFAULTkS = 0.1; + public static final double DEFAULTkG = 1.02; + public static final double DEFAULTkV = 1.01; + public static final double DEFAULTkA = 0.05; + public static final double DEFAULTkS = 0.1; // Algae FF - public static double ALGAEkG = 1.02; - public static double ALGAEkV = 1.01; - public static double ALGAEkA = 0.05; - public static double ALGAEkS = 0.1; + public static final double ALGAEkG = 1.02; + public static final double ALGAEkV = 1.01; + public static final double ALGAEkA = 0.05; + public static final double ALGAEkS = 0.1; // Default FF - public static double CORALkG = 1.09; - public static double CORALkV = 1.01; - public static double CORALkA = 0.05; - public static double CORALkS = 0.1; + public static final double CORALkG = 1.09; + public static final double CORALkV = 1.01; + public static final double CORALkA = 0.05; + public static final double CORALkS = 0.1; public static class Sim { public static double GEARING = 40; diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 12c5a9d..3a7a68a 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -111,7 +111,6 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs(getName(), inputs); io.setVoltage(voltage); - } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index f4e0464..688752d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -4,22 +4,22 @@ public final class ElevatorConstants{ public static final double maxDistance = 0.80; - // public static double constraints = 0.2; //figure this out later + // public static final double constraints = 0.2; //figure this out later public static final Constraints constraints = new Constraints(4, 5); //change later - public static double kD = 0; //figure this out later - public static double kI = 0; //figure this out later - public static double kP = 1; //figure this out later - public static int rightDeviceID = 12; - public static int leftDeviceID = 13; - public static double ks = 0; - public static double kg = 0.3; - public static double kv = 15.96; //IS THIS RIGHT?!?!? + public static final double kD = 0; //figure this out later + public static final double kI = 0; //figure this out later + public static final double kP = 1; //figure this out later + public static final int rightDeviceID = 12; + public static final int leftDeviceID = 13; + public static final double ks = 0; + public static final double kg = 0.3; + public static final double kv = 15.96; //IS THIS RIGHT?!?!? public static final double MAX_ACCELERATION = 1; public static final double MAX_VELOCITY = 1; //change this later - public static double conversionFactor = 0.1; + public static final double conversionFactor = 0.1; public static final int elevatorCurrentLimits = 70; //might need to adjust later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 2d440ef..34248bf 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -118,7 +118,7 @@ public void updateInputs(ElevatorIOInputs inputs) { Logger.recordOutput("Elevator/Setpoint/Position", setpoint.position); Logger.recordOutput("Elevator/Setpoint/Velocity", setpoint.velocity); - setVoltage(ffmodel.calculate(setpoint.velocity, ElevatorConstants.MAX_ACCELERATION) + controller.calculate(setpoint.velocity)); //originally poassing in position, changed to max acceleration not sure if right + setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(setpoint.velocity)); //originally poassing in position, changed to max acceleration not sure if right setpoint = profile.calculate(0.02, setpoint, goal); // log goal after updating From ff44db854753024f458548faf323ecb272bc2d11 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Wed, 5 Feb 2025 12:04:51 -0500 Subject: [PATCH 076/110] Log intake gampiece detection --- .vscode/settings.json | 2 +- src/main/java/frc/robot/subsystems/intake/IntakeIO.java | 1 + src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java | 1 + src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java | 1 + 4 files changed, 4 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 2c3170b..c9c0948 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,5 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "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 -Xmx16G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index cee9632..62193a7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -11,6 +11,7 @@ public static class IntakeIOInputs { public double angularPositionRot = 0.0; public double angularVelocityRPM = 0.0; public double currentAmps = 0.0; + public boolean hasGamepiece = false; } public default void updateInputs(IntakeIOInputs inputs) { } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index 6f246b0..d47dca7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -58,6 +58,7 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.angularPositionRot = intakeMotor.getPosition().getValueAsDouble(); inputs.currentAmps = current.getValueAsDouble(); inputs.voltage = voltage.getValueAsDouble(); + inputs.hasGamepiece = hasGamepiece(); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 31912b4..85d1db4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -19,6 +19,7 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.angularVelocityRPM = motor.getAngularVelocityRPM(); inputs.currentAmps = motor.getCurrentDrawAmps(); isGamepieceDetected = inputs.currentAmps > IntakeConstants.currentThreshold; + inputs.hasGamepiece = hasGamepiece(); } @Override public boolean hasGamepiece(){ From 96838323771dff34862b7853155ccac6d23a1390 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Wed, 5 Feb 2025 12:17:28 -0500 Subject: [PATCH 077/110] only use setpoints if open loop mode is off --- src/main/java/frc/robot/RobotContainer.java | 24 ++++++++++++------- .../robot/subsystems/elevator/Elevator.java | 1 + .../subsystems/elevator/ElevatorIOReal.java | 11 ++++----- 3 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4c29137..2631126 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -117,15 +117,21 @@ private void configureBindings() { // () -> elevator.moveElevator(0), // elevator)); - OIConstants.operatorController.rightBumper() - .whileTrue(Commands.startEnd(() -> elevator.setVoltage(4), - () -> elevator.setVoltage(-ElevatorConstants.kg), - elevator)); - - OIConstants.operatorController.leftBumper() - .whileTrue(Commands.startEnd(() -> elevator.setVoltage(-4), - () -> elevator.setVoltage(-ElevatorConstants.kg), - elevator)); + OIConstants.operatorController.rightBumper().whileTrue( + Commands.startEnd( + () -> elevator.setVoltage(4), + () -> elevator.setVoltage(-ElevatorConstants.kg), + elevator + ) + ); + + OIConstants.operatorController.leftBumper().whileTrue( + Commands.startEnd( + () -> elevator.setVoltage(-4), + () -> elevator.setVoltage(-ElevatorConstants.kg), + elevator + ) + ); Constants.OIConstants.operatorController.povUp() diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 3a7a68a..90936b5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -102,6 +102,7 @@ public void reset() { } public void setVoltage(double volts) { + setOpenLoop(true); io.setVoltage(volts); voltage = volts; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 34248bf..ba5bbd8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -101,10 +101,10 @@ public void setVoltage(double voltage) { public void updateInputs(ElevatorIOInputs inputs) { //might want to separate by motor or you can average if they don't need to be ran in reverse - // if(!inputs.openLoop){ - // setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - // setpoint = profile.calculate(0.02, setpoint, goal); - // } + if(!inputs.openLoop){ + setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(setpoint.velocity)); + setpoint = profile.calculate(0.02, setpoint, goal); + } inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); @@ -118,9 +118,6 @@ public void updateInputs(ElevatorIOInputs inputs) { Logger.recordOutput("Elevator/Setpoint/Position", setpoint.position); Logger.recordOutput("Elevator/Setpoint/Velocity", setpoint.velocity); - setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(setpoint.velocity)); //originally poassing in position, changed to max acceleration not sure if right - setpoint = profile.calculate(0.02, setpoint, goal); - // log goal after updating Logger.recordOutput("Elevator/Goal/Position", goal.position); Logger.recordOutput("Elevator/Goal/Velocity", goal.velocity); From a48c839307a66046365d48f91456dcc3008fd287 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Wed, 5 Feb 2025 12:31:32 -0500 Subject: [PATCH 078/110] right and left elevator inputs --- .../robot/subsystems/elevator/ElevatorIO.java | 20 ++++++++----- .../subsystems/elevator/ElevatorIOReal.java | 29 +++++++++++-------- .../subsystems/elevator/ElevatorIOSim.java | 10 +++---- 3 files changed, 33 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 26aebbb..152ada3 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -5,14 +5,18 @@ public interface ElevatorIO { @AutoLog public static class ElevatorIOInputs { - public double elevatorInputVolts = 0.0; - public double elevatorAppliedVolts = 0.0; - public double elevatorPositionMetersPerSecond = 0.0; - public double elevatorVelocityMetersPerSecond = 0.0; - public double elevatorCurrent = 0.0; - - public double elevatorMotorVelocityMetersPerSecond = 0.0; - public double elevatorMotorPositionMeters = 0.0; + public double elevatorRightInputVolts = 0.0; + public double elevatorRightAppliedVolts = 0.0; + public double elevatorRightPositionMeters= 0.0; + public double elevatorRightVelocityMetersPerSecond = 0.0; + public double elevatorRightCurrent = 0.0; + + public double elevatorLeftInputVolts = 0.0; + public double elevatorLeftAppliedVolts = 0.0; + public double elevatorLeftPositionMeters = 0.0; + public double elevatorLeftVelocityMetersPerSecond = 0.0; + public double elevatorLeftCurrent = 0.0; + public boolean openLoop = true; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index ba5bbd8..6ec0853 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -68,8 +68,8 @@ public void zeroElevator(){ public ElevatorIOReal(){ leftElevatorMotor.clearFaults(); rightElevatorMotor.clearFaults(); - // config.encoder.positionConversionFactor(Units.inchesToMeters(11.0/18.0)); - // config.encoder.velocityConversionFactor(Units.inchesToMeters(11.0/18)/60.0); + config.encoder.positionConversionFactor(Units.inchesToMeters(11.0/18.0)); + config.encoder.velocityConversionFactor(Units.inchesToMeters(11.0/18.0)/60.0); config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); config.idleMode(IdleMode.kBrake); @@ -105,16 +105,21 @@ public void updateInputs(ElevatorIOInputs inputs) { setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(setpoint.velocity)); setpoint = profile.calculate(0.02, setpoint, goal); } - inputs.elevatorCurrent = leftElevatorMotor.getOutputCurrent(); - inputs.elevatorAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); - - inputs.elevatorMotorPositionMeters = leftEncoder.getPosition(); - inputs.elevatorMotorPositionMeters = rightEncoder.getPosition(); - inputs.elevatorMotorVelocityMetersPerSecond = leftEncoder.getVelocity(); - inputs.elevatorMotorVelocityMetersPerSecond = rightEncoder.getVelocity(); - // inputs.elevatorMotorPositionMeters = leftAbsoluteEncoder.getPosition(); - // inputs.elevatorMotorVelocityMetersPerSecond = leftAbsoluteEncoder.getVelocity(); will use absolute encoder later - inputs.elevatorInputVolts = inputVolts; + inputs.elevatorRightCurrent = rightElevatorMotor.getOutputCurrent(); + inputs.elevatorRightAppliedVolts = rightElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); + inputs.elevatorRightPositionMeters = rightEncoder.getPosition(); + inputs.elevatorRightVelocityMetersPerSecond = rightEncoder.getVelocity(); + // inputs.elevatorRightMotorPositionMeters = leftAbsoluteEncoder.getPosition(); + // inputs.elevatorRightMotorVelocityMetersPerSecond = leftAbsoluteEncoder.getVelocity(); will use absolute encoder later + inputs.elevatorRightInputVolts = inputVolts; + + + inputs.elevatorLeftCurrent = leftElevatorMotor.getOutputCurrent(); + inputs.elevatorLeftAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); + inputs.elevatorLeftPositionMeters = leftEncoder.getPosition(); + inputs.elevatorLeftVelocityMetersPerSecond = leftEncoder.getVelocity(); + inputs.elevatorLeftInputVolts = inputVolts; + Logger.recordOutput("Elevator/Setpoint/Position", setpoint.position); Logger.recordOutput("Elevator/Setpoint/Velocity", setpoint.velocity); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 3f8b7d0..c5daf88 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -37,13 +37,11 @@ public void updateInputs(ElevatorIOInputs inputs) { setpoint = profile.calculate(0.02, setpoint, goal); } - inputs.elevatorPositionMetersPerSecond = elevatorSim.getPositionMeters(); - inputs.elevatorMotorPositionMeters = elevatorSim.getPositionMeters(); - inputs.elevatorMotorVelocityMetersPerSecond = elevatorSim.getVelocityMetersPerSecond(); - inputs.elevatorCurrent = elevatorSim.getCurrentDrawAmps(); - inputs.elevatorAppliedVolts = elevatorAppliedVolts; + inputs.elevatorLeftPositionMeters = elevatorSim.getPositionMeters(); + inputs.elevatorLeftVelocityMetersPerSecond = elevatorSim.getVelocityMetersPerSecond(); + inputs.elevatorLeftCurrent = elevatorSim.getCurrentDrawAmps(); + inputs.elevatorRightAppliedVolts = elevatorAppliedVolts; elevatorSim.update(0.02); - } @Override From 62f04d4c92ded50ce269b0c4653659fd9a5259b0 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Wed, 5 Feb 2025 12:32:49 -0500 Subject: [PATCH 079/110] arm offset and voltage inputs --- src/main/java/frc/robot/subsystems/arm/ArmIOReal.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 7f35b3c..dee3937 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -67,14 +67,14 @@ public void resetAbsoluteEncoder() { public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { // setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity) + controller.calculate(inputs.angularVelocity, setpoint.velocity)); - setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity + ArmConstants.ARM_OFFSET)); + setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity - ArmConstants.ARM_OFFSET)); setpoint = profile.calculate(0.02, setpoint, goal); } inputs.angularPosition = armMotor.getAbsoluteEncoder().getPosition(); inputs.angularVelocity = armEncoder.getVelocity(); inputs.current = armMotor.getOutputCurrent(); - inputs.voltage = armMotor.getAppliedOutput(); + inputs.voltage = armMotor.getAppliedOutput() * armMotor.getBusVoltage(); inputs.setpointVelocity = setpoint.velocity; } From 3215530025157684e624c455d0893b761269219b Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Wed, 5 Feb 2025 21:03:10 -0500 Subject: [PATCH 080/110] onboard arm PID --- .../frc/robot/subsystems/arm/ArmIOReal.java | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index dee3937..73d6a92 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -3,8 +3,11 @@ import java.util.concurrent.atomic.AtomicReferenceFieldUpdater; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -23,6 +26,7 @@ public class ArmIOReal implements ArmIO { private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); private final PIDController controller = new PIDController(0.04, 0,0.00); + private final SparkClosedLoopController onboardController = armMotor.getClosedLoopController(); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 2); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State goal = new TrapezoidProfile.State(0, 0); @@ -31,16 +35,23 @@ public class ArmIOReal implements ArmIO { public ArmIOReal(){ config.smartCurrentLimit(ArmConstants.CURRENT_LIMIT); config.idleMode(IdleMode.kBrake); - config.absoluteEncoder.positionConversionFactor(1); + config.absoluteEncoder.positionConversionFactor(Math.PI); config.absoluteEncoder.velocityConversionFactor(Math.PI / 60); config.absoluteEncoder.inverted(false); + config.encoder.positionConversionFactor(Math.PI); config.encoder.velocityConversionFactor(Math.PI / 60); + config.softLimit.forwardSoftLimitEnabled(true); config.softLimit.reverseSoftLimitEnabled(false); config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); + + config.closedLoop.p(0.04); + config.closedLoop.i(0.0); + config.closedLoop.d(0.0); + armMotor.clearFaults(); armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -66,8 +77,9 @@ public void resetAbsoluteEncoder() { @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { - // setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity) + controller.calculate(inputs.angularVelocity, setpoint.velocity)); - setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity - ArmConstants.ARM_OFFSET)); + // setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity)); + double ffvolts = ffmodel.calculate(setpoint.position, setpoint.velocity); + onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); setpoint = profile.calculate(0.02, setpoint, goal); } @@ -79,6 +91,8 @@ public void updateInputs(ArmIOInputs inputs) { } public void setVoltage(double voltage) { - armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); + double ffvolts = ffmodel.calculate(armEncoder.getPosition() , 0); + onboardController.setReference(ffvolts, ControlType.kVoltage); + // armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); } } From 129122a7d7e376776c29cdbd93e27d7526c059db Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Thu, 6 Feb 2025 13:03:27 -0500 Subject: [PATCH 081/110] new arm controls and fixed dumb voltage mistake --- src/main/java/frc/robot/RobotContainer.java | 36 +++++++++++++------ .../java/frc/robot/subsystems/arm/Arm.java | 1 + .../java/frc/robot/subsystems/arm/ArmIO.java | 2 ++ .../frc/robot/subsystems/arm/ArmIOReal.java | 9 +++-- 4 files changed, 35 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2631126..16821fa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -166,20 +166,34 @@ private void configureBindings() { ); - OIConstants.driverController.leftTrigger().whileTrue( - Commands.startEnd( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), - () -> Arm.getInstance().manualVoltage(0), - Arm.getInstance() - )); + // OIConstants.driverController.leftTrigger().whileTrue( + // Commands.startEnd( + // () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), + // () -> Arm.getInstance().manualVoltage(0), + // Arm.getInstance() + // )); + + // OIConstants.driverController.rightTrigger().whileTrue( + // Commands.startEnd( + // () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), + // () -> Arm.getInstance().manualVoltage(0), + // Arm.getInstance() + // )); + OIConstants.driverController.leftTrigger().whileTrue( + Commands.run( + () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), + Arm.getInstance() + ) + ); + OIConstants.driverController.rightTrigger().whileTrue( - Commands.startEnd( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), - () -> Arm.getInstance().manualVoltage(0), - Arm.getInstance() - )); + Commands.run( + () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * -6), + Arm.getInstance() + ) + ); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index a8c6119..b831bae 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -22,6 +22,7 @@ public class Arm extends SubsystemBase { private Arm(ArmIO io) { this.io = io; + setDefaultCommand(Commands.run(() -> io.hold())); } public double getAngle() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 0e25798..ba81db1 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -29,6 +29,8 @@ public default void setVoltage(double voltage) { public default void setGoal(double angle) { } + public default void hold() {} + public default void setFFValues(double kS, double kG, double kV, double kA) { } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 73d6a92..a27baa3 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -69,6 +69,12 @@ public void setFFValues(double kS, double kG, double kV, double kA) { ffmodel = new ArmFeedforward(kS, kG, kV); } + @Override + public void hold() { + double ffvolts = ffmodel.calculate(armEncoder.getPosition(), 0); + onboardController.setReference(ffvolts, ControlType.kVoltage); + } + @Override public void resetAbsoluteEncoder() { armMotor.getEncoder().setPosition(0); @@ -91,8 +97,7 @@ public void updateInputs(ArmIOInputs inputs) { } public void setVoltage(double voltage) { - double ffvolts = ffmodel.calculate(armEncoder.getPosition() , 0); - onboardController.setReference(ffvolts, ControlType.kVoltage); + onboardController.setReference(voltage, ControlType.kVoltage); // armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); } } From c6c56798dfb4f5d0a814d08f4e01bf1820b6b73e Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Thu, 6 Feb 2025 14:14:51 -0500 Subject: [PATCH 082/110] use encoder position instead of setpoint position in motion profile --- src/main/java/frc/robot/subsystems/arm/ArmIOReal.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index a27baa3..fb19166 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -48,7 +48,7 @@ public ArmIOReal(){ config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); - config.closedLoop.p(0.04); + config.closedLoop.p(0.0); config.closedLoop.i(0.0); config.closedLoop.d(0.0); @@ -84,7 +84,7 @@ public void resetAbsoluteEncoder() { public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { // setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity)); - double ffvolts = ffmodel.calculate(setpoint.position, setpoint.velocity); + double ffvolts = ffmodel.calculate(armEncoder.getPosition(), setpoint.velocity); onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); setpoint = profile.calculate(0.02, setpoint, goal); } From 1e156b5a244eab00c7ac66e0010a422f1f6d607d Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Thu, 6 Feb 2025 15:55:04 -0500 Subject: [PATCH 083/110] some of amals changes my computer is about to die changed elevator constants, intake, and made the arm set goals (no setpoints yet) --- src/main/java/frc/robot/RobotContainer.java | 54 ++++++++++--------- .../robot/subsystems/arm/ArmConstants.java | 4 ++ .../robot/subsystems/elevator/Elevator.java | 10 ---- .../elevator/ElevatorConstants.java | 8 +-- .../subsystems/elevator/ElevatorIOReal.java | 2 +- .../subsystems/elevator/ElevatorIOSim.java | 2 +- .../frc/robot/subsystems/intake/IntakeIO.java | 2 - 7 files changed, 38 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16821fa..d2a15d0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; @@ -149,37 +150,38 @@ private void configureBindings() { Constants.OIConstants.operatorController.a().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(0.5), () -> intake.setIntakeSpeed(0), intake)); - OIConstants.driverController.povUp().whileTrue( - Commands.startEnd( - () -> Arm.getInstance().manualVoltage(9), - () -> Arm.getInstance().manualVoltage(ArmConstants.DEFAULTkG * Math.cos(Arm.getInstance().getAngle() - Math.PI/2)), - Arm.getInstance() - ) - ); + // OIConstants.driverController.povUp().whileTrue( + // Commands.startEnd( + // () -> Arm.getInstance().manualVoltage(9), + // () -> Arm.getInstance().manualVoltage(ArmConstants.DEFAULTkG * Math.cos(Arm.getInstance().getAngle() - Math.PI/2)), + // Arm.getInstance() + // ) + // ); - OIConstants.driverController.povDown().whileTrue( - Commands.startEnd( - () -> Arm.getInstance().manualVoltage(-9), - () -> Arm.getInstance().manualVoltage(ArmConstants.DEFAULTkG * Math.cos(Arm.getInstance().getAngle() - Math.PI/2)), - Arm.getInstance() - ) - ); + OIConstants.driverController.povDown().onTrue( + new InstantCommand(() -> Arm.getInstance().setGoal(0))); + + OIConstants.driverController.povRight().onTrue( + new InstantCommand(() -> Arm.getInstance().setGoal(0))); + OIConstants.driverController.povUp().onTrue( + new InstantCommand(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L3))); - // OIConstants.driverController.leftTrigger().whileTrue( - // Commands.startEnd( - // () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), - // () -> Arm.getInstance().manualVoltage(0), - // Arm.getInstance() - // )); + OIConstants.driverController.leftTrigger().whileTrue( + Commands.startEnd( + () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), + () -> Arm.getInstance().manualVoltage(0), + Arm.getInstance() + )); - // OIConstants.driverController.rightTrigger().whileTrue( - // Commands.startEnd( - // () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), - // () -> Arm.getInstance().manualVoltage(0), - // Arm.getInstance() - // )); + + OIConstants.driverController.rightTrigger().whileTrue( + Commands.startEnd( + () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), + () -> Arm.getInstance().manualVoltage(0), + Arm.getInstance() + )); OIConstants.driverController.leftTrigger().whileTrue( Commands.run( diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index eb51449..b3df15c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -7,6 +7,10 @@ public class ArmConstants { public static final int CURRENT_LIMIT = 50; public static final double ARM_OFFSET = Math.PI/2; + public static final double CORAL_L2 = 0; + public static final double CORAL_L3 = 54.1 * (Math.PI/180); + + // Default FF public static final double DEFAULTkG = 1.02; public static final double DEFAULTkV = 1.01; diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 90936b5..58f5216 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -35,14 +35,6 @@ public class Elevator extends SubsystemBase { private double maxDistance = ElevatorConstants.maxDistance; private static Elevator instance; - - private PIDController controller = new PIDController( - ElevatorConstants.kP, - ElevatorConstants.kI, - ElevatorConstants.kD); - - private ElevatorFeedforward feedforward = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg, ElevatorConstants.kv); - public static Elevator getInstance(){ return instance; @@ -96,7 +88,6 @@ public double getVelocity() { } public void reset() { - controller.reset(); io.resetEncoders(); this.setGoal(getPosition()); } @@ -111,7 +102,6 @@ public void setVoltage(double volts) { public void periodic() { io.updateInputs(inputs); Logger.processInputs(getName(), inputs); - io.setVoltage(voltage); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 688752d..afa5de4 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -9,12 +9,12 @@ public final class ElevatorConstants{ public static final double kD = 0; //figure this out later public static final double kI = 0; //figure this out later - public static final double kP = 1; //figure this out later + public static final double kP = 0; //figure this out later public static final int rightDeviceID = 12; public static final int leftDeviceID = 13; public static final double ks = 0; - public static final double kg = 0.3; - public static final double kv = 15.96; //IS THIS RIGHT?!?!? + public static final double kg = 0.38; + public static final double kv = 6.84; //IS THIS RIGHT?!?!? public static final double MAX_ACCELERATION = 1; public static final double MAX_VELOCITY = 1; @@ -22,7 +22,7 @@ public final class ElevatorConstants{ public static final double conversionFactor = 0.1; - public static final int elevatorCurrentLimits = 70; //might need to adjust later + public static final int elevatorCurrentLimits = 40; //might need to adjust later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 6ec0853..7c675bd 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -39,7 +39,7 @@ public class ElevatorIOReal implements ElevatorIO { private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(0, 0, 0); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION); - private final PIDController controller = new PIDController(1, 0, 0); + private final PIDController controller = new PIDController(ElevatorConstants.kP, ElevatorConstants.kI, ElevatorConstants.kP); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); private TrapezoidProfile.State goal = new TrapezoidProfile.State(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index c5daf88..33e5037 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -59,7 +59,7 @@ public void setVoltage(double volts){ @Override public void setGoal(double height) { - goal = new TrapezoidProfile.State(height, 0.1); + goal = new TrapezoidProfile.State(height, 0); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 62193a7..1cf1d54 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.intake; -import javax.annotation.processing.SupportedOptions; - import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { From 96ce230da83409739d16abba3e753c8f04fd1b12 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Thu, 6 Feb 2025 16:42:22 -0500 Subject: [PATCH 084/110] More merge request changes --- src/main/java/frc/robot/RobotContainer.java | 26 +++++++++---------- .../frc/robot/subsystems/arm/ArmIOReal.java | 1 - .../robot/subsystems/elevator/Elevator.java | 14 ++-------- .../elevator/ElevatorConstants.java | 4 +-- .../subsystems/elevator/ElevatorIOReal.java | 15 ++++++----- 5 files changed, 26 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2a15d0..e9d78b3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -168,20 +168,20 @@ private void configureBindings() { new InstantCommand(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L3))); - OIConstants.driverController.leftTrigger().whileTrue( - Commands.startEnd( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), - () -> Arm.getInstance().manualVoltage(0), - Arm.getInstance() - )); + // OIConstants.driverController.leftTrigger().whileTrue( + // Commands.startEnd( + // () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), + // () -> Arm.getInstance().manualVoltage(0), + // Arm.getInstance() + // )); - OIConstants.driverController.rightTrigger().whileTrue( - Commands.startEnd( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), - () -> Arm.getInstance().manualVoltage(0), - Arm.getInstance() - )); + // OIConstants.driverController.rightTrigger().whileTrue( + // Commands.startEnd( + // () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), + // () -> Arm.getInstance().manualVoltage(0), + // Arm.getInstance() + // )); OIConstants.driverController.leftTrigger().whileTrue( Commands.run( @@ -192,7 +192,7 @@ private void configureBindings() { OIConstants.driverController.rightTrigger().whileTrue( Commands.run( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * -6), + () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * -6 + ArmConstants.DEFAULTkG), Arm.getInstance() ) ); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index fb19166..eda859c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -25,7 +25,6 @@ public class ArmIOReal implements ArmIO { private SparkAbsoluteEncoder armEncoder; private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); - private final PIDController controller = new PIDController(0.04, 0,0.00); private final SparkClosedLoopController onboardController = armMotor.getClosedLoopController(); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 2); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 58f5216..1cbdf02 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -1,16 +1,10 @@ package frc.robot.subsystems.elevator; - -import java.io.DataOutput; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; @@ -73,17 +67,13 @@ public void moveElevator(double speed) { io.moveElevator(speed); } - public double getTruePosition() { - return getPosition(); - } - public double getPosition(){ - return maxDistance; //change later + return inputs.elevatorLeftPositionMeters; //change later } public double getVelocity() { - return maxDistance; //change later + return inputs.elevatorLeftVelocityMetersPerSecond; //change later } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index afa5de4..b97efd2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.elevator; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.util.Units; public final class ElevatorConstants{ public static final double maxDistance = 0.80; @@ -18,8 +19,7 @@ public final class ElevatorConstants{ public static final double MAX_ACCELERATION = 1; public static final double MAX_VELOCITY = 1; - //change this later - public static final double conversionFactor = 0.1; + public static final double conversionFactor = Units.inchesToMeters(1/9 * Math.PI * 1.751); // gearing * pi * sprocket diameter public static final int elevatorCurrentLimits = 40; //might need to adjust later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 7c675bd..12e003b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -15,6 +15,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkFlex; @@ -37,7 +38,7 @@ public class ElevatorIOReal implements ElevatorIO { // private AbsoluteEncoder leftAbsoluteEncoder = leftElevatorMotor.getAbsoluteEncoder(); - private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(0, 0, 0); + private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg, ElevatorConstants.kv); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION); private final PIDController controller = new PIDController(ElevatorConstants.kP, ElevatorConstants.kI, ElevatorConstants.kP); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); @@ -54,9 +55,6 @@ public class ElevatorIOReal implements ElevatorIO { //final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); - private boolean isEnabled; - private boolean hasPlayed = false; - //zero stuff public void zeroElevator(){ // double AbsolutePosition = leftAbsoluteEncoder.getPosition(); @@ -68,8 +66,8 @@ public void zeroElevator(){ public ElevatorIOReal(){ leftElevatorMotor.clearFaults(); rightElevatorMotor.clearFaults(); - config.encoder.positionConversionFactor(Units.inchesToMeters(11.0/18.0)); - config.encoder.velocityConversionFactor(Units.inchesToMeters(11.0/18.0)/60.0); + config.encoder.positionConversionFactor(ElevatorConstants.conversionFactor); + config.encoder.velocityConversionFactor(ElevatorConstants.conversionFactor / 60); config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); config.idleMode(IdleMode.kBrake); @@ -97,6 +95,11 @@ public void setVoltage(double voltage) { } + @Override + public void setGoal(double height) { + goal = new TrapezoidProfile.State(height, 0); + } + @Override public void updateInputs(ElevatorIOInputs inputs) { //might want to separate by motor or you can average if they don't need to be ran in reverse From ad2970d2bb112caca0ebaaf52e18744f5953855e Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Thu, 6 Feb 2025 17:08:43 -0500 Subject: [PATCH 085/110] Elevator onboard control, soft limits, and default command --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../robot/subsystems/elevator/Elevator.java | 37 +------- .../elevator/ElevatorConstants.java | 3 +- .../robot/subsystems/elevator/ElevatorIO.java | 5 +- .../subsystems/elevator/ElevatorIOReal.java | 87 ++++++------------- 5 files changed, 36 insertions(+), 102 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9d78b3..c9031a4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -119,17 +119,15 @@ private void configureBindings() { // elevator)); OIConstants.operatorController.rightBumper().whileTrue( - Commands.startEnd( + Commands.run( () -> elevator.setVoltage(4), - () -> elevator.setVoltage(-ElevatorConstants.kg), elevator ) ); OIConstants.operatorController.leftBumper().whileTrue( - Commands.startEnd( + Commands.run( () -> elevator.setVoltage(-4), - () -> elevator.setVoltage(-ElevatorConstants.kg), elevator ) ); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 1cbdf02..04cb420 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -11,22 +11,6 @@ public class Elevator extends SubsystemBase { private ElevatorIO io; private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); - private double voltage = 0; - - @AutoLogOutput - private double leftMovingSpeed = 0.0; - - @AutoLogOutput - private double rightMovingSpeed = 0.0; - - @AutoLogOutput - private double setGoal = 0.0; - - @AutoLogOutput - private double setPoint = 0.0; - - - private double maxDistance = ElevatorConstants.maxDistance; private static Elevator instance; @@ -44,17 +28,11 @@ public static Elevator initialize(ElevatorIO io){ private Elevator(ElevatorIO elevatorIO){ io = elevatorIO; io.updateInputs(inputs); - } - - - // public void setMovingSpeedRPM(double leftSpeed, double rightSpeed){ - // leftMovingSpeed = leftSpeed; - // rightMovingSpeed = rightSpeed; - // } + setDefaultCommand(Commands.run(() -> setVoltage(-ElevatorConstants.kg))); + } public Command setGoal(double goal){ - return Commands.runOnce(() -> setOpenLoop(false)).andThen(() -> io.setGoal(goal), this); } @@ -69,7 +47,6 @@ public void moveElevator(double speed) { public double getPosition(){ return inputs.elevatorLeftPositionMeters; //change later - } public double getVelocity() { @@ -85,7 +62,6 @@ public void reset() { public void setVoltage(double volts) { setOpenLoop(true); io.setVoltage(volts); - voltage = volts; } @Override @@ -93,11 +69,4 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs(getName(), inputs); } - - } - - - - - - +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index b97efd2..904ee19 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -4,7 +4,8 @@ import edu.wpi.first.math.util.Units; public final class ElevatorConstants{ - public static final double maxDistance = 0.80; + public static final double maxDistance = 0.7112; + public static final double minDistance = 0.0; // public static final double constraints = 0.2; //figure this out later public static final Constraints constraints = new Constraints(4, 5); //change later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 152ada3..6608e9d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -5,18 +5,19 @@ public interface ElevatorIO { @AutoLog public static class ElevatorIOInputs { - public double elevatorRightInputVolts = 0.0; public double elevatorRightAppliedVolts = 0.0; public double elevatorRightPositionMeters= 0.0; public double elevatorRightVelocityMetersPerSecond = 0.0; public double elevatorRightCurrent = 0.0; - public double elevatorLeftInputVolts = 0.0; public double elevatorLeftAppliedVolts = 0.0; public double elevatorLeftPositionMeters = 0.0; public double elevatorLeftVelocityMetersPerSecond = 0.0; public double elevatorLeftCurrent = 0.0; + public double goalHeight = 0.0; + public double setpointVelocity = 0.0; + public boolean openLoop = true; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 12e003b..2854a18 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -2,62 +2,44 @@ import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.AngularVelocity; -import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.wpilibj.motorcontrol.Spark; - - public class ElevatorIOReal implements ElevatorIO { - private SparkFlex rightElevatorMotor = new SparkFlex(ElevatorConstants.rightDeviceID, MotorType.kBrushless); private SparkFlex leftElevatorMotor = new SparkFlex(ElevatorConstants.leftDeviceID, MotorType.kBrushless); private RelativeEncoder leftEncoder = leftElevatorMotor.getEncoder(); private RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder(); + private SparkClosedLoopController controller = rightElevatorMotor.getClosedLoopController(); + // private AbsoluteEncoder leftAbsoluteEncoder = leftElevatorMotor.getAbsoluteEncoder(); private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg, ElevatorConstants.kv); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION); - private final PIDController controller = new PIDController(ElevatorConstants.kP, ElevatorConstants.kI, ElevatorConstants.kP); + // private final PIDController controller = new PIDController(ElevatorConstants.kP, ElevatorConstants.kI, ElevatorConstants.kD); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); private TrapezoidProfile.State goal = new TrapezoidProfile.State(); - - private double minPositionMeters = 0.0; - private double maxPositionMeters = 0.7112; - private SparkFlexConfig config = new SparkFlexConfig(); - - private double inputVolts = 0.0; - - //final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); //zero stuff public void zeroElevator(){ - // double AbsolutePosition = leftAbsoluteEncoder.getPosition(); leftEncoder.setPosition(0); rightEncoder.setPosition(0); } @@ -68,33 +50,32 @@ public ElevatorIOReal(){ rightElevatorMotor.clearFaults(); config.encoder.positionConversionFactor(ElevatorConstants.conversionFactor); config.encoder.velocityConversionFactor(ElevatorConstants.conversionFactor / 60); + + config.softLimit.forwardSoftLimitEnabled(true); + config.softLimit.reverseSoftLimitEnabled(true); + config.softLimit.forwardSoftLimit(ElevatorConstants.maxDistance); + config.softLimit.reverseSoftLimit(ElevatorConstants.minDistance); + + config.closedLoop.p(0); + config.closedLoop.i(0); + config.closedLoop.d(0); + config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); config.idleMode(IdleMode.kBrake); rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); config.follow(ElevatorConstants.rightDeviceID, true); - - - // config.absoluteEncoder.positionConversionFactor(Units.inchesToMeters(11 / 18)); use these later - // config.absoluteEncoder.velocityConversionFactor(Units.inchesToMeters(11 / 18) / 60); use these later - leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - -} + // config.absoluteEncoder.positionConversionFactor(Units.inchesToMeters(11 / 18)); use these later + // config.absoluteEncoder.velocityConversionFactor(Units.inchesToMeters(11 / 18) / 60); use these later + leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } @Override public void setVoltage(double voltage) { - // double currentPosition = leftEncoder.getPosition(); // Relative encoder position - // if (currentPosition <= minPositionMeters && voltage < 0) { - // voltage = 0; - // } else if (currentPosition >= maxPositionMeters && voltage > 0) { - // voltage = 0; - // } - inputVolts = voltage; - rightElevatorMotor.setVoltage(voltage); + controller.setReference(voltage, ControlType.kVoltage); } - @Override public void setGoal(double height) { goal = new TrapezoidProfile.State(height, 0); @@ -102,48 +83,32 @@ public void setGoal(double height) { @Override public void updateInputs(ElevatorIOInputs inputs) { - //might want to separate by motor or you can average if they don't need to be ran in reverse - if(!inputs.openLoop){ - setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(setpoint.velocity)); + double ffvolts = ffmodel.calculate(setpoint.velocity); + controller.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); setpoint = profile.calculate(0.02, setpoint, goal); } + inputs.elevatorRightCurrent = rightElevatorMotor.getOutputCurrent(); inputs.elevatorRightAppliedVolts = rightElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); inputs.elevatorRightPositionMeters = rightEncoder.getPosition(); inputs.elevatorRightVelocityMetersPerSecond = rightEncoder.getVelocity(); // inputs.elevatorRightMotorPositionMeters = leftAbsoluteEncoder.getPosition(); // inputs.elevatorRightMotorVelocityMetersPerSecond = leftAbsoluteEncoder.getVelocity(); will use absolute encoder later - inputs.elevatorRightInputVolts = inputVolts; inputs.elevatorLeftCurrent = leftElevatorMotor.getOutputCurrent(); inputs.elevatorLeftAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); inputs.elevatorLeftPositionMeters = leftEncoder.getPosition(); inputs.elevatorLeftVelocityMetersPerSecond = leftEncoder.getVelocity(); - inputs.elevatorLeftInputVolts = inputVolts; - - Logger.recordOutput("Elevator/Setpoint/Position", setpoint.position); - Logger.recordOutput("Elevator/Setpoint/Velocity", setpoint.velocity); - - // log goal after updating - Logger.recordOutput("Elevator/Goal/Position", goal.position); - Logger.recordOutput("Elevator/Goal/Velocity", goal.velocity); - - Logger.recordOutput("Elevator/Left Motor", leftEncoder.getPosition()); - Logger.recordOutput("Elevator/Right Motor", rightEncoder.getPosition()); - + inputs.setpointVelocity = setpoint.velocity; + inputs.goalHeight = goal.position; } @Override public void moveElevator(double speed) { rightElevatorMotor.set(speed); } - - public void setSoftLimits(double min, double max) { - this.minPositionMeters = min; - this.maxPositionMeters = max; - } - } +} From e9178a4dbdd907ac2d425dad2c57d0a71195176d Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Thu, 6 Feb 2025 17:24:09 -0500 Subject: [PATCH 086/110] Use absolute encoder for onboard pid on arm --- .../java/frc/robot/subsystems/arm/Arm.java | 6 ------ .../robot/subsystems/arm/ArmConstants.java | 2 ++ .../frc/robot/subsystems/arm/ArmIOReal.java | 21 +++++++++---------- 3 files changed, 12 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index b831bae..f0c798a 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -1,16 +1,10 @@ package frc.robot.subsystems.arm; -import java.util.Arrays; - -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.Gamepiece; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index b3df15c..6a08688 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -10,6 +10,8 @@ public class ArmConstants { public static final double CORAL_L2 = 0; public static final double CORAL_L3 = 54.1 * (Math.PI/180); + public static final double MAX_VELOCITY = 2; + public static final double MAX_ACCELERATION = 2; // Default FF public static final double DEFAULTkG = 1.02; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index eda859c..cef7081 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -12,6 +12,7 @@ import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.MathUtil; @@ -26,7 +27,7 @@ public class ArmIOReal implements ArmIO { private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); private final SparkClosedLoopController onboardController = armMotor.getClosedLoopController(); - private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 2); + private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ArmConstants.MAX_VELOCITY, ArmConstants.MAX_ACCELERATION); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State goal = new TrapezoidProfile.State(0, 0); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); @@ -39,14 +40,12 @@ public ArmIOReal(){ config.absoluteEncoder.velocityConversionFactor(Math.PI / 60); config.absoluteEncoder.inverted(false); - config.encoder.positionConversionFactor(Math.PI); - config.encoder.velocityConversionFactor(Math.PI / 60); - config.softLimit.forwardSoftLimitEnabled(true); config.softLimit.reverseSoftLimitEnabled(false); config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); + config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); config.closedLoop.p(0.0); config.closedLoop.i(0.0); config.closedLoop.d(0.0); @@ -55,7 +54,6 @@ public ArmIOReal(){ armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); armEncoder = armMotor.getAbsoluteEncoder(); - armMotor.getEncoder().setPosition(armEncoder.getPosition()); } @Override @@ -79,6 +77,12 @@ public void resetAbsoluteEncoder() { armMotor.getEncoder().setPosition(0); } + @Override + public void setVoltage(double voltage) { + onboardController.setReference(voltage, ControlType.kVoltage); + // armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); + } + @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { @@ -88,15 +92,10 @@ public void updateInputs(ArmIOInputs inputs) { setpoint = profile.calculate(0.02, setpoint, goal); } - inputs.angularPosition = armMotor.getAbsoluteEncoder().getPosition(); + inputs.angularPosition = armEncoder.getPosition(); inputs.angularVelocity = armEncoder.getVelocity(); inputs.current = armMotor.getOutputCurrent(); inputs.voltage = armMotor.getAppliedOutput() * armMotor.getBusVoltage(); inputs.setpointVelocity = setpoint.velocity; } - - public void setVoltage(double voltage) { - onboardController.setReference(voltage, ControlType.kVoltage); - // armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); - } } From ad7a16756b7112075b89e719a3a41ed03c8ea396 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Thu, 6 Feb 2025 17:36:48 -0500 Subject: [PATCH 087/110] Arm setpoints --- src/main/java/frc/robot/subsystems/arm/ArmConstants.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 6a08688..b270897 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -7,8 +7,10 @@ public class ArmConstants { public static final int CURRENT_LIMIT = 50; public static final double ARM_OFFSET = Math.PI/2; - public static final double CORAL_L2 = 0; - public static final double CORAL_L3 = 54.1 * (Math.PI/180); + public static final double CORAL_L1 = Units.degreesToRadians(-34.74); + public static final double CORAL_L2 = Units.degreesToRadians(-54.1); // with elevator up + public static final double CORAL_L3 = Units.degreesToRadians(54.1); + public static final double CORAL_L4 = Units.degreesToRadians(54.61) public static final double MAX_VELOCITY = 2; public static final double MAX_ACCELERATION = 2; From 0796b756de62efbed602d56778043d9e8c4a3e4f Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Thu, 6 Feb 2025 18:14:26 -0500 Subject: [PATCH 088/110] onboard turnPID added --- .../java/frc/robot/subsystems/drive/Module.java | 16 ++++++++-------- .../robot/subsystems/drive/ModuleIOTalonFX.java | 3 --- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index fdb262a..fe43568 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -16,7 +16,7 @@ public class Module { private final int index; private final SimpleMotorFeedforward drivFeedforward; private final PIDController driveFeddbackPidController; - private final PIDController turnFeePidController; + private final PIDController turnFeedPidController; private Rotation2d angleSetpoint = null; private Double speedSetPoint = null; private Rotation2d turnRelativeOffset = null; @@ -29,26 +29,26 @@ public Module(ModuleIO io, int index) { case REAL: drivFeedforward = new SimpleMotorFeedforward(DriveConstants.TranslationKS, DriveConstants.TranslationKV); driveFeddbackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD); - turnFeePidController = new PIDController(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD); + turnFeedPidController = new PIDController(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD); break; case REPLAY: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); - turnFeePidController = new PIDController(7, 0.0, 0.0); + turnFeedPidController = new PIDController(7, 0.0, 0.0); break; case SIM: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); - turnFeePidController = new PIDController(10, 0.0, 0.0); + turnFeedPidController = new PIDController(10, 0.0, 0.0); break; default: drivFeedforward = new SimpleMotorFeedforward(0.0, 0.0); driveFeddbackPidController = new PIDController(0.0, 0.0, 0.0); - turnFeePidController = new PIDController(0.0, 0.0, 0.0); + turnFeedPidController = new PIDController(0.0, 0.0, 0.0); break; } - turnFeePidController.enableContinuousInput(-Math.PI, Math.PI); + turnFeedPidController.enableContinuousInput(-Math.PI, Math.PI); setBrakeMode(true); } @@ -61,11 +61,11 @@ public void periodic() { } if (angleSetpoint != null) { - io.setTurnVoltage(turnFeePidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + io.setTurnVoltage(turnFeedPidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); } if (speedSetPoint != null) { - double adjustSpeedSetpoint = speedSetPoint * Math.cos(turnFeePidController.getPositionError()); + double adjustSpeedSetpoint = speedSetPoint * Math.cos(turnFeedPidController.getPositionError()); double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS; io.setDriveVoltage( diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 9bf8a35..fa9faa4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -127,9 +127,6 @@ public ModuleIOTalonFX(int index) { driveTalonFX.setPosition(0); driveTalonFX.setInverted(true); - encoderconfig.positionConversionFactor(2 * Math.PI); - encoderconfig.velocityConversionFactor(2 * Math.PI / 60); - turnSparkFlex.setCANTimeout(0); From da942b3b1669ab152e87cba2c3bcd2169d88f305 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Thu, 6 Feb 2025 20:13:52 -0500 Subject: [PATCH 089/110] onboard turning PID --- .../frc/robot/subsystems/drive/Module.java | 17 +++++++------- .../frc/robot/subsystems/drive/ModuleIO.java | 8 +++++++ .../subsystems/drive/ModuleIOTalonFX.java | 23 +++++++++++++++---- 3 files changed, 35 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index fe43568..17343d6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -16,10 +16,10 @@ public class Module { private final int index; private final SimpleMotorFeedforward drivFeedforward; private final PIDController driveFeddbackPidController; - private final PIDController turnFeedPidController; private Rotation2d angleSetpoint = null; private Double speedSetPoint = null; private Rotation2d turnRelativeOffset = null; + public Module(ModuleIO io, int index) { this.io = io; @@ -29,27 +29,26 @@ public Module(ModuleIO io, int index) { case REAL: drivFeedforward = new SimpleMotorFeedforward(DriveConstants.TranslationKS, DriveConstants.TranslationKV); driveFeddbackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD); - turnFeedPidController = new PIDController(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD); break; case REPLAY: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); - turnFeedPidController = new PIDController(7, 0.0, 0.0); + // turnOnboardPIDController = new PIDController(7, 0.0, 0.0); break; case SIM: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); - turnFeedPidController = new PIDController(10, 0.0, 0.0); + // turnOnboardPIDController = new PIDController(10, 0.0, 0.0); break; default: drivFeedforward = new SimpleMotorFeedforward(0.0, 0.0); driveFeddbackPidController = new PIDController(0.0, 0.0, 0.0); - turnFeedPidController = new PIDController(0.0, 0.0, 0.0); + // turnOnboardPIDController = new PIDController(0.0, 0.0, 0.0); break; } - turnFeedPidController.enableContinuousInput(-Math.PI, Math.PI); - setBrakeMode(true); + // turnOnboardPIDController.enableContinuousInput(-Math.PI, Math.PI); + // setBrakeMode(true); } public void periodic() { @@ -61,11 +60,11 @@ public void periodic() { } if (angleSetpoint != null) { - io.setTurnVoltage(turnFeedPidController.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + io.setTurnAngleReference(angleSetpoint);; } if (speedSetPoint != null) { - double adjustSpeedSetpoint = speedSetPoint * Math.cos(turnFeedPidController.getPositionError()); + double adjustSpeedSetpoint = speedSetPoint; // add equation when we have onboard DrivePid double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS; io.setDriveVoltage( diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index a562471..84ed3eb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -36,9 +36,17 @@ public default void setDesiredState(SwerveModuleState state) { public default void setTurnVoltage(double volts) { } + public default void setTurnAngleReference(Rotation2d angle){ + + } + public default void setDriveBrakeMode(boolean enable) { } public default void setTurnBrakeMode(boolean enable) { } + + public default void getError(double error){ + + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index fa9faa4..51951ad 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -20,6 +20,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.ExternalEncoderConfig; @@ -27,6 +28,7 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.geometry.Rotation2d; @@ -54,8 +56,8 @@ public class ModuleIOTalonFX implements ModuleIO { final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); - private final TalonFX driveTalonFX; - private final SparkFlex turnSparkFlex; + public final TalonFX driveTalonFX; + public final SparkFlex turnSparkFlex; private final RelativeEncoder turnRelativeEncoder; private final AbsoluteEncoder turnAbsoluteEncoder; @@ -63,6 +65,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final boolean isTurnMotorInverted = true; public static final SparkFlexConfig turningConfig = new SparkFlexConfig(); + public ModuleIOTalonFX(int index) { switch (index) { @@ -94,9 +97,10 @@ public ModuleIOTalonFX(int index) { break; default: throw new RuntimeException("Invalid module index"); + } - + turningConfig .idleMode(IdleMode.kBrake) .smartCurrentLimit(DriveConstants.turnCurrentLimit); @@ -110,6 +114,9 @@ public ModuleIOTalonFX(int index) { .outputRange(-1, 1) .positionWrappingEnabled(true) .positionWrappingInputRange(0, DriveConstants.turningFactor); + + + turnSparkFlex.setCANTimeout(250); @@ -206,6 +213,7 @@ private TalonFXConfiguration config() { return talonFXConfig; } + @Override public void setDriveVoltage(double volts) { driveTalonFX.setVoltage(volts); @@ -226,6 +234,13 @@ public void setTurnBrakeMode(boolean enable) { // turnSparkFlex.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); configSpark(enable ? IdleMode.kBrake : IdleMode.kCoast); } - + @Override + public void setTurnAngleReference(Rotation2d angle){ + turnSparkFlex.getClosedLoopController().setReference(angle.getRadians(), ControlType.kPosition); + } + @Override + public void getError(double error){ + turnSparkFlex.getClosedLoopController().getIAccum(); + } } \ No newline at end of file From b5f1864c8507b90e4cc3b42b2ba1a8a71dcbf3a4 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Fri, 7 Feb 2025 00:12:45 -0500 Subject: [PATCH 090/110] Turning configuration implemented and code cleanup --- src/main/java/frc/robot/Main.java | 1 - src/main/java/frc/robot/RobotContainer.java | 11 ----- .../robot/subsystems/arm/ArmConstants.java | 2 +- .../java/frc/robot/subsystems/arm/ArmIO.java | 2 - .../frc/robot/subsystems/arm/ArmIOReal.java | 5 -- .../subsystems/drive/DriveConstants.java | 4 +- .../subsystems/drive/ModuleIOTalonFX.java | 48 +++++++++++-------- .../robot/subsystems/elevator/Elevator.java | 1 - .../subsystems/elevator/ElevatorIOReal.java | 3 -- .../subsystems/elevator/ElevatorIOSim.java | 8 ---- .../frc/robot/subsystems/intake/Intake.java | 5 -- .../subsystems/intake/IntakeConstants.java | 2 - .../robot/subsystems/intake/IntakeIOReal.java | 3 -- .../subsystems/intake/IntakeIORealVortex.java | 13 ----- .../robot/subsystems/intake/IntakeIOSim.java | 1 - 15 files changed, 30 insertions(+), 79 deletions(-) diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fb6ba2f..5585907 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -5,7 +5,6 @@ package frc.robot; import java.io.IOException; -import java.text.ParseException; import edu.wpi.first.wpilibj.RobotBase; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c9031a4..a75cb8f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,31 +17,20 @@ import frc.robot.subsystems.drive.GyroIOPigeon; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorConstants; -import frc.robot.subsystems.elevator.ElevatorIO; import frc.robot.subsystems.elevator.ElevatorIOReal; import frc.robot.subsystems.elevator.ElevatorIOSim; import frc.robot.Constants.Gamepiece; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.intake.IntakeIOReal; import frc.robot.subsystems.intake.IntakeIORealVortex; import frc.robot.subsystems.intake.IntakeIOSim; -import org.littletonrobotics.junction.Logger; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmConstants; -import frc.robot.subsystems.arm.ArmIO; import frc.robot.subsystems.arm.ArmIOSim; import frc.robot.subsystems.arm.ArmIOReal; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; - public class RobotContainer { private Elevator elevator; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index b270897..30d09b6 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -10,7 +10,7 @@ public class ArmConstants { public static final double CORAL_L1 = Units.degreesToRadians(-34.74); public static final double CORAL_L2 = Units.degreesToRadians(-54.1); // with elevator up public static final double CORAL_L3 = Units.degreesToRadians(54.1); - public static final double CORAL_L4 = Units.degreesToRadians(54.61) + public static final double CORAL_L4 = Units.degreesToRadians(54.61); public static final double MAX_VELOCITY = 2; public static final double MAX_ACCELERATION = 2; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index ba81db1..27ef5f4 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.arm; -import java.util.Arrays; - import org.littletonrobotics.junction.AutoLog; public interface ArmIO { @AutoLog diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index cef7081..6f17569 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -1,8 +1,5 @@ package frc.robot.subsystems.arm; -import java.util.concurrent.atomic.AtomicReferenceFieldUpdater; - -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkClosedLoopController; @@ -15,9 +12,7 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; public class ArmIOReal implements ArmIO { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 79a79d7..c25e3a0 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -4,7 +4,7 @@ public class DriveConstants { - public static final double MAX_LINEAR_SPEED = Units.metersToFeet(5); + public static final double MAX_LINEAR_SPEED = 17.14; public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); public static final double DRIVE_BASE_RADIUS = Math.hypot(DriveConstants.TRACK_LENGTH / 2.0, @@ -19,7 +19,7 @@ public class DriveConstants { public static final double RotationKP = 5.0; public static final double RotationKI = 0.0; public static final double RotationKD = 0.0; - public static final double WHEEL_RADIUS = Units.inchesToMeters(1.5); + public static final double WHEEL_RADIUS = Units.inchesToMeters(4); public static final double TurnGearing = 12.155818; public static final double DriveGearing = 6.11; public static final int turnCurrentLimit = 60; // May need to change later diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 51951ad..98de595 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -28,12 +28,10 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import frc.robot.commands.DriveCommands; /** * Module IO implementation for SparkMax drive motor controller, SparkMax turn @@ -64,7 +62,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final boolean isTurnMotorInverted = true; public static final SparkFlexConfig turningConfig = new SparkFlexConfig(); - + public static final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); public ModuleIOTalonFX(int index) { @@ -75,12 +73,15 @@ public ModuleIOTalonFX(int index) { turnSparkFlex = new SparkFlex(2, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); driveTalonFX.setInverted(false); + driveConfig.Audio.BeepOnBoot = true; + break; case 1: // Front right driveTalonFX = new TalonFX(3); turnSparkFlex = new SparkFlex(4, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); + driveConfig.Audio.BeepOnBoot = true; break; case 2: // Back left @@ -88,12 +89,14 @@ public ModuleIOTalonFX(int index) { turnSparkFlex = new SparkFlex(7, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); driveTalonFX.setInverted(false); + driveConfig.Audio.BeepOnBoot = true; break; case 3: // Back right driveTalonFX = new TalonFX(6); turnSparkFlex = new SparkFlex(5, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); + driveConfig.Audio.BeepOnBoot = true; break; default: throw new RuntimeException("Invalid module index"); @@ -114,6 +117,9 @@ public ModuleIOTalonFX(int index) { .outputRange(-1, 1) .positionWrappingEnabled(true) .positionWrappingInputRange(0, DriveConstants.turningFactor); + + driveConfig + .Audio.BeepOnBoot = true; @@ -137,9 +143,9 @@ public ModuleIOTalonFX(int index) { turnSparkFlex.setCANTimeout(0); - var config = config(); - var configSpark = configSpark(IdleMode.kBrake); - configSpark.apply(encoderconfig); + var config = config(); + // var configSpark = configSpark(IdleMode.kBrake); + // configSpark.apply(encoderconfig); driveTalonFX.optimizeBusUtilization(); @@ -149,7 +155,7 @@ public ModuleIOTalonFX(int index) { REVLibError status2 = REVLibError.kError; // not work maybe for (int i = 0; i < 5; i++) { status = driveTalonFX.getConfigurator().apply(config); - turnSparkFlex.configure(configSpark, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + turnSparkFlex.configure(turningConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); if (status.isOK()) break; } @@ -181,18 +187,18 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnCurrentAmps = new double[] { turnSparkFlex.getOutputCurrent() }; } - private SparkFlexConfig configSpark(IdleMode idle) { - var turningConfig = new SparkFlexConfig(); + // private SparkFlexConfig configSpark(IdleMode idle) { + // var turningConfig = new SparkFlexConfig(); - turningConfig.smartCurrentLimit(DriveConstants.turnCurrentLimit); - turningConfig.voltageCompensation(12.0); + // turningConfig.smartCurrentLimit(DriveConstants.turnCurrentLimit); + // turningConfig.voltageCompensation(12.0); - if (idle != null) { - turningConfig.idleMode(idle); - } + // if (idle != null) { + // turningConfig.idleMode(); + // } - return turningConfig; - } + // return turningConfig; + // } private TalonFXConfiguration config() { var talonFXConfig = new TalonFXConfiguration(); @@ -229,11 +235,11 @@ public void setTurnVoltage(double volts) { // driveTalonFX.setBrakeMode(enable ? IdleMode.kBrake : IdleMode.kCoast); // } - @Override - public void setTurnBrakeMode(boolean enable) { - // turnSparkFlex.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - configSpark(enable ? IdleMode.kBrake : IdleMode.kCoast); - } + // @Override + // public void setTurnBrakeMode(boolean enable) { + // // turnSparkFlex.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + // configSpark(enable ? IdleMode.kBrake : IdleMode.kCoast); + // } @Override public void setTurnAngleReference(Rotation2d angle){ turnSparkFlex.getClosedLoopController().setReference(angle.getRadians(), ControlType.kPosition); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 04cb420..55bc5b5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.elevator; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Commands; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 2854a18..019a6d9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -1,11 +1,8 @@ package frc.robot.subsystems.elevator; -import org.littletonrobotics.junction.Logger; - import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import com.revrobotics.spark.SparkBase.ControlType; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 33e5037..f4835bb 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,17 +1,9 @@ package frc.robot.subsystems.elevator; -import java.lang.System.Logger; - -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class ElevatorIOSim implements ElevatorIO { diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index fa4face..991a225 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,14 +1,9 @@ package frc.robot.subsystems.intake; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeSetpoints; -import frc.robot.subsystems.intake.IntakeIO.IntakeIOInputs; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggableInputs; public class Intake extends SubsystemBase { private IntakeIO io; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 0be4950..e92c1d1 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.intake; -import com.ctre.phoenix6.hardware.DeviceIdentifier; - public final class IntakeConstants { //PLACEHOLDERS public static final int canID = 18; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index d47dca7..eb1e706 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -4,12 +4,9 @@ import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.spark.SparkLimitSwitch; - import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java b/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java index 4121e2e..b583c82 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java @@ -1,26 +1,13 @@ package frc.robot.subsystems.intake; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLimitSwitch; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; - public class IntakeIORealVortex implements IntakeIO { private final SparkFlex intakeMotor = new SparkFlex(IntakeConstants.canID, MotorType.kBrushless); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 85d1db4..a8f25d9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.math.system.plant.LinearSystemId; From df1cf2ce1ee471e9510f71258f800d88e723d7fe Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Sat, 8 Feb 2025 11:07:13 -0500 Subject: [PATCH 091/110] onboard pid added --- .../subsystems/drive/DriveConstants.java | 12 +- .../frc/robot/subsystems/drive/Module.java | 36 +++-- .../frc/robot/subsystems/drive/ModuleIO.java | 3 +- .../subsystems/drive/ModuleIOTalonFX.java | 127 +++++++----------- 4 files changed, 77 insertions(+), 101 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index c25e3a0..2e725e5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -1,9 +1,12 @@ package frc.robot.subsystems.drive; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + import edu.wpi.first.math.util.Units; public class DriveConstants { - + public static final double MAX_LINEAR_SPEED = 17.14; public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); @@ -20,11 +23,14 @@ public class DriveConstants { public static final double RotationKI = 0.0; public static final double RotationKD = 0.0; public static final double WHEEL_RADIUS = Units.inchesToMeters(4); - public static final double TurnGearing = 12.155818; + public static final double TurnGearing = (468/35) * (10/11); public static final double DriveGearing = 6.11; public static final int turnCurrentLimit = 60; // May need to change later public static final int driveCurrentLimit = 60; public static final int pigeonID = 20; public static final double turningFactor = 2*Math.PI; - + public static final InvertedValue driveMotorInverted = InvertedValue.Clockwise_Positive; + public static final NeutralModeValue driveMotorNeutralMode = NeutralModeValue.Brake; + public static final double openLoopRamp = 0.25; + public static final double closedLoopRamp = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 17343d6..cbc567e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -15,7 +14,7 @@ public class Module { private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; private final SimpleMotorFeedforward drivFeedforward; - private final PIDController driveFeddbackPidController; + // private final PIDController driveFeedBackPidController; private Rotation2d angleSetpoint = null; private Double speedSetPoint = null; private Rotation2d turnRelativeOffset = null; @@ -28,27 +27,23 @@ public Module(ModuleIO io, int index) { switch (Constants.currentMode) { case REAL: drivFeedforward = new SimpleMotorFeedforward(DriveConstants.TranslationKS, DriveConstants.TranslationKV); - driveFeddbackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD); + // driveFeedBackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD); break; case REPLAY: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); - // turnOnboardPIDController = new PIDController(7, 0.0, 0.0); + // driveFeedBackPidController = new PIDController(0.05, 0.0, 0.0); break; case SIM: drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeddbackPidController = new PIDController(0.05, 0.0, 0.0); - // turnOnboardPIDController = new PIDController(10, 0.0, 0.0); + // driveFeedBackPidController = new PIDController(0.05, 0.0, 0.0); break; default: drivFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - driveFeddbackPidController = new PIDController(0.0, 0.0, 0.0); - // turnOnboardPIDController = new PIDController(0.0, 0.0, 0.0); + // driveFeedBackPidController = new PIDController(0.0, 0.0, 0.0); break; } - // turnOnboardPIDController.enableContinuousInput(-Math.PI, Math.PI); - // setBrakeMode(true); + } public void periodic() { @@ -63,14 +58,15 @@ public void periodic() { io.setTurnAngleReference(angleSetpoint);; } - if (speedSetPoint != null) { - double adjustSpeedSetpoint = speedSetPoint; // add equation when we have onboard DrivePid + // if (speedSetPoint != null) { + // double adjustSpeedSetpoint = speedSetPoint * Math.cos(io.getError()) ; // add equation with onboard DrivePid - double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS; - io.setDriveVoltage( - drivFeedforward.calculate(velocityRadPerSec) - + driveFeddbackPidController.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - } + + // double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS; + // io.setDriveVoltage( + // drivFeedforward.calculate(velocityRadPerSec) + // + driveFeedBackPidController.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + // } } public SwerveModuleState runSetpoint(SwerveModuleState state) { @@ -109,7 +105,7 @@ public Rotation2d getAngle() { } } - public double getVelocityRadPerSec() { + public double getVelocityMetersPerSec() { return inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS; } @@ -122,7 +118,7 @@ public SwerveModulePosition getPosition() { } public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityRadPerSec(), getAngle()); + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } public double getCharacterizationVelocity() { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 84ed3eb..0dd07cf 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -47,6 +47,7 @@ public default void setTurnBrakeMode(boolean enable) { } public default void getError(double error){ - + } + } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 98de595..632a1b8 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -13,10 +13,15 @@ package frc.robot.subsystems.drive; +import java.io.ObjectInputFilter.Config; + import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveModuleConstants; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.REVLibError; @@ -60,7 +65,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final RelativeEncoder turnRelativeEncoder; private final AbsoluteEncoder turnAbsoluteEncoder; - private final boolean isTurnMotorInverted = true; + public static final SparkFlexConfig turningConfig = new SparkFlexConfig(); public static final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); @@ -72,38 +77,31 @@ public ModuleIOTalonFX(int index) { driveTalonFX = new TalonFX(1); turnSparkFlex = new SparkFlex(2, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); - driveTalonFX.setInverted(false); - driveConfig.Audio.BeepOnBoot = true; - break; case 1: // Front right driveTalonFX = new TalonFX(3); turnSparkFlex = new SparkFlex(4, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); - driveConfig.Audio.BeepOnBoot = true; break; case 2: // Back left driveTalonFX = new TalonFX(8); turnSparkFlex = new SparkFlex(7, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); - driveTalonFX.setInverted(false); - driveConfig.Audio.BeepOnBoot = true; break; case 3: // Back right driveTalonFX = new TalonFX(6); turnSparkFlex = new SparkFlex(5, MotorType.kBrushless); turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); - driveConfig.Audio.BeepOnBoot = true; break; default: throw new RuntimeException("Invalid module index"); } - + // turning SparkFlex Configuration turningConfig .idleMode(IdleMode.kBrake) .smartCurrentLimit(DriveConstants.turnCurrentLimit); @@ -117,44 +115,47 @@ public ModuleIOTalonFX(int index) { .outputRange(-1, 1) .positionWrappingEnabled(true) .positionWrappingInputRange(0, DriveConstants.turningFactor); - - driveConfig - .Audio.BeepOnBoot = true; - - - - - turnSparkFlex.setCANTimeout(250); - - turnRelativeEncoder = turnSparkFlex.getEncoder(); // ??? + turnRelativeEncoder = turnSparkFlex.getEncoder(); // only getting one? final ExternalEncoderConfig encoderconfig = new ExternalEncoderConfig(); - encoderconfig.measurementPeriod(10); turnRelativeEncoder.setPosition(0.0); encoderconfig.measurementPeriod(10); encoderconfig.averageDepth(2); - turnSparkFlex.setInverted(isTurnMotorInverted); - - driveTalonFX.setPosition(0); - driveTalonFX.setInverted(true); - turnSparkFlex.setCANTimeout(0); - - - var config = config(); - // var configSpark = configSpark(IdleMode.kBrake); - // configSpark.apply(encoderconfig); + // Drive TalonFX Configuration + driveConfig.MotorOutput.Inverted = DriveConstants.driveMotorInverted; + driveConfig.MotorOutput.NeutralMode = DriveConstants.driveMotorNeutralMode; + driveConfig.Feedback.SensorToMechanismRatio = DriveConstants.DriveGearing; + + // May need to add Thresholds + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.CurrentLimits.StatorCurrentLimit = DriveConstants.driveCurrentLimit; + // driveConfig.Slot0 = new Slot0Configs().withKP(0).withKI(0).withKD(0) + driveConfig.Slot0.kP = DriveConstants.TranslationKP; + driveConfig.Slot0.kI = DriveConstants.TranslationKI; + driveConfig.Slot0.kD = DriveConstants.TranslationKD; + // driveConfig.Slot0.kS = DriveConstants.TranslationKS; + // driveConfig.Slot0.kV = DriveConstants.TranslationKV; + + //May need to change ramp and openloop + driveConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = DriveConstants.closedLoopRamp; + driveConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = DriveConstants.closedLoopRamp; + driveConfig.Audio.BeepOnBoot = true; + driveTalonFX.setPosition(0); + driveTalonFX.optimizeBusUtilization(); + driveTalonFX.clearStickyFaults(); + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DriveConstants.driveCurrentLimit; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DriveConstants.driveCurrentLimit; + - driveTalonFX.optimizeBusUtilization(); - driveTalonFX.clearStickyFaults(); StatusCode status = StatusCode.StatusCodeNotInitialized; - REVLibError status2 = REVLibError.kError; // not work maybe - for (int i = 0; i < 5; i++) { - status = driveTalonFX.getConfigurator().apply(config); + // REVLibError status2 = REVLibError.kError; // not work maybe + for (int i = 0; i < 5; i++) { + status = driveTalonFX.getConfigurator().apply(driveConfig); turnSparkFlex.configure(turningConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); if (status.isOK()) break; @@ -187,38 +188,6 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnCurrentAmps = new double[] { turnSparkFlex.getOutputCurrent() }; } - // private SparkFlexConfig configSpark(IdleMode idle) { - // var turningConfig = new SparkFlexConfig(); - - // turningConfig.smartCurrentLimit(DriveConstants.turnCurrentLimit); - // turningConfig.voltageCompensation(12.0); - - // if (idle != null) { - // turningConfig.idleMode(); - // } - - // return turningConfig; - // } - - private TalonFXConfiguration config() { - var talonFXConfig = new TalonFXConfiguration(); - - talonFXConfig.Slot0.kP = 1; - talonFXConfig.Slot0.kI = 0; - talonFXConfig.Slot0.kD = 0; - talonFXConfig.Slot0.kS = 0; - talonFXConfig.Slot0.kV = 0; - - talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfig.CurrentLimits.StatorCurrentLimit = DriveConstants.driveCurrentLimit; // CurrentLimits.drive; - - talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - talonFXConfig.Audio.BeepOnBoot = true; - - return talonFXConfig; - } - @Override public void setDriveVoltage(double volts) { @@ -230,16 +199,20 @@ public void setTurnVoltage(double volts) { turnSparkFlex.setVoltage(volts); } - // @Override - // public void setDriveBrakeMode(boolean enable) { - // driveTalonFX.setBrakeMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - // } + @Override + public void setDriveBrakeMode(boolean enable) { + if (driveConfig.MotorOutput.NeutralMode == NeutralModeValue.Brake){ + enable = true; + } else { + enable = false; + } + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turningConfig.idleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } - // @Override - // public void setTurnBrakeMode(boolean enable) { - // // turnSparkFlex.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - // configSpark(enable ? IdleMode.kBrake : IdleMode.kCoast); - // } @Override public void setTurnAngleReference(Rotation2d angle){ turnSparkFlex.getClosedLoopController().setReference(angle.getRadians(), ControlType.kPosition); @@ -248,5 +221,5 @@ public void setTurnAngleReference(Rotation2d angle){ public void getError(double error){ turnSparkFlex.getClosedLoopController().getIAccum(); } - + } \ No newline at end of file From 2708b3d03c7f6ef52c203ebbba4bf1a0efd98fdb Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sat, 8 Feb 2025 12:10:36 -0500 Subject: [PATCH 092/110] Elevator and arm setGoal commands --- simgui-ds.json | 20 ------------------- src/main/java/frc/robot/RobotContainer.java | 3 +++ .../java/frc/robot/subsystems/arm/Arm.java | 18 +++++++++++++---- .../robot/subsystems/elevator/Elevator.java | 14 +++++++++++-- 4 files changed, 29 insertions(+), 26 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 831b8e4..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -89,26 +89,6 @@ "povCount": 0 } ], - "robotJoysticks": [ - { - "guid": "78696e70757401000000000000000000", - "useGamepad": true - } - ], - "robotJoysticks": [ - { - "guid": "Keyboard0" - }, - { - "guid": "78696e70757401000000000000000000", - "useGamepad": true - } - ], - "robotJoysticks": [ - { - "guid": "03000000c82d00000631000014010000" - } - ], "robotJoysticks": [ { "guid": "78696e70757401000000000000000000", diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a75cb8f..a22c48f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -183,6 +183,9 @@ private void configureBindings() { Arm.getInstance() ) ); + + OIConstants.driverController.leftBumper().onTrue(Arm.getInstance().setGoal(ArmConstants.CORAL_L1)); + OIConstants.driverController.rightBumper().onTrue(Arm.getInstance().setGoal(ArmConstants.CORAL_L2)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index f0c798a..2e7772b 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -2,9 +2,11 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.Gamepiece; @@ -16,7 +18,7 @@ public class Arm extends SubsystemBase { private Arm(ArmIO io) { this.io = io; - setDefaultCommand(Commands.run(() -> io.hold())); + setDefaultCommand(Commands.run(() -> io.hold(), this)); } public double getAngle() { @@ -31,9 +33,17 @@ public static Arm getInstance() { return instance; } - public void setGoal(double angle) { - setOpenLoop(false); - io.setGoal(angle); + public Command setGoal(double angle) { + return Commands.startEnd( + () -> { + setOpenLoop(false); + io.setGoal(angle); + }, + () -> { + setOpenLoop(true); + }, + this + ).until(() -> MathUtil.isNear(angle, inputs.angularPosition, 0.008)); } public void resetAbsoluteEncoder() { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 55bc5b5..db6e90e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; @@ -28,11 +29,20 @@ private Elevator(ElevatorIO elevatorIO){ io = elevatorIO; io.updateInputs(inputs); - setDefaultCommand(Commands.run(() -> setVoltage(-ElevatorConstants.kg))); + setDefaultCommand(Commands.run(() -> setVoltage(-ElevatorConstants.kg), this)); } public Command setGoal(double goal){ - return Commands.runOnce(() -> setOpenLoop(false)).andThen(() -> io.setGoal(goal), this); + return Commands.startEnd( + () -> { + setOpenLoop(false); + io.setGoal(goal); + }, + () -> { + setOpenLoop(true); + }, + this + ).until(() -> MathUtil.isNear(goal, inputs.elevatorLeftPositionMeters, 0.008)); } public void setOpenLoop(boolean openLoop) { From 77756b5553800bafaa959cdb8f1eedde0f43c681 Mon Sep 17 00:00:00 2001 From: MikhailSaeed Date: Sat, 8 Feb 2025 13:25:04 -0500 Subject: [PATCH 093/110] updated control implementation and initialization --- src/main/java/frc/robot/Constants.java | 1 + .../frc/robot/Util/LoggedTunableNumber.java | 122 ++++++++++++++++++ .../subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/drive/Module.java | 47 +++++-- .../frc/robot/subsystems/drive/ModuleIO.java | 4 + .../robot/subsystems/drive/ModuleIOSim.java | 21 ++- .../subsystems/drive/ModuleIOTalonFX.java | 18 ++- 7 files changed, 185 insertions(+), 30 deletions(-) create mode 100644 src/main/java/frc/robot/Util/LoggedTunableNumber.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b1e8803..079fb1b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class Constants { + public static final boolean tuningMode = false; public static final class IntakeSetpoints{ public static final double intake = -0.6; public static final double slow = -0.3; diff --git a/src/main/java/frc/robot/Util/LoggedTunableNumber.java b/src/main/java/frc/robot/Util/LoggedTunableNumber.java new file mode 100644 index 0000000..23ecf6d --- /dev/null +++ b/src/main/java/frc/robot/Util/LoggedTunableNumber.java @@ -0,0 +1,122 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. +package frc.robot.Util; + +import frc.robot.Constants; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "/Tuning"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Constants.tuningMode) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Constants.tuningMode ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() { + return get(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 2e725e5..29dd609 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -23,7 +23,7 @@ public class DriveConstants { public static final double RotationKI = 0.0; public static final double RotationKD = 0.0; public static final double WHEEL_RADIUS = Units.inchesToMeters(4); - public static final double TurnGearing = (468/35) * (10/11); + public static final double TurnGearing = (468.0/35.0) * (10.0/11.0); public static final double DriveGearing = 6.11; public static final int turnCurrentLimit = 60; // May need to change later public static final int driveCurrentLimit = 60; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index cbc567e..431ab5b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -1,24 +1,39 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.Constants; - +import frc.robot.Util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; public class Module { + private static final LoggedTunableNumber drivekS = + new LoggedTunableNumber("Drive/Module/DrivekS"); +private static final LoggedTunableNumber drivekV = + new LoggedTunableNumber("Drive/Module/DrivekV"); +private static final LoggedTunableNumber drivekP = + new LoggedTunableNumber("Drive/Module/DrivekP"); + private static final LoggedTunableNumber drivekI = + new LoggedTunableNumber("Drive/Module/DrivekI"); + private static final LoggedTunableNumber drivekD = + new LoggedTunableNumber("Drive/Module/DrivekD"); +private static final LoggedTunableNumber turnkP = new LoggedTunableNumber("Drive/Module/TurnkP"); +private static final LoggedTunableNumber turnkI = new LoggedTunableNumber("Drive/Module/TurnkI"); +private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; - private final SimpleMotorFeedforward drivFeedforward; - // private final PIDController driveFeedBackPidController; + // private final SimpleMotorFeedforward drivFeedforward; + // private final PIDController driveFeedBackPidController; private Rotation2d angleSetpoint = null; private Double speedSetPoint = null; private Rotation2d turnRelativeOffset = null; + public Module(ModuleIO io, int index) { this.io = io; @@ -26,20 +41,28 @@ public Module(ModuleIO io, int index) { switch (Constants.currentMode) { case REAL: - drivFeedforward = new SimpleMotorFeedforward(DriveConstants.TranslationKS, DriveConstants.TranslationKV); - // driveFeedBackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD); - break; + drivekS.initDefault(0.014); + drivekV.initDefault(0.134); + drivekP.initDefault(0.1); + drivekI.initDefault(0); + drivekD.initDefault(0); + turnkP.initDefault(10); + turnkI.initDefault(0); + turnkD.initDefault(0); + // drivFeedforward = new SimpleMotorFeedforward(DriveConstants.TranslationKS, DriveConstants.TranslationKV); + // driveFeedBackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD); + break; case REPLAY: - drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - // driveFeedBackPidController = new PIDController(0.05, 0.0, 0.0); + // drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + // driveFeedBackPidController = new PIDController(0.05, 0.0, 0.0); break; case SIM: - drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - // driveFeedBackPidController = new PIDController(0.05, 0.0, 0.0); + // drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + // driveFeedBackPidController = new PIDController(0.05, 0.0, 0.0); break; default: - drivFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - // driveFeedBackPidController = new PIDController(0.0, 0.0, 0.0); + // drivFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + // driveFeedBackPidController = new PIDController(0.0, 0.0, 0.0); break; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 0dd07cf..7328731 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -50,4 +50,8 @@ public default void getError(double error){ } + public default void setDesiredSpeed(double speed){ + + } + } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index a0ad003..351682f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -1,22 +1,29 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.DCMotorSim; public class ModuleIOSim implements ModuleIO { - private static final double LOOP_PERIOD_SECS = 0.02; + private static final DCMotor driveMotorModel = DCMotor.getKrakenX60Foc(1); + private static final DCMotor turnMotorModle = DCMotor.getNeoVortex(1); + private static final double LOOP_PERIOD_SECS = 0.02; // change gearbox argument private DCMotorSim driveSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(4), 0.025, DriveConstants.DriveGearing), - DCMotor.getKrakenX60(4)); // needs to change + LinearSystemId.createDCMotorSystem(driveMotorModel, 0.025, DriveConstants.DriveGearing), + driveMotorModel); // needs to change private DCMotorSim turnSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(4), 0.004, DriveConstants.TurnGearing), - DCMotor.getNeoVortex(4)); // needs to change + LinearSystemId.createDCMotorSystem(turnMotorModle, 0.004, DriveConstants.TurnGearing), + turnMotorModle); // needs to change - private final Rotation2d turnAbsoluteInitPositon = new Rotation2d(Math.random() * 2.0 * Math.PI); + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private PIDController driveController = new PIDController(0, 0, 0); + private PIDController turnController = new PIDController(0, 0, 0); + private double driveFFVolts = 0; private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; @@ -29,7 +36,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; inputs.driveCurrentAmps = new double[] { Math.abs(driveSim.getCurrentDrawAmps()) }; - inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPositon); + // inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPositon); inputs.turnPosition = new Rotation2d(turnSim.getAngularPosition()); inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = new double[] { Math.abs(turnSim.getCurrentDrawAmps()) }; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 632a1b8..992136b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -13,18 +13,12 @@ package frc.robot.subsystems.drive; -import java.io.ObjectInputFilter.Config; - import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveModuleConstants; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; @@ -136,8 +130,8 @@ public ModuleIOTalonFX(int index) { driveConfig.Slot0.kP = DriveConstants.TranslationKP; driveConfig.Slot0.kI = DriveConstants.TranslationKI; driveConfig.Slot0.kD = DriveConstants.TranslationKD; - // driveConfig.Slot0.kS = DriveConstants.TranslationKS; - // driveConfig.Slot0.kV = DriveConstants.TranslationKV; + driveConfig.Slot0.kS = DriveConstants.TranslationKS; + driveConfig.Slot0.kV = DriveConstants.TranslationKV; //May need to change ramp and openloop driveConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = DriveConstants.closedLoopRamp; @@ -146,8 +140,7 @@ public ModuleIOTalonFX(int index) { driveTalonFX.setPosition(0); driveTalonFX.optimizeBusUtilization(); driveTalonFX.clearStickyFaults(); - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DriveConstants.driveCurrentLimit; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DriveConstants.driveCurrentLimit; + @@ -221,5 +214,10 @@ public void setTurnAngleReference(Rotation2d angle){ public void getError(double error){ turnSparkFlex.getClosedLoopController().getIAccum(); } + + @Override + public void setDesiredSpeed(double speed){ + + } } \ No newline at end of file From d31a4425555b65569605ce780b1ea0ac9470b31f Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Sat, 8 Feb 2025 17:27:22 -0500 Subject: [PATCH 094/110] Passed in new control --- .../robot/subsystems/drive/DriveConstants.java | 2 +- .../java/frc/robot/subsystems/drive/Module.java | 14 ++++++-------- .../robot/subsystems/drive/ModuleIOTalonFX.java | 17 +++++++---------- 3 files changed, 14 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 29dd609..c854402 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -28,7 +28,7 @@ public class DriveConstants { public static final int turnCurrentLimit = 60; // May need to change later public static final int driveCurrentLimit = 60; public static final int pigeonID = 20; - public static final double turningFactor = 2*Math.PI; + public static final double turningFactor = 2 * Math.PI; public static final InvertedValue driveMotorInverted = InvertedValue.Clockwise_Positive; public static final NeutralModeValue driveMotorNeutralMode = NeutralModeValue.Brake; public static final double openLoopRamp = 0.25; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 431ab5b..bfb452b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -43,7 +43,7 @@ public Module(ModuleIO io, int index) { case REAL: drivekS.initDefault(0.014); drivekV.initDefault(0.134); - drivekP.initDefault(0.1); + drivekP.initDefault(0.0); drivekI.initDefault(0); drivekD.initDefault(0); turnkP.initDefault(10); @@ -81,15 +81,13 @@ public void periodic() { io.setTurnAngleReference(angleSetpoint);; } - // if (speedSetPoint != null) { - // double adjustSpeedSetpoint = speedSetPoint * Math.cos(io.getError()) ; // add equation with onboard DrivePid + if (speedSetPoint != null) { + // double adjustSpeedSetpoint = speedSetPoint * Math.cos(io.getError()) ; // add equation with onboard DrivePid - // double velocityRadPerSec = adjustSpeedSetpoint / DriveConstants.WHEEL_RADIUS; - // io.setDriveVoltage( - // drivFeedforward.calculate(velocityRadPerSec) - // + driveFeedBackPidController.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - // } + double velocityRadPerSec = speedSetPoint / DriveConstants.WHEEL_RADIUS; + io.setDesiredSpeed(velocityRadPerSec); + } } public SwerveModuleState runSetpoint(SwerveModuleState state) { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 992136b..e1529af 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.AbsoluteEncoder; @@ -51,17 +52,17 @@ * "/Drive/ModuleX/TurnAbsolutePositionRad" */ public class ModuleIOTalonFX implements ModuleIO { - final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); + // final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); public final TalonFX driveTalonFX; public final SparkFlex turnSparkFlex; - private final RelativeEncoder turnRelativeEncoder; private final AbsoluteEncoder turnAbsoluteEncoder; public static final SparkFlexConfig turningConfig = new SparkFlexConfig(); public static final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); + public ModuleIOTalonFX(int index) { @@ -167,13 +168,12 @@ public ModuleIOTalonFX(int index) { public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = Units.rotationsToRadians(driveTalonFX.getPosition().getValueAsDouble()) / DriveConstants.DriveGearing; - inputs.driveVelocityRadPerSec = Units - .rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble()) + inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble()) / DriveConstants.DriveGearing; inputs.driveAppliedVolts = driveTalonFX.getMotorVoltage().getValueAsDouble(); inputs.driveCurrentAmps = new double[] { driveTalonFX.getSupplyCurrent().getValueAsDouble() }; - inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition()); + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsoluteEncoder.getPosition()); inputs.turnPosition = Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / DriveConstants.TurnGearing); inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) / DriveConstants.TurnGearing; @@ -210,14 +210,11 @@ public void setTurnBrakeMode(boolean enable) { public void setTurnAngleReference(Rotation2d angle){ turnSparkFlex.getClosedLoopController().setReference(angle.getRadians(), ControlType.kPosition); } - @Override - public void getError(double error){ - turnSparkFlex.getClosedLoopController().getIAccum(); - } @Override public void setDesiredSpeed(double speed){ - + final VelocityVoltage m_request = new VelocityVoltage(speed); + driveTalonFX.setControl(m_request); } } \ No newline at end of file From bf9e1b24cd68dc0a120a9a60cc5dd59f76374ed2 Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Sat, 8 Feb 2025 17:34:59 -0500 Subject: [PATCH 095/110] updated conversion --- src/main/java/frc/robot/subsystems/drive/Module.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index bfb452b..7060aec 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -85,8 +85,8 @@ public void periodic() { // double adjustSpeedSetpoint = speedSetPoint * Math.cos(io.getError()) ; // add equation with onboard DrivePid - double velocityRadPerSec = speedSetPoint / DriveConstants.WHEEL_RADIUS; - io.setDesiredSpeed(velocityRadPerSec); + double velocityRPS = speedSetPoint / 2 * Math.PI * DriveConstants.WHEEL_RADIUS; + io.setDesiredSpeed(velocityRPS); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index e1529af..820e4ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -213,6 +213,7 @@ public void setTurnAngleReference(Rotation2d angle){ @Override public void setDesiredSpeed(double speed){ + final VelocityVoltage m_request = new VelocityVoltage(speed); driveTalonFX.setControl(m_request); } From 845e83f28d5973699e60322df3013dc404c7559c Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Sat, 8 Feb 2025 18:20:52 -0500 Subject: [PATCH 096/110] removed drive --- src/main/java/frc/robot/RobotContainer.java | 76 ++-- .../frc/robot/commands/DriveCommands.java | 65 --- .../robot/subsystems/arm/ArmConstants.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 420 ------------------ .../subsystems/drive/DriveConstants.java | 36 -- .../frc/robot/subsystems/drive/GyroIO.java | 20 - .../robot/subsystems/drive/GyroIOPigeon.java | 38 -- .../frc/robot/subsystems/drive/Module.java | 152 ------- .../frc/robot/subsystems/drive/ModuleIO.java | 57 --- .../robot/subsystems/drive/ModuleIOSim.java | 56 --- .../subsystems/drive/ModuleIOSparkFlex.Java | 150 ------- .../subsystems/drive/ModuleIOTalonFX.java | 221 --------- .../elevator/ElevatorConstants.java | 2 +- 13 files changed, 36 insertions(+), 1259 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/DriveCommands.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/Drive.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a22c48f..8764184 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,13 +10,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.commands.DriveCommands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.GyroIO; -import frc.robot.subsystems.drive.GyroIOPigeon; -import frc.robot.subsystems.drive.ModuleIOSim; -import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorConstants; import frc.robot.subsystems.elevator.ElevatorIOReal; @@ -34,7 +27,7 @@ public class RobotContainer { private Elevator elevator; - private Drive drive; + //private Drive drive; private Intake intake; public RobotContainer() throws IOException, ParseException { @@ -44,12 +37,12 @@ public RobotContainer() throws IOException, ParseException { intake = Intake.initialize(new IntakeIORealVortex()); Arm.initialize(new ArmIOReal()); - drive = new Drive( - new GyroIOPigeon(), - new ModuleIOTalonFX(0), - new ModuleIOTalonFX(1), - new ModuleIOTalonFX(2), - new ModuleIOTalonFX(3)); + // drive = new Drive( + // new GyroIOPigeon(), + // new ModuleIOTalonFX(0), + // new ModuleIOTalonFX(1), + // new ModuleIOTalonFX(2), + // new ModuleIOTalonFX(3)); break; case SIM: @@ -57,13 +50,13 @@ public RobotContainer() throws IOException, ParseException { intake = Intake.initialize(new IntakeIOSim()); Arm.initialize(new ArmIOSim()); - drive = new Drive( - new GyroIO() { - }, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); + // drive = new Drive( + // new GyroIO() { + // }, + // new ModuleIOSim(), + // new ModuleIOSim(), + // new ModuleIOSim(), + // new ModuleIOSim()); break; @@ -73,13 +66,13 @@ public RobotContainer() throws IOException, ParseException { intake = Intake.initialize(new IntakeIOSim()); Arm.initialize(new ArmIOSim()); - drive = new Drive( - new GyroIO() { - }, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); + // drive = new Drive( + // new GyroIO() { + // }, + // new ModuleIOSim(), + // new ModuleIOSim(), + // new ModuleIOSim(), + // new ModuleIOSim()); } @@ -88,14 +81,14 @@ public RobotContainer() throws IOException, ParseException { } private void configureBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> Constants.OIConstants.driverController.getLeftY(), - () -> Constants.OIConstants.driverController.getLeftX(), - () -> -Constants.OIConstants.driverController.getRightX())); + // drive.setDefaultCommand( + // DriveCommands.joystickDrive( + // drive, + // () -> Constants.OIConstants.driverController.getLeftY(), + // () -> Constants.OIConstants.driverController.getLeftX(), + // () -> -Constants.OIConstants.driverController.getRightX())); - Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); + //Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); // OIConstants.operatorController.leftTrigger() // .whileTrue(Commands.startEnd(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 1.5), @@ -145,14 +138,14 @@ private void configureBindings() { // ) // ); - OIConstants.driverController.povDown().onTrue( - new InstantCommand(() -> Arm.getInstance().setGoal(0))); + OIConstants.driverController.povDown().onTrue((Arm.getInstance().setGoal(0))); + OIConstants.driverController.povRight().onTrue((Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); + OIConstants.driverController.povUp().onTrue((Arm.getInstance().setGoal(ArmConstants.CORAL_L3))); - OIConstants.driverController.povRight().onTrue( - new InstantCommand(() -> Arm.getInstance().setGoal(0))); - OIConstants.driverController.povUp().onTrue( - new InstantCommand(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L3))); + // OIConstants.operatorController.povDown().onTrue((elevator.setGoal(0.05))); + // OIConstants.operatorController.povRight().onTrue((elevator.setGoal(0.4))); + // OIConstants.operatorController.povUp().onTrue((elevator.setGoal(0.69))); // OIConstants.driverController.leftTrigger().whileTrue( @@ -169,7 +162,6 @@ private void configureBindings() { // () -> Arm.getInstance().manualVoltage(0), // Arm.getInstance() // )); - OIConstants.driverController.leftTrigger().whileTrue( Commands.run( () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java deleted file mode 100644 index 8ed1fd2..0000000 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ /dev/null @@ -1,65 +0,0 @@ -package frc.robot.commands; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Constants; -import frc.robot.subsystems.drive.Drive; - -public class DriveCommands { - private DriveCommands() {} - - /** - * Field relative drive command using two joysticks (controlling linear and angular velocities). - */ - public static Command joystickDrive( - Drive drive, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { - return Commands.run( - () -> { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), Constants.OIConstants.KAxisDeadband); - Rotation2d linearDirection = - new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), Constants.OIConstants.KAxisDeadband); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); - - // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - - // Convert to field relative speeds & send command - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); - }, - drive); - } -} - diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 30d09b6..9ed460a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -13,7 +13,7 @@ public class ArmConstants { public static final double CORAL_L4 = Units.degreesToRadians(54.61); public static final double MAX_VELOCITY = 2; - public static final double MAX_ACCELERATION = 2; + public static final double MAX_ACCELERATION = 4; // Default FF public static final double DEFAULTkG = 1.02; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java deleted file mode 100644 index 745795d..0000000 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ /dev/null @@ -1,420 +0,0 @@ -package frc.robot.subsystems.drive; - -import static edu.wpi.first.units.Units.*; - -import java.io.IOException; - -import org.json.simple.parser.ParseException; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.util.PathPlannerLogging; - -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -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.units.AngleUnit; -import edu.wpi.first.units.AngularVelocityUnit; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.VoltageUnit; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.MutAngle; -import edu.wpi.first.units.measure.MutAngularVelocity; -import edu.wpi.first.units.measure.MutVoltage; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; - -public class Drive extends SubsystemBase { - private static Drive instance; - private static final SwerveDriveKinematics K_DRIVE_KINEMATICS = new SwerveDriveKinematics(getModuleTranslation2d()); - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final Module[] modules = new Module[4]; - private static SysIdRoutine sysId; - private final MutableMeasure m_appliedVoltage = new MutVoltage(0, 0, Volt); - private final MutableMeasure m_position = new MutAngle(0, 0, Radian); - private final MutableMeasure m_velocity = new MutAngularVelocity( - 0, 0, RadiansPerSecond); - private static SwerveDriveKinematics Modulekinematics = new SwerveDriveKinematics(getModuleTranslation2d()); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslation2d()); - private Rotation2d rawGyroRotation2d = new Rotation2d(); - private SwerveModulePosition[] lastModulePositions = new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - - private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation2d, - lastModulePositions, new Pose2d()); - - public static Drive getInstance() { - return instance; - } - - public static Drive initialize(GyroIO gyro, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) - throws IOException, ParseException { - if (instance == null) { - instance = new Drive(gyro, fl, fr, bl, br); - } - return instance; - } - - public Drive( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO) throws IOException, ParseException { - this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); - - RobotConfig config; - - config = RobotConfig.fromGUISettings(); - - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } - // } finally { - // config = null; // if something went horribly wrong - // } - - // Configure AutoBuilder last - AutoBuilder.configure( - this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also - // optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for - // holonomic drive trains - new PIDConstants(DriveConstants.TranslationKP, DriveConstants.TranslationKI, - DriveConstants.TranslationKD), // Translation PID constants - new PIDConstants(DriveConstants.RotationKP, DriveConstants.RotationKI, - DriveConstants.RotationKD) // Rotation PID constants - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red - // alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - } - - public void robotInit() { - - // Pathfinding.setPathfinder(new LocalADStarAK()); // AK code needs to be added - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); - sysId = new SysIdRoutine( - new SysIdRoutine.Config(), - new SysIdRoutine.Mechanism( - (voltage) -> { - for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(voltage.in(Volts)); - } - }, - log -> { - log.motor("driveSparkMax") - .voltage(m_appliedVoltage.mut_replace(inputs.driveAppliedVolts, Volts)) - .angularPosition(m_position.mut_replace(inputs.drivePositionRad, Radians)) - .angularVelocity( - m_velocity.mut_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond)); - - }, - this)); - } - - public void periodic() { - gyroIO.updateInputs(gyroInputs); - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); - - } - - if (DriverStation.isDisabled()) { - for (var module : modules) { - module.stop(); - } - } - - if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - } - - SwerveModulePosition[] modulePositions = getModulePositions(); - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - moduleDeltas[moduleIndex] = new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - } - - if (gyroInputs.connected) { - - rawGyroRotation2d = gyroInputs.yawPosition; - } else { - - Twist2d twist = K_DRIVE_KINEMATICS.toTwist2d(moduleDeltas); - rawGyroRotation2d = rawGyroRotation2d.plus(new Rotation2d(twist.dtheta)); - } - poseEstimator.update(rawGyroRotation2d, modulePositions); - } - - public void runVelocity(ChassisSpeeds speeds) { - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.MAX_LINEAR_SPEED); - - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); - } - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); - } - - public void stop() { - runVelocity(new ChassisSpeeds()); - } - - public void stopWithx() { - Rotation2d[] headings = new Rotation2d[4]; - for (int i = 0; i < 4; i++) { - headings[i] = getModuleTranslation2d()[i].getAngle(); - } - Modulekinematics.resetHeadings(); - stop(); - } - - public void zeroHeading() { - if (Constants.currentMode == frc.robot.Constants.Mode.SIM) { - poseEstimator.resetPosition( - new Rotation2d(), - new SwerveModulePosition[] { - modules[0].getPosition(), - modules[1].getPosition(), - modules[2].getPosition(), - modules[3].getPosition(), - }, - getPose()); - } - - gyroIO.reset(); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } - - @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getState(); - } - return states; - } - - private SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] states = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getPosition(); - } - return states; - } - - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, 4.8); // DriveConstants.kMaxSpeedMetersPerSecond - for (int i = 0; i < 4; i++) { - modules[i].setDesiredState(desiredStates[i]); - } - } - - @AutoLogOutput(key = "odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - public Rotation2d getRotation() { - return getPose().getRotation(); - } - - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation2d, getModulePositions(), pose); - } - - private ChassisSpeeds getRobotRelativeSpeeds() { - return K_DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates()); - - } - - public void driveRobotRelative(ChassisSpeeds speeds) { - SwerveModuleState[] moduleStates = K_DRIVE_KINEMATICS.toSwerveModuleStates(speeds); - setModuleStates(moduleStates); - } - - public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - poseEstimator.addVisionMeasurement(visionPose, timestamp); - } - - public double getMaxLinearSpeedMetersPerSec() { - return DriveConstants.MAX_LINEAR_SPEED; - } - - public double getMaxAngularSpeedRadPerSec() { - return DriveConstants.MAX_ANGULAR_SPEED; - } - - public static Translation2d[] getModuleTranslation2d() { - return new Translation2d[] { - new Translation2d(DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0), - new Translation2d(DriveConstants.TRACK_LENGTH / 2.0, -DriveConstants.TRACK_WIDTH / 2.0), - new Translation2d(-DriveConstants.TRACK_LENGTH / 2.0, DriveConstants.TRACK_WIDTH / 2.0), - new Translation2d(-DriveConstants.TRACK_LENGTH / 2.0, -DriveConstants.TRACK_WIDTH / 2.0), - }; - } - - public static void setInstance(Drive instance) { - Drive.instance = instance; - } - - public static double getMaxLinearSpeed() { - return DriveConstants.MAX_LINEAR_SPEED; - } - - public static double getTrackWidthX() { - return DriveConstants.TRACK_LENGTH; - } - - public static double getTrackWidthY() { - return DriveConstants.TRACK_WIDTH; - } - - public static double getDriveBaseRadius() { - return DriveConstants.DRIVE_BASE_RADIUS; - } - - public static double getMaxAngularSpeed() { - return DriveConstants.MAX_ANGULAR_SPEED; - } - - public static SwerveDriveKinematics getkDriveKinematics() { - return K_DRIVE_KINEMATICS; - } - - public GyroIO getGyroIO() { - return gyroIO; - } - - public GyroIOInputsAutoLogged getGyroIOInputs() { - return gyroInputs; - } - - public ModuleIOInputsAutoLogged getInputs() { - return inputs; - } - - public Module[] getModules() { - return modules; - } - - public SysIdRoutine getSysId() { - return sysId; - } - - public MutableMeasure getM_appliedVoltage() { - return m_appliedVoltage; - } - - public MutableMeasure getM_position() { - return m_position; - } - - public MutableMeasure getM_velocity() { - return m_velocity; - } - - public static SwerveDriveKinematics getModulekinematics() { - return Modulekinematics; - } - - public static void setModulekinematics(SwerveDriveKinematics modulekinematics) { - Modulekinematics = modulekinematics; - } - - public SwerveDriveKinematics getKinematics() { - return kinematics; - } - - public void setKinematics(SwerveDriveKinematics kinematics) { - this.kinematics = kinematics; - } - - public Rotation2d getRawGyroRotation2d() { - return rawGyroRotation2d; - } - - public void setRawGyroRotation2d(Rotation2d rawGyroRotation2d) { - this.rawGyroRotation2d = rawGyroRotation2d; - } - - public SwerveModulePosition[] getLastModulePositions() { - return lastModulePositions; - } - - public void setLastModulePositions(SwerveModulePosition[] lastModulePositions) { - this.lastModulePositions = lastModulePositions; - } - - public SwerveDrivePoseEstimator getPoseEstimator() { - return poseEstimator; - } - - public void setPoseEstimator(SwerveDrivePoseEstimator poseEstimator) { - this.poseEstimator = poseEstimator; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java deleted file mode 100644 index c854402..0000000 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.math.util.Units; - -public class DriveConstants { - - public static final double MAX_LINEAR_SPEED = 17.14; - public static final double TRACK_LENGTH = Units.inchesToMeters(21.25); - public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); - public static final double DRIVE_BASE_RADIUS = Math.hypot(DriveConstants.TRACK_LENGTH / 2.0, - DriveConstants.TRACK_WIDTH / 2.0); - public static final double MAX_ANGULAR_SPEED = Units.feetToMeters(DriveConstants.MAX_LINEAR_SPEED) - / DRIVE_BASE_RADIUS; - public static final double TranslationKS = 0.014; - public static final double TranslationKV = 0.134; - public static final double TranslationKP = 0.1; - public static final double TranslationKI = 0.0; - public static final double TranslationKD = 0.0; - public static final double RotationKP = 5.0; - public static final double RotationKI = 0.0; - public static final double RotationKD = 0.0; - public static final double WHEEL_RADIUS = Units.inchesToMeters(4); - public static final double TurnGearing = (468.0/35.0) * (10.0/11.0); - public static final double DriveGearing = 6.11; - public static final int turnCurrentLimit = 60; // May need to change later - public static final int driveCurrentLimit = 60; - public static final int pigeonID = 20; - public static final double turningFactor = 2 * Math.PI; - public static final InvertedValue driveMotorInverted = InvertedValue.Clockwise_Positive; - public static final NeutralModeValue driveMotorNeutralMode = NeutralModeValue.Brake; - public static final double openLoopRamp = 0.25; - public static final double closedLoopRamp = 0.0; -} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java deleted file mode 100644 index c3dd8d8..0000000 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.subsystems.drive; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.geometry.Rotation2d; - -public interface GyroIO { - @AutoLog - public static class GyroIOInputs { - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public double yawVelocityRadPerSec = 0.0; - } - - public default void reset() { - } - - public default void updateInputs(GyroIOInputs inputs) { - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java deleted file mode 100644 index 1be99c3..0000000 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.hardware.Pigeon2; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; - -public class GyroIOPigeon implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(DriveConstants.pigeonID); - private final StatusSignal yaw = pigeon.getYaw(); - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - - public GyroIOPigeon() { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(100); - yawVelocity.setUpdateFrequency(100); - pigeon.optimizeBusUtilization(); - } - - @Override - public void reset() { - pigeon.reset(); - } - - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java deleted file mode 100644 index 7060aec..0000000 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ /dev/null @@ -1,152 +0,0 @@ -package frc.robot.subsystems.drive; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.Constants; -import frc.robot.Util.LoggedTunableNumber; -import org.littletonrobotics.junction.Logger; - -public class Module { - private static final LoggedTunableNumber drivekS = - new LoggedTunableNumber("Drive/Module/DrivekS"); -private static final LoggedTunableNumber drivekV = - new LoggedTunableNumber("Drive/Module/DrivekV"); -private static final LoggedTunableNumber drivekP = - new LoggedTunableNumber("Drive/Module/DrivekP"); - private static final LoggedTunableNumber drivekI = - new LoggedTunableNumber("Drive/Module/DrivekI"); - private static final LoggedTunableNumber drivekD = - new LoggedTunableNumber("Drive/Module/DrivekD"); -private static final LoggedTunableNumber turnkP = new LoggedTunableNumber("Drive/Module/TurnkP"); -private static final LoggedTunableNumber turnkI = new LoggedTunableNumber("Drive/Module/TurnkI"); -private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); - - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; - // private final SimpleMotorFeedforward drivFeedforward; - // private final PIDController driveFeedBackPidController; - private Rotation2d angleSetpoint = null; - private Double speedSetPoint = null; - private Rotation2d turnRelativeOffset = null; - - - - public Module(ModuleIO io, int index) { - this.io = io; - this.index = index; - - switch (Constants.currentMode) { - case REAL: - drivekS.initDefault(0.014); - drivekV.initDefault(0.134); - drivekP.initDefault(0.0); - drivekI.initDefault(0); - drivekD.initDefault(0); - turnkP.initDefault(10); - turnkI.initDefault(0); - turnkD.initDefault(0); - // drivFeedforward = new SimpleMotorFeedforward(DriveConstants.TranslationKS, DriveConstants.TranslationKV); - // driveFeedBackPidController = new PIDController(DriveConstants.TranslationKP, DriveConstants.TranslationKI, DriveConstants.TranslationKD); - break; - case REPLAY: - // drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - // driveFeedBackPidController = new PIDController(0.05, 0.0, 0.0); - break; - case SIM: - // drivFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - // driveFeedBackPidController = new PIDController(0.05, 0.0, 0.0); - break; - default: - // drivFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - // driveFeedBackPidController = new PIDController(0.0, 0.0, 0.0); - break; - } - - - } - - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); - - if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { - turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); - } - - if (angleSetpoint != null) { - io.setTurnAngleReference(angleSetpoint);; - } - - if (speedSetPoint != null) { - // double adjustSpeedSetpoint = speedSetPoint * Math.cos(io.getError()) ; // add equation with onboard DrivePid - - - double velocityRPS = speedSetPoint / 2 * Math.PI * DriveConstants.WHEEL_RADIUS; - io.setDesiredSpeed(velocityRPS); - } - } - - public SwerveModuleState runSetpoint(SwerveModuleState state) { - state.optimize(getAngle()); - angleSetpoint = state.angle; - speedSetPoint = state.speedMetersPerSecond; - return state; - } - - public void runCharacterization(double volts) { - - angleSetpoint = new Rotation2d(); - - io.setDriveVoltage(volts); - speedSetPoint = null; - } - - public void stop() { - io.setTurnVoltage(0.0); - io.setDriveVoltage(0.0); - - angleSetpoint = null; - speedSetPoint = null; - } - - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); - } - - public Rotation2d getAngle() { - if (turnRelativeOffset == null) { - return new Rotation2d(); - } else { - return inputs.turnAbsolutePosition; - } - } - - public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS; - } - - public double getPositionMeters() { - return inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS; - } - - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), getAngle()); - } - - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); - } - - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } - - public void setDesiredState(SwerveModuleState desiredState) { - this.io.setDesiredState(desiredState); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java deleted file mode 100644 index 7328731..0000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ /dev/null @@ -1,57 +0,0 @@ -package frc.robot.subsystems.drive; - -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; - -public interface ModuleIO { - - @AutoLog - public static class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnAppliedVolts = 0.0; - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedRadPerSec = 0.0; - public double[] turnCurrentAmps = new double[] {}; - } - - public default void updateInputs(ModuleIOInputs inputs) { - } - - public default void setDriveVoltage(double volts) { - } - - public default void runMovementPID(double desiredStateRotPerSecond, double rotationDesiredPosition) { - } - - public default void setDesiredState(SwerveModuleState state) { - } - - public default void setTurnVoltage(double volts) { - } - - public default void setTurnAngleReference(Rotation2d angle){ - - } - - public default void setDriveBrakeMode(boolean enable) { - } - - public default void setTurnBrakeMode(boolean enable) { - } - - public default void getError(double error){ - - } - - public default void setDesiredSpeed(double speed){ - - } - -} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java deleted file mode 100644 index 351682f..0000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ /dev/null @@ -1,56 +0,0 @@ -package frc.robot.subsystems.drive; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; - -public class ModuleIOSim implements ModuleIO { - private static final DCMotor driveMotorModel = DCMotor.getKrakenX60Foc(1); - private static final DCMotor turnMotorModle = DCMotor.getNeoVortex(1); - private static final double LOOP_PERIOD_SECS = 0.02; - // change gearbox argument - private DCMotorSim driveSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(driveMotorModel, 0.025, DriveConstants.DriveGearing), - driveMotorModel); // needs to change - private DCMotorSim turnSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(turnMotorModle, 0.004, DriveConstants.TurnGearing), - turnMotorModle); // needs to change - - private boolean driveClosedLoop = false; - private boolean turnClosedLoop = false; - private PIDController driveController = new PIDController(0, 0, 0); - private PIDController turnController = new PIDController(0, 0, 0); - private double driveFFVolts = 0; - private double driveAppliedVolts = 0.0; - private double turnAppliedVolts = 0.0; - - @Override - public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(LOOP_PERIOD_SECS); - turnSim.update(LOOP_PERIOD_SECS); - - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] { Math.abs(driveSim.getCurrentDrawAmps()) }; - // inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPositon); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPosition()); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] { Math.abs(turnSim.getCurrentDrawAmps()) }; - } - - @Override - public void setDriveVoltage(double volts) { - driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - driveSim.setInputVoltage(driveAppliedVolts); - } - - @Override - public void setTurnVoltage(double volts) { - turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - turnSim.setInputVoltage(turnAppliedVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java deleted file mode 100644 index f002987..0000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.Java +++ /dev/null @@ -1,150 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.Drive.Drive; - - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; - -/** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO - * or NEO 550), and analog absolute encoder connected to the RIO - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ -public class ModuleIOSparkMax implements ModuleIO { - - ; - private final CANSparkFlex turnSparkMax; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AbsoluteEncoder turnAbsoluteEncoder; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSparkMax(int index) { - switch (index) { - case 0: - driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); - turnSparkMax = new CANSparkFlex(2, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveSparkMax = new CANSparkMax(10, MotorType.kBrushless); - turnSparkMax = new CANSparkFlex(9, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveSparkMax = new CANSparkMax(4, MotorType.kBrushless); - turnSparkMax = new CANSparkFlex(6, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); - turnSparkMax = new CANSparkFlex(8, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - - turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians - turnAbsoluteEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); // Radians per second - - driveSparkMax.setCANTimeout(250); - turnSparkMax.setCANTimeout(250); - - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); - - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DriveConstants.DriveGearing; - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DriveConstants.DriveGearing; - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; - - inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition()); - inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / DriveConstants.TurnGearing); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / DriveConstants.TurnGearing; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - } - - @Override - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java deleted file mode 100644 index 820e4ef..0000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ /dev/null @@ -1,221 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.ExternalEncoderConfig; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; - -/** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn - * motor controller (NEO - * or NEO 550), and analog absolute encoder connected to the RIO - * - *

- * NOTE: This implementation should be used as a starting point and adapted to - * different hardware - * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") - * - *

- * To calibrate the absolute encoder offsets, point the modules straight (such - * that forward - * motion on the drive motor will propel the robot forward) and copy the - * reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ -public class ModuleIOTalonFX implements ModuleIO { - // final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0); - - public final TalonFX driveTalonFX; - public final SparkFlex turnSparkFlex; - private final RelativeEncoder turnRelativeEncoder; - private final AbsoluteEncoder turnAbsoluteEncoder; - - - public static final SparkFlexConfig turningConfig = new SparkFlexConfig(); - public static final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); - - - public ModuleIOTalonFX(int index) { - - switch (index) { - case 0: - // Front left - driveTalonFX = new TalonFX(1); - turnSparkFlex = new SparkFlex(2, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); - break; - case 1: - // Front right - driveTalonFX = new TalonFX(3); - turnSparkFlex = new SparkFlex(4, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); - break; - case 2: - // Back left - driveTalonFX = new TalonFX(8); - turnSparkFlex = new SparkFlex(7, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); - break; - case 3: - // Back right - driveTalonFX = new TalonFX(6); - turnSparkFlex = new SparkFlex(5, MotorType.kBrushless); - turnAbsoluteEncoder = turnSparkFlex.getAbsoluteEncoder(); - break; - default: - throw new RuntimeException("Invalid module index"); - - } - - // turning SparkFlex Configuration - turningConfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(DriveConstants.turnCurrentLimit); - turningConfig.absoluteEncoder - .inverted(true) - .positionConversionFactor(DriveConstants.turningFactor) - .velocityConversionFactor(DriveConstants.turningFactor/60); - turningConfig.closedLoop - .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) - .pid(DriveConstants.RotationKP, DriveConstants.RotationKI, DriveConstants.RotationKD) - .outputRange(-1, 1) - .positionWrappingEnabled(true) - .positionWrappingInputRange(0, DriveConstants.turningFactor); - turnSparkFlex.setCANTimeout(250); - turnRelativeEncoder = turnSparkFlex.getEncoder(); // only getting one? - final ExternalEncoderConfig encoderconfig = new ExternalEncoderConfig(); - encoderconfig.measurementPeriod(10); - turnRelativeEncoder.setPosition(0.0); - encoderconfig.measurementPeriod(10); - encoderconfig.averageDepth(2); - - - // Drive TalonFX Configuration - driveConfig.MotorOutput.Inverted = DriveConstants.driveMotorInverted; - driveConfig.MotorOutput.NeutralMode = DriveConstants.driveMotorNeutralMode; - driveConfig.Feedback.SensorToMechanismRatio = DriveConstants.DriveGearing; - - // May need to add Thresholds - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - driveConfig.CurrentLimits.StatorCurrentLimit = DriveConstants.driveCurrentLimit; - // driveConfig.Slot0 = new Slot0Configs().withKP(0).withKI(0).withKD(0) - driveConfig.Slot0.kP = DriveConstants.TranslationKP; - driveConfig.Slot0.kI = DriveConstants.TranslationKI; - driveConfig.Slot0.kD = DriveConstants.TranslationKD; - driveConfig.Slot0.kS = DriveConstants.TranslationKS; - driveConfig.Slot0.kV = DriveConstants.TranslationKV; - - //May need to change ramp and openloop - driveConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = DriveConstants.closedLoopRamp; - driveConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = DriveConstants.closedLoopRamp; - driveConfig.Audio.BeepOnBoot = true; - driveTalonFX.setPosition(0); - driveTalonFX.optimizeBusUtilization(); - driveTalonFX.clearStickyFaults(); - - - - - - StatusCode status = StatusCode.StatusCodeNotInitialized; - // REVLibError status2 = REVLibError.kError; // not work maybe - for (int i = 0; i < 5; i++) { - status = driveTalonFX.getConfigurator().apply(driveConfig); - turnSparkFlex.configure(turningConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - if (status.isOK()) - break; - } - - if (!status.isOK()) { - System.out.println( - "Talon ID " - + driveTalonFX.getDeviceID() - + " failed config with error " - + status.toString()); - } - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = Units.rotationsToRadians(driveTalonFX.getPosition().getValueAsDouble()) - / DriveConstants.DriveGearing; - inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble()) - / DriveConstants.DriveGearing; - inputs.driveAppliedVolts = driveTalonFX.getMotorVoltage().getValueAsDouble(); - inputs.driveCurrentAmps = new double[] { driveTalonFX.getSupplyCurrent().getValueAsDouble() }; - - inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsoluteEncoder.getPosition()); - inputs.turnPosition = Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / DriveConstants.TurnGearing); - inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / DriveConstants.TurnGearing; - inputs.turnAppliedVolts = turnSparkFlex.getAppliedOutput() * turnSparkFlex.getBusVoltage(); - inputs.turnCurrentAmps = new double[] { turnSparkFlex.getOutputCurrent() }; - } - - - @Override - public void setDriveVoltage(double volts) { - driveTalonFX.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkFlex.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - if (driveConfig.MotorOutput.NeutralMode == NeutralModeValue.Brake){ - enable = true; - } else { - enable = false; - } - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turningConfig.idleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnAngleReference(Rotation2d angle){ - turnSparkFlex.getClosedLoopController().setReference(angle.getRadians(), ControlType.kPosition); - } - - @Override - public void setDesiredSpeed(double speed){ - - final VelocityVoltage m_request = new VelocityVoltage(speed); - driveTalonFX.setControl(m_request); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 904ee19..1b7ce81 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -20,7 +20,7 @@ public final class ElevatorConstants{ public static final double MAX_ACCELERATION = 1; public static final double MAX_VELOCITY = 1; - public static final double conversionFactor = Units.inchesToMeters(1/9 * Math.PI * 1.751); // gearing * pi * sprocket diameter + public static final double conversionFactor = Units.inchesToMeters(1.0/9.0 * Math.PI * 1.751); // gearing * pi * sprocket diameter public static final int elevatorCurrentLimits = 40; //might need to adjust later From 81b87e17719870b8dda60276eb60168f14939947 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Sat, 8 Feb 2025 20:52:59 -0500 Subject: [PATCH 097/110] Testing changes 2/8/2025 --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++---- .../frc/robot/subsystems/CANCoderTest.java | 15 +++++++++ .../java/frc/robot/subsystems/arm/Arm.java | 1 + .../java/frc/robot/subsystems/arm/ArmIO.java | 2 ++ .../frc/robot/subsystems/arm/ArmIOReal.java | 32 ++++++++++++++----- 5 files changed, 52 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/CANCoderTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8764184..cef5f41 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIORealVortex; import frc.robot.subsystems.intake.IntakeIOSim; +import frc.robot.subsystems.CANCoderTest; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmConstants; import frc.robot.subsystems.arm.ArmIOSim; @@ -36,6 +37,7 @@ public RobotContainer() throws IOException, ParseException { elevator = Elevator.initialize(new ElevatorIOReal()); intake = Intake.initialize(new IntakeIORealVortex()); Arm.initialize(new ArmIOReal()); + CANCoderTest canCoderTest = new CANCoderTest(); // drive = new Drive( // new GyroIOPigeon(), @@ -162,25 +164,27 @@ private void configureBindings() { // () -> Arm.getInstance().manualVoltage(0), // Arm.getInstance() // )); - OIConstants.driverController.leftTrigger().whileTrue( + OIConstants.driverController.y().whileTrue( Commands.run( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), + () -> Arm.getInstance().manualVoltage( 6), Arm.getInstance() ) ); - OIConstants.driverController.rightTrigger().whileTrue( + // up + OIConstants.driverController.a().whileTrue( Commands.run( - () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * -6 + ArmConstants.DEFAULTkG), + () -> Arm.getInstance().manualVoltage(-6), Arm.getInstance() ) ); - OIConstants.driverController.leftBumper().onTrue(Arm.getInstance().setGoal(ArmConstants.CORAL_L1)); + OIConstants.driverController.leftBumper().onTrue(Arm.getInstance().setGoal(0)); OIConstants.driverController.rightBumper().onTrue(Arm.getInstance().setGoal(ArmConstants.CORAL_L2)); } public Command getAutonomousCommand() { - return Commands.runOnce(() -> elevator.setVoltage(ElevatorConstants.kg)); + return Commands.sequence( + ); } } diff --git a/src/main/java/frc/robot/subsystems/CANCoderTest.java b/src/main/java/frc/robot/subsystems/CANCoderTest.java new file mode 100644 index 0000000..637e484 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CANCoderTest.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.CANcoder; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class CANCoderTest extends SubsystemBase { + private CANcoder encoder = new CANcoder(19); + @Override + public void periodic() { + Logger.recordOutput("CANCoder Angle", encoder.getAbsolutePosition().getValueAsDouble()); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 2e7772b..f9c5716 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -51,6 +51,7 @@ public void resetAbsoluteEncoder() { } public void manualVoltage(double voltage) { + Logger.recordOutput("isHolding", false); setOpenLoop(true); io.setVoltage(voltage); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 27ef5f4..1ca28ef 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -11,6 +11,8 @@ public static class ArmIOInputs{ public double goalAngle = 0.0; public boolean openLoop = true; public double setpointVelocity = 0.0; + public double appliedOutput = 0.0; + public double busVoltage = 0.0; } public default void updateInputs(ArmIOInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 6f17569..58cf60b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.arm; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkClosedLoopController; @@ -35,16 +37,20 @@ public ArmIOReal(){ config.absoluteEncoder.velocityConversionFactor(Math.PI / 60); config.absoluteEncoder.inverted(false); - config.softLimit.forwardSoftLimitEnabled(true); - config.softLimit.reverseSoftLimitEnabled(false); - config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); - config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); + config.openLoopRampRate(1); + + // config.softLimit.forwardSoftLimitEnabled(true); + // config.softLimit.reverseSoftLimitEnabled(false); + // config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); + // config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); config.closedLoop.p(0.0); config.closedLoop.i(0.0); config.closedLoop.d(0.0); + config.inverted(true); + armMotor.clearFaults(); armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -63,8 +69,11 @@ public void setFFValues(double kS, double kG, double kV, double kA) { @Override public void hold() { - double ffvolts = ffmodel.calculate(armEncoder.getPosition(), 0); - onboardController.setReference(ffvolts, ControlType.kVoltage); + double ffvolts = ffmodel.calculate(getOffsetAngle(), 0); + Logger.recordOutput("arm ffvolts", ffvolts); + Logger.recordOutput("isHolding", true); + Logger.recordOutput("arm angle", getOffsetAngle()); + setVoltage(ffvolts); } @Override @@ -78,19 +87,26 @@ public void setVoltage(double voltage) { // armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); } + public double getOffsetAngle() { + return armEncoder.getPosition() - ArmConstants.ARM_OFFSET; + } + @Override public void updateInputs(ArmIOInputs inputs) { if(!inputs.openLoop) { // setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity)); - double ffvolts = ffmodel.calculate(armEncoder.getPosition(), setpoint.velocity); + double ffvolts = ffmodel.calculate(getOffsetAngle(), setpoint.velocity); onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); setpoint = profile.calculate(0.02, setpoint, goal); } - inputs.angularPosition = armEncoder.getPosition(); + inputs.angularPosition = getOffsetAngle(); inputs.angularVelocity = armEncoder.getVelocity(); inputs.current = armMotor.getOutputCurrent(); inputs.voltage = armMotor.getAppliedOutput() * armMotor.getBusVoltage(); inputs.setpointVelocity = setpoint.velocity; + inputs.goalAngle = goal.position; + inputs.busVoltage = armMotor.getBusVoltage(); + inputs.appliedOutput = armMotor.getAppliedOutput(); } } From 6d9b1fb6ad9c729ef42b102a002b34c31160143d Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sun, 9 Feb 2025 13:30:00 -0500 Subject: [PATCH 098/110] new arm goal command and sim updates --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 1 - .../java/frc/robot/subsystems/arm/Arm.java | 28 ++++++++----------- .../robot/subsystems/arm/ArmConstants.java | 10 +++++-- .../java/frc/robot/subsystems/arm/ArmIO.java | 5 +++- .../frc/robot/subsystems/arm/ArmIOReal.java | 18 +++++------- .../frc/robot/subsystems/arm/ArmIOSim.java | 17 +++++------ 7 files changed, 40 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 079fb1b..f5dbaed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,5 +26,6 @@ public static enum Gamepiece { ALGAE, CORAL, NONE, + SIM, } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cef5f41..5f9a4b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,6 @@ public RobotContainer() throws IOException, ParseException { } - Arm.getInstance().setFFMode(Gamepiece.CORAL); configureBindings(); } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index f9c5716..b07c4d8 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -9,7 +9,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Constants.Gamepiece; +import frc.robot.Constants.Mode; public class Arm extends SubsystemBase { private ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); @@ -19,6 +21,7 @@ public class Arm extends SubsystemBase { private Arm(ArmIO io) { this.io = io; setDefaultCommand(Commands.run(() -> io.hold(), this)); + setFFMode(Constants.currentMode == Mode.SIM ? Gamepiece.SIM: Gamepiece.NONE); } public double getAngle() { @@ -34,16 +37,9 @@ public static Arm getInstance() { } public Command setGoal(double angle) { - return Commands.startEnd( - () -> { - setOpenLoop(false); - io.setGoal(angle); - }, - () -> { - setOpenLoop(true); - }, - this - ).until(() -> MathUtil.isNear(angle, inputs.angularPosition, 0.008)); + return Commands.runOnce(() -> io.setGoal(angle), this) + .andThen(Commands.run(() -> io.updateMotionProfile(), this)) + .until(() -> MathUtil.isNear(angle, inputs.angularPosition, 0.008)); } public void resetAbsoluteEncoder() { @@ -51,15 +47,9 @@ public void resetAbsoluteEncoder() { } public void manualVoltage(double voltage) { - Logger.recordOutput("isHolding", false); - setOpenLoop(true); io.setVoltage(voltage); } - public void setOpenLoop(boolean openLoop) { - inputs.openLoop = openLoop; - } - public Pose3d getArmPose(){ return new Pose3d( new Translation3d(10 + -(0.45) * Math.cos(inputs.angularPosition), 1, 1 + (0.45) * Math.sin(inputs.angularPosition)), @@ -68,12 +58,16 @@ public Pose3d getArmPose(){ } public void setFFMode(Gamepiece gamepieceType) { + Logger.recordOutput(getName() + "/FFMode", gamepieceType); switch (gamepieceType) { case CORAL: io.setFFValues(ArmConstants.CORALkS, ArmConstants.CORALkG, ArmConstants.CORALkV, ArmConstants.CORALkA); break; case ALGAE: - io.setFFValues(ArmConstants.ALGAEkS, ArmConstants.ALGAEkG, ArmConstants.CORALkV, ArmConstants.CORALkA); + io.setFFValues(ArmConstants.ALGAEkS, ArmConstants.ALGAEkG, ArmConstants.ALGAEkV, ArmConstants.ALGAEkA); + break; + case SIM: + io.setFFValues(ArmConstants.SIMkS, ArmConstants.SIMkG, ArmConstants.SIMkV, ArmConstants.SIMkA); break; case NONE: io.setFFValues(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 9ed460a..9ffb424 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -15,6 +15,12 @@ public class ArmConstants { public static final double MAX_VELOCITY = 2; public static final double MAX_ACCELERATION = 4; + // Simulation FF + public static final double SIMkG = 1.3; + public static final double SIMkV = 1.00; + public static final double SIMkA = 0.05; + public static final double SIMkS = 0.001; + // Default FF public static final double DEFAULTkG = 1.02; public static final double DEFAULTkV = 1.01; @@ -34,12 +40,12 @@ public class ArmConstants { public static final double CORALkS = 0.1; public static class Sim { - public static double GEARING = 40; + public static double GEARING = 60; public static double MOI = 2.09670337984; public static double LENGTH = 0.6604; public static double MIN_ANGLE = Units.degreesToRadians(-95); public static double MAX_ANGLE = Units.degreesToRadians(55); public static boolean GRAVITY = true; - public static double INIT_ANGLE = -Math.PI / 6; + public static double INIT_ANGLE = -Math.PI / 2; } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 1ca28ef..ed20e93 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -9,7 +9,6 @@ public static class ArmIOInputs{ public double angularVelocity = 0.0; public double current = 0.0; public double goalAngle = 0.0; - public boolean openLoop = true; public double setpointVelocity = 0.0; public double appliedOutput = 0.0; public double busVoltage = 0.0; @@ -24,6 +23,10 @@ public default void resetAbsoluteEncoder() { public default void setVoltage(double voltage) { + } + + public default void updateMotionProfile(){ + } public default void setGoal(double angle) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 58cf60b..dad48eb 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -61,6 +61,13 @@ public ArmIOReal(){ public void setGoal(double angle) { goal = new TrapezoidProfile.State(angle, 0); } + + @Override + public void updateMotionProfile() { + double ffvolts = ffmodel.calculate(getOffsetAngle(), setpoint.velocity); + onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); + setpoint = profile.calculate(0.02, setpoint, goal); + } @Override public void setFFValues(double kS, double kG, double kV, double kA) { @@ -70,9 +77,6 @@ public void setFFValues(double kS, double kG, double kV, double kA) { @Override public void hold() { double ffvolts = ffmodel.calculate(getOffsetAngle(), 0); - Logger.recordOutput("arm ffvolts", ffvolts); - Logger.recordOutput("isHolding", true); - Logger.recordOutput("arm angle", getOffsetAngle()); setVoltage(ffvolts); } @@ -84,7 +88,6 @@ public void resetAbsoluteEncoder() { @Override public void setVoltage(double voltage) { onboardController.setReference(voltage, ControlType.kVoltage); - // armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12)); } public double getOffsetAngle() { @@ -93,13 +96,6 @@ public double getOffsetAngle() { @Override public void updateInputs(ArmIOInputs inputs) { - if(!inputs.openLoop) { - // setVoltage(ffmodel.calculate(setpoint.position, setpoint.velocity)); - double ffvolts = ffmodel.calculate(getOffsetAngle(), setpoint.velocity); - onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); - setpoint = profile.calculate(0.02, setpoint, goal); - } - inputs.angularPosition = getOffsetAngle(); inputs.angularVelocity = armEncoder.getVelocity(); inputs.current = armMotor.getOutputCurrent(); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 55db7e3..1f6d6d5 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -18,12 +18,12 @@ public class ArmIOSim implements ArmIO { ArmConstants.Sim.GRAVITY, ArmConstants.Sim.INIT_ANGLE ); - private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); + private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.SIMkS, ArmConstants.SIMkG, ArmConstants.SIMkV, ArmConstants.SIMkA); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.5, 1); - private final PIDController controller = new PIDController(0.02, 0,0.00); + private final PIDController controller = new PIDController(2, 0,0.00); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0); - private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0); private double voltage = 0; @Override @@ -48,12 +48,13 @@ public void setFFValues(double kS, double kG, double kV, double kA) { } @Override - public void updateInputs(ArmIOInputs inputs) { - if(!inputs.openLoop) { - setpoint = profile.calculate(0.02, setpoint, goal); - setVoltage(ffmodel.calculate(inputs.angularPosition, setpoint.velocity)); - } + public void updateMotionProfile() { + setpoint = profile.calculate(0.02, setpoint, goal); + setVoltage(ffmodel.calculate(armSim.getAngleRads(), setpoint.velocity) + controller.calculate(armSim.getAngleRads(), setpoint.position)); + } + @Override + public void updateInputs(ArmIOInputs inputs) { inputs.angularPosition = armSim.getAngleRads(); inputs.angularVelocity = armSim.getVelocityRadPerSec(); inputs.current = armSim.getCurrentDrawAmps(); From 4a42cd1bbc9d0575e43eac138570c33af56527f9 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sun, 9 Feb 2025 14:59:36 -0500 Subject: [PATCH 099/110] visualizer and arm/elevator sequence --- simgui-ds.json | 3 ++ src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 15 ++++++++ src/main/java/frc/robot/RobotVisualizer.java | 35 +++++++++++++++++++ .../frc/robot/subsystems/arm/ArmIOSim.java | 13 +++++-- .../robot/subsystems/elevator/Elevator.java | 2 +- .../elevator/ElevatorConstants.java | 2 +- .../robot/subsystems/elevator/ElevatorIO.java | 2 ++ .../subsystems/elevator/ElevatorIOSim.java | 15 +++++--- 9 files changed, 78 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/RobotVisualizer.java diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..b16ea5c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,6 +90,9 @@ } ], "robotJoysticks": [ + { + "useGamepad": true + }, { "guid": "78696e70757401000000000000000000", "useGamepad": true diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f5dbaed..42645fe 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,7 +9,7 @@ public static final class IntakeSetpoints{ public static final double slow = -0.3; public static final double reverse = 0.7; } - public static Mode currentMode = Mode.REAL; + public static Mode currentMode = Mode.SIM; public static enum Mode { SIM, REAL, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5f9a4b8..44946c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,6 +78,7 @@ public RobotContainer() throws IOException, ParseException { } + RobotVisualizer robotVisualizer = new RobotVisualizer(); configureBindings(); } @@ -115,6 +116,20 @@ private void configureBindings() { ) ); + Constants.OIConstants.operatorController.povRight().onTrue( + Commands.parallel( + Arm.getInstance().setGoal(ArmConstants.CORAL_L4), + Elevator.getInstance().setGoal(ElevatorConstants.maxDistance) + ) + ); + + Constants.OIConstants.operatorController.povLeft().onTrue( + Commands.parallel( + Arm.getInstance().setGoal(ArmConstants.Sim.INIT_ANGLE), + Elevator.getInstance().setGoal(ElevatorConstants.minDistance) + ) + ); + Constants.OIConstants.operatorController.povUp() .whileTrue( diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java new file mode 100644 index 0000000..0a48878 --- /dev/null +++ b/src/main/java/frc/robot/RobotVisualizer.java @@ -0,0 +1,35 @@ +package frc.robot; + +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.ArmConstants; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorConstants; + +public class RobotVisualizer extends SubsystemBase { + private LoggedMechanism2d robotVisualizer = new LoggedMechanism2d(2, 2.5); + private LoggedMechanismRoot2d robotRoot = robotVisualizer.getRoot("Robot Visualizer", 1, 0.1); + + private LoggedMechanismLigament2d elevatorstage1Ligament = robotRoot.append(new LoggedMechanismLigament2d("Elevator Stage 1 Ligament", ElevatorConstants.maxDistance/.8, 90, 10, new Color8Bit(Color.kRed))); + private LoggedMechanismLigament2d elevatorstage2Ligament = robotRoot.append(new LoggedMechanismLigament2d("Elevator Stage 2 Ligament", ElevatorConstants.maxDistance, 90, 10, new Color8Bit(Color.kBlue))); + private LoggedMechanismLigament2d armLigament = elevatorstage2Ligament.append(new LoggedMechanismLigament2d("Arm Ligament", ArmConstants.Sim.LENGTH, -90, 10, new Color8Bit(Color.kGreen))); + + public RobotVisualizer() { + + } + + @Override + public void periodic() { + armLigament.setAngle(Units.radiansToDegrees(Arm.getInstance().getAngle()) - 90); + elevatorstage2Ligament.setLength(ElevatorConstants.maxDistance/.8 + Elevator.getInstance().getPosition()); + Logger.recordOutput("RobotVisualizer", robotVisualizer); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 1f6d6d5..4d53c7f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -19,8 +19,8 @@ public class ArmIOSim implements ArmIO { ArmConstants.Sim.INIT_ANGLE ); private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.SIMkS, ArmConstants.SIMkG, ArmConstants.SIMkV, ArmConstants.SIMkA); - private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.5, 1); - private final PIDController controller = new PIDController(2, 0,0.00); + private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(3, 10); + private final PIDController controller = new PIDController(1, 0, .5); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0); @@ -42,6 +42,13 @@ public void setGoal(double angle) { goal = new TrapezoidProfile.State(angle, 0); } + @Override + public void hold() { + double ffvolts = ffmodel.calculate(armSim.getAngleRads(), 0); + double pidvolts = controller.calculate(armSim.getAngleRads(), goal.position); + setVoltage(ffvolts + pidvolts); + } + @Override public void setFFValues(double kS, double kG, double kV, double kA) { ffmodel = new ArmFeedforward(kS, kG, kV, kA); @@ -50,7 +57,7 @@ public void setFFValues(double kS, double kG, double kV, double kA) { @Override public void updateMotionProfile() { setpoint = profile.calculate(0.02, setpoint, goal); - setVoltage(ffmodel.calculate(armSim.getAngleRads(), setpoint.velocity) + controller.calculate(armSim.getAngleRads(), setpoint.position)); + setVoltage(ffmodel.calculate(armSim.getAngleRads(), setpoint.velocity)); } @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index db6e90e..73a8ec2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -29,7 +29,7 @@ private Elevator(ElevatorIO elevatorIO){ io = elevatorIO; io.updateInputs(inputs); - setDefaultCommand(Commands.run(() -> setVoltage(-ElevatorConstants.kg), this)); + setDefaultCommand(Commands.run(() -> io.hold(), this)); } public Command setGoal(double goal){ diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 1b7ce81..f2f1ef1 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -14,7 +14,7 @@ public final class ElevatorConstants{ public static final double kP = 0; //figure this out later public static final int rightDeviceID = 12; public static final int leftDeviceID = 13; - public static final double ks = 0; + public static final double ks = 0.01; public static final double kg = 0.38; public static final double kv = 6.84; //IS THIS RIGHT?!?!? public static final double MAX_ACCELERATION = 1; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 6608e9d..aad8084 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -27,6 +27,8 @@ public default void updateInputs(ElevatorIOInputs inputs) { } + public default void hold() {} + public default void setGoal(double goal) { } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index f4835bb..80abe8e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.elevator; +import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -7,20 +8,18 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class ElevatorIOSim implements ElevatorIO { - private ElevatorSim elevatorSim = new ElevatorSim(ElevatorConstants.kv, 1, DCMotor.getNeoVortex(2), 0, 0.7112, true, 0); + private ElevatorSim elevatorSim = new ElevatorSim(ElevatorConstants.kv, ElevatorConstants.ks, DCMotor.getNeoVortex(2), 0, 0.7112, true, 0); private double elevatorAppliedVolts = 0.0; -private final SimpleMotorFeedforward ffmodel = new SimpleMotorFeedforward(ElevatorConstants.ks, ElevatorConstants.kv); +private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg - .3, ElevatorConstants.kv); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 1); private final PIDController controller = new PIDController(1, 0, 0); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); private TrapezoidProfile.State goal = new TrapezoidProfile.State(); - - @Override public void updateInputs(ElevatorIOInputs inputs) { @@ -32,7 +31,7 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.elevatorLeftPositionMeters = elevatorSim.getPositionMeters(); inputs.elevatorLeftVelocityMetersPerSecond = elevatorSim.getVelocityMetersPerSecond(); inputs.elevatorLeftCurrent = elevatorSim.getCurrentDrawAmps(); - inputs.elevatorRightAppliedVolts = elevatorAppliedVolts; + inputs.elevatorLeftAppliedVolts = elevatorAppliedVolts; elevatorSim.update(0.02); } @@ -42,6 +41,12 @@ public void moveElevator(double speed) { elevatorSim.setInputVoltage(elevatorAppliedVolts); } +@Override +public void hold() { + double ffvolts = ffmodel.calculate(0); + setVoltage(ffvolts); +} + @Override public void setVoltage(double volts){ elevatorAppliedVolts = volts; From f70e5b88b7bdbfad02791eb991c19f61bfe61d2c Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sun, 9 Feb 2025 15:13:12 -0500 Subject: [PATCH 100/110] Log arm relative encoder values --- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- src/main/java/frc/robot/subsystems/arm/ArmIO.java | 4 +++- src/main/java/frc/robot/subsystems/arm/ArmIOReal.java | 8 +++++++- src/main/java/frc/robot/subsystems/arm/ArmIOSim.java | 2 +- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index b07c4d8..30a23bc 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -43,7 +43,7 @@ public Command setGoal(double angle) { } public void resetAbsoluteEncoder() { - io.resetAbsoluteEncoder(); + io.resetEncoder(); } public void manualVoltage(double voltage) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index ed20e93..1fca7f3 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -12,13 +12,15 @@ public static class ArmIOInputs{ public double setpointVelocity = 0.0; public double appliedOutput = 0.0; public double busVoltage = 0.0; + public double relativeEncoderPosition = 0.0; + public double relativeEncoderVelocity = 0.0; } public default void updateInputs(ArmIOInputs inputs) { } - public default void resetAbsoluteEncoder() { + public default void resetEncoder() { } public default void setVoltage(double voltage) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index dad48eb..9cfa75c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -37,6 +37,9 @@ public ArmIOReal(){ config.absoluteEncoder.velocityConversionFactor(Math.PI / 60); config.absoluteEncoder.inverted(false); + config.encoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); + config.encoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); + config.openLoopRampRate(1); // config.softLimit.forwardSoftLimitEnabled(true); @@ -55,6 +58,7 @@ public ArmIOReal(){ armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); armEncoder = armMotor.getAbsoluteEncoder(); + armMotor.getEncoder().setPosition(getOffsetAngle()); } @Override @@ -81,7 +85,7 @@ public void hold() { } @Override - public void resetAbsoluteEncoder() { + public void resetEncoder() { armMotor.getEncoder().setPosition(0); } @@ -104,5 +108,7 @@ public void updateInputs(ArmIOInputs inputs) { inputs.goalAngle = goal.position; inputs.busVoltage = armMotor.getBusVoltage(); inputs.appliedOutput = armMotor.getAppliedOutput(); + inputs.relativeEncoderPosition = armMotor.getEncoder().getPosition(); + inputs.relativeEncoderVelocity = armMotor.getEncoder().getVelocity(); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 4d53c7f..deed76b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -33,7 +33,7 @@ public void setVoltage(double voltage) { } @Override - public void resetAbsoluteEncoder() { + public void resetEncoder() { armSim.setState(0, armSim.getVelocityRadPerSec()); } From 61479945c266bec3b52604a12797c844ab9241e5 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sun, 9 Feb 2025 15:19:56 -0500 Subject: [PATCH 101/110] New elevator goal command --- .../robot/subsystems/elevator/Elevator.java | 19 +++---------------- .../robot/subsystems/elevator/ElevatorIO.java | 4 ++-- .../subsystems/elevator/ElevatorIOReal.java | 13 +++++++------ .../subsystems/elevator/ElevatorIOSim.java | 11 ++++++----- 4 files changed, 18 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 73a8ec2..e2aaf4e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -33,24 +33,12 @@ private Elevator(ElevatorIO elevatorIO){ } public Command setGoal(double goal){ - return Commands.startEnd( - () -> { - setOpenLoop(false); - io.setGoal(goal); - }, - () -> { - setOpenLoop(true); - }, - this - ).until(() -> MathUtil.isNear(goal, inputs.elevatorLeftPositionMeters, 0.008)); - } - - public void setOpenLoop(boolean openLoop) { - inputs.openLoop = openLoop; + return Commands.runOnce(() -> io.setGoal(goal), this) + .andThen(Commands.run(() -> io.updateMotionProfile(), this)) + .until(() -> MathUtil.isNear(goal, inputs.elevatorLeftPositionMeters, 0.008)); } public void moveElevator(double speed) { - setOpenLoop(true); io.moveElevator(speed); } @@ -69,7 +57,6 @@ public void reset() { } public void setVoltage(double volts) { - setOpenLoop(true); io.setVoltage(volts); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index aad8084..a55a048 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -17,8 +17,6 @@ public static class ElevatorIOInputs { public double goalHeight = 0.0; public double setpointVelocity = 0.0; - - public boolean openLoop = true; } @@ -36,6 +34,8 @@ public default void setGoal(double goal) { public default void moveElevator(double speed) { } + + public default void updateMotionProfile() {} public default void setVoltage(double volts) {} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 019a6d9..5b26053 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -79,13 +79,14 @@ public void setGoal(double height) { } @Override - public void updateInputs(ElevatorIOInputs inputs) { - if(!inputs.openLoop){ - double ffvolts = ffmodel.calculate(setpoint.velocity); - controller.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); - setpoint = profile.calculate(0.02, setpoint, goal); - } + public void updateMotionProfile() { + double ffvolts = ffmodel.calculate(setpoint.velocity); + controller.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); + setpoint = profile.calculate(0.02, setpoint, goal); + } + @Override + public void updateInputs(ElevatorIOInputs inputs) { inputs.elevatorRightCurrent = rightElevatorMotor.getOutputCurrent(); inputs.elevatorRightAppliedVolts = rightElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); inputs.elevatorRightPositionMeters = rightEncoder.getPosition(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 80abe8e..d0f6801 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -22,11 +22,6 @@ public class ElevatorIOSim implements ElevatorIO { @Override public void updateInputs(ElevatorIOInputs inputs) { - -if(!inputs.openLoop){ - setVoltage(ffmodel.calculate(setpoint.velocity, setpoint.position) + controller.calculate(setpoint.velocity)); - setpoint = profile.calculate(0.02, setpoint, goal); - } inputs.elevatorLeftPositionMeters = elevatorSim.getPositionMeters(); inputs.elevatorLeftVelocityMetersPerSecond = elevatorSim.getVelocityMetersPerSecond(); @@ -41,6 +36,12 @@ public void moveElevator(double speed) { elevatorSim.setInputVoltage(elevatorAppliedVolts); } +@Override +public void updateMotionProfile() { + setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(setpoint.velocity)); + setpoint = profile.calculate(0.02, setpoint, goal); +} + @Override public void hold() { double ffvolts = ffmodel.calculate(0); From fded3f0c2fb8deedcf55fd5abaee52cf70f24620 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Sun, 9 Feb 2025 17:15:28 -0500 Subject: [PATCH 102/110] 3d pose with intake sim --- src/main/java/frc/robot/RobotContainer.java | 9 ++++++--- src/main/java/frc/robot/RobotVisualizer.java | 18 ++++++++++++++---- .../robot/subsystems/elevator/ElevatorIO.java | 3 ++- .../subsystems/elevator/ElevatorIOSim.java | 19 +++++++++++-------- .../frc/robot/subsystems/intake/Intake.java | 6 +++++- .../robot/subsystems/intake/IntakeIOSim.java | 2 +- 6 files changed, 39 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 44946c5..a5bfc7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -117,9 +117,12 @@ private void configureBindings() { ); Constants.OIConstants.operatorController.povRight().onTrue( - Commands.parallel( - Arm.getInstance().setGoal(ArmConstants.CORAL_L4), - Elevator.getInstance().setGoal(ElevatorConstants.maxDistance) + Commands.sequence( + Commands.parallel( + Arm.getInstance().setGoal(ArmConstants.CORAL_L4), + Elevator.getInstance().setGoal(ElevatorConstants.maxDistance) + ), + intake.intake().withTimeout(2) ) ); diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java index 0a48878..d96b44a 100644 --- a/src/main/java/frc/robot/RobotVisualizer.java +++ b/src/main/java/frc/robot/RobotVisualizer.java @@ -5,6 +5,11 @@ import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; @@ -13,14 +18,16 @@ import frc.robot.subsystems.arm.ArmConstants; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorConstants; +import frc.robot.subsystems.intake.Intake; public class RobotVisualizer extends SubsystemBase { private LoggedMechanism2d robotVisualizer = new LoggedMechanism2d(2, 2.5); private LoggedMechanismRoot2d robotRoot = robotVisualizer.getRoot("Robot Visualizer", 1, 0.1); - private LoggedMechanismLigament2d elevatorstage1Ligament = robotRoot.append(new LoggedMechanismLigament2d("Elevator Stage 1 Ligament", ElevatorConstants.maxDistance/.8, 90, 10, new Color8Bit(Color.kRed))); - private LoggedMechanismLigament2d elevatorstage2Ligament = robotRoot.append(new LoggedMechanismLigament2d("Elevator Stage 2 Ligament", ElevatorConstants.maxDistance, 90, 10, new Color8Bit(Color.kBlue))); - private LoggedMechanismLigament2d armLigament = elevatorstage2Ligament.append(new LoggedMechanismLigament2d("Arm Ligament", ArmConstants.Sim.LENGTH, -90, 10, new Color8Bit(Color.kGreen))); + private LoggedMechanismLigament2d elevatorstage1Ligament = robotRoot.append(new LoggedMechanismLigament2d("Elevator Stage 1 Ligament", ElevatorConstants.maxDistance/.9, 90, 10, new Color8Bit(Color.kRed))); + private LoggedMechanismLigament2d elevatorstage2Ligament = robotRoot.append(new LoggedMechanismLigament2d("Elevator Stage 2 Ligament", ElevatorConstants.maxDistance, 90, 9, new Color8Bit(Color.kBlue))); + private LoggedMechanismLigament2d armLigament = elevatorstage2Ligament.append(new LoggedMechanismLigament2d("Arm Ligament", ArmConstants.Sim.LENGTH, -90, 8, new Color8Bit(Color.kGreen))); + private LoggedMechanismLigament2d intakeLigament = armLigament.append(new LoggedMechanismLigament2d("Intake Ligament", 0.1, -90, 7, new Color8Bit(Color.kYellow))); public RobotVisualizer() { @@ -28,8 +35,11 @@ public RobotVisualizer() { @Override public void periodic() { + intakeLigament.setAngle(Units.rotationsToDegrees(Intake.getInstance().getPosition())); armLigament.setAngle(Units.radiansToDegrees(Arm.getInstance().getAngle()) - 90); - elevatorstage2Ligament.setLength(ElevatorConstants.maxDistance/.8 + Elevator.getInstance().getPosition()); + elevatorstage2Ligament.setLength(ElevatorConstants.maxDistance/.9 + Elevator.getInstance().getPosition()); Logger.recordOutput("RobotVisualizer", robotVisualizer); + Pose2d fakePose = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape).getTagPose(7).get().toPose2d().plus(new Transform2d(1, 0, new Rotation2d(Math.PI))); + Logger.recordOutput("Fake robot pose", fakePose); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index a55a048..4cf27f2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -14,9 +14,10 @@ public static class ElevatorIOInputs { public double elevatorLeftPositionMeters = 0.0; public double elevatorLeftVelocityMetersPerSecond = 0.0; public double elevatorLeftCurrent = 0.0; - + public double goalHeight = 0.0; public double setpointVelocity = 0.0; + public double setpointPosition = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index d0f6801..e9b4483 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.elevator; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -13,21 +14,23 @@ public class ElevatorIOSim implements ElevatorIO { private double elevatorAppliedVolts = 0.0; -private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg - .3, ElevatorConstants.kv); -private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1, 1); -private final PIDController controller = new PIDController(1, 0, 0); +private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg - .28, ElevatorConstants.kv); +private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 4); +private final PIDController controller = new PIDController(50, 0, 1); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); private TrapezoidProfile.State goal = new TrapezoidProfile.State(); @Override public void updateInputs(ElevatorIOInputs inputs) { - + elevatorSim.update(0.02); inputs.elevatorLeftPositionMeters = elevatorSim.getPositionMeters(); inputs.elevatorLeftVelocityMetersPerSecond = elevatorSim.getVelocityMetersPerSecond(); inputs.elevatorLeftCurrent = elevatorSim.getCurrentDrawAmps(); inputs.elevatorLeftAppliedVolts = elevatorAppliedVolts; - elevatorSim.update(0.02); + inputs.goalHeight = goal.position; + inputs.setpointVelocity = setpoint.velocity; + inputs.setpointPosition = setpoint.position; } @Override @@ -38,7 +41,7 @@ public void moveElevator(double speed) { @Override public void updateMotionProfile() { - setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(setpoint.velocity)); + setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(elevatorSim.getPositionMeters(), setpoint.position)); setpoint = profile.calculate(0.02, setpoint, goal); } @@ -50,9 +53,9 @@ public void hold() { @Override public void setVoltage(double volts){ - elevatorAppliedVolts = volts; + elevatorAppliedVolts = MathUtil.clamp(volts, -12, 12); // Logger.recordOutput("Setting output", volts); - elevatorSim.setInputVoltage(volts); + elevatorSim.setInputVoltage(elevatorAppliedVolts); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 991a225..f06abcf 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -28,8 +28,13 @@ private Intake(IntakeIO io) { io.updateInputs(inputs); } + public double getPosition() { + return inputs.angularPositionRot; + } + public void setIntakeSpeed(double speed){ intakeSpeed = speed; + io.setMotorSpeed(intakeSpeed); } /* @@ -68,7 +73,6 @@ public boolean hasGamepiece() { public void periodic(){ io.updateInputs(inputs); Logger.processInputs("Intake", inputs); - io.setMotorSpeed(intakeSpeed); Logger.recordOutput("Intake/Intake speed", intakeSpeed); Logger.recordOutput("Intake/Gamepiece detected", hasGamepiece()); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index a8f25d9..e84c1c7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -8,7 +8,7 @@ public class IntakeIOSim implements IntakeIO { // not done - private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1), DCMotor.getKrakenX60(1), 0.0, 0.0); + private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.001, 2), DCMotor.getKrakenX60(1)); private boolean isGamepieceDetected = false; @Override public void updateInputs(IntakeIOInputs inputs) { From dcc9d9f9b9fe8e4594902ce369464932df364a19 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Mon, 10 Feb 2025 18:09:56 -0500 Subject: [PATCH 103/110] arm weirdness --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 86 ++++++------------- .../java/frc/robot/subsystems/arm/Arm.java | 24 ++++-- .../java/frc/robot/subsystems/arm/ArmIO.java | 4 + .../frc/robot/subsystems/arm/ArmIOReal.java | 29 +++++-- .../elevator/ElevatorConstants.java | 2 +- .../subsystems/elevator/ElevatorIOReal.java | 18 ++-- 7 files changed, 83 insertions(+), 82 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 42645fe..f5dbaed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,7 +9,7 @@ public static final class IntakeSetpoints{ public static final double slow = -0.3; public static final double reverse = 0.7; } - public static Mode currentMode = Mode.SIM; + public static Mode currentMode = Mode.REAL; public static enum Mode { SIM, REAL, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5bfc7e..e34c707 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,34 +104,34 @@ private void configureBindings() { OIConstants.operatorController.rightBumper().whileTrue( Commands.run( - () -> elevator.setVoltage(4), + () -> elevator.setVoltage(1), elevator ) ); OIConstants.operatorController.leftBumper().whileTrue( Commands.run( - () -> elevator.setVoltage(-4), + () -> elevator.setVoltage(-1), elevator ) ); - Constants.OIConstants.operatorController.povRight().onTrue( - Commands.sequence( - Commands.parallel( - Arm.getInstance().setGoal(ArmConstants.CORAL_L4), - Elevator.getInstance().setGoal(ElevatorConstants.maxDistance) - ), - intake.intake().withTimeout(2) - ) - ); + // Constants.OIConstants.operatorController.povRight().onTrue( + // Commands.sequence( + // Commands.parallel( + // Arm.getInstance().setGoal(ArmConstants.CORAL_L4), + // Elevator.getInstance().setGoal(ElevatorConstants.maxDistance) + // ), + // intake.intake().withTimeout(2) + // ) + // ); - Constants.OIConstants.operatorController.povLeft().onTrue( - Commands.parallel( - Arm.getInstance().setGoal(ArmConstants.Sim.INIT_ANGLE), - Elevator.getInstance().setGoal(ElevatorConstants.minDistance) - ) - ); + // Constants.OIConstants.operatorController.povLeft().onTrue( + // Commands.parallel( + // Arm.getInstance().setGoal(ArmConstants.Sim.INIT_ANGLE), + // Elevator.getInstance().setGoal(ElevatorConstants.minDistance) + // ) + // ); Constants.OIConstants.operatorController.povUp() @@ -146,58 +146,24 @@ private void configureBindings() { Constants.OIConstants.operatorController.x().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(-0.5), () -> intake.setIntakeSpeed(0), intake)); - Constants.OIConstants.operatorController.a().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(0.5), + Constants.OIConstants.operatorController.b().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(0.5), () -> intake.setIntakeSpeed(0), intake)); - // OIConstants.driverController.povUp().whileTrue( - // Commands.startEnd( - // () -> Arm.getInstance().manualVoltage(9), - // () -> Arm.getInstance().manualVoltage(ArmConstants.DEFAULTkG * Math.cos(Arm.getInstance().getAngle() - Math.PI/2)), - // Arm.getInstance() - // ) - // ); + Constants.OIConstants.operatorController.y().whileTrue( + elevator.setGoal(ElevatorConstants.maxDistance/2) + ); - OIConstants.driverController.povDown().onTrue((Arm.getInstance().setGoal(0))); - OIConstants.driverController.povRight().onTrue((Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); - OIConstants.driverController.povUp().onTrue((Arm.getInstance().setGoal(ArmConstants.CORAL_L3))); + // OIConstants.driverController.povDown().onTrue((Arm.getInstance().setGoal(0))); + // OIConstants.driverController.povRight().onTrue((Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); + OIConstants.driverController.povUp().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L3))); // OIConstants.operatorController.povDown().onTrue((elevator.setGoal(0.05))); // OIConstants.operatorController.povRight().onTrue((elevator.setGoal(0.4))); // OIConstants.operatorController.povUp().onTrue((elevator.setGoal(0.69))); - - // OIConstants.driverController.leftTrigger().whileTrue( - // Commands.startEnd( - // () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), - // () -> Arm.getInstance().manualVoltage(0), - // Arm.getInstance() - // )); - - - // OIConstants.driverController.rightTrigger().whileTrue( - // Commands.startEnd( - // () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), - // () -> Arm.getInstance().manualVoltage(0), - // Arm.getInstance() - // )); - OIConstants.driverController.y().whileTrue( - Commands.run( - () -> Arm.getInstance().manualVoltage( 6), - Arm.getInstance() - ) - ); - - // up - OIConstants.driverController.a().whileTrue( - Commands.run( - () -> Arm.getInstance().manualVoltage(-6), - Arm.getInstance() - ) - ); - - OIConstants.driverController.leftBumper().onTrue(Arm.getInstance().setGoal(0)); - OIConstants.driverController.rightBumper().onTrue(Arm.getInstance().setGoal(ArmConstants.CORAL_L2)); + OIConstants.driverController.leftBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(0))); + OIConstants.driverController.rightBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 30a23bc..58dd9d3 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.arm; +import java.util.regex.MatchResult; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; @@ -12,6 +14,7 @@ import frc.robot.Constants; import frc.robot.Constants.Gamepiece; import frc.robot.Constants.Mode; +import frc.robot.Constants.OIConstants; public class Arm extends SubsystemBase { private ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); @@ -20,7 +23,15 @@ public class Arm extends SubsystemBase { private Arm(ArmIO io) { this.io = io; - setDefaultCommand(Commands.run(() -> io.hold(), this)); + setDefaultCommand( + Commands.run(() -> { + double up = MathUtil.applyDeadband(OIConstants.driverController.getLeftTriggerAxis(), 0.1); + double down = MathUtil.applyDeadband(OIConstants.driverController.getRightTriggerAxis(), 0.1); + double change = (up - down) * 0.1; + + io.setGoal(inputs.goalAngle + change); + }, this) + ); setFFMode(Constants.currentMode == Mode.SIM ? Gamepiece.SIM: Gamepiece.NONE); } @@ -28,6 +39,10 @@ public double getAngle() { return inputs.angularPosition; } + public double getGoalAngle() { + return inputs.goalAngle; + } + public static void initialize(ArmIO io) { instance = new Arm(io); } @@ -36,10 +51,8 @@ public static Arm getInstance() { return instance; } - public Command setGoal(double angle) { - return Commands.runOnce(() -> io.setGoal(angle), this) - .andThen(Commands.run(() -> io.updateMotionProfile(), this)) - .until(() -> MathUtil.isNear(angle, inputs.angularPosition, 0.008)); + public void setGoal(double angle) { + io.setGoal(angle); } public void resetAbsoluteEncoder() { @@ -77,6 +90,7 @@ public void setFFMode(Gamepiece gamepieceType) { @Override public void periodic() { + io.updateMotionProfile(); Logger.recordOutput(getName() + "/Pose", getArmPose()); io.updateInputs(inputs); Logger.processInputs(getName(), inputs); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 1fca7f3..1991efa 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -27,6 +27,10 @@ public default void setVoltage(double voltage) { } + public default double getGoal() { + return 0.0; + } + public default void updateMotionProfile(){ } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 9cfa75c..1e80121 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -26,8 +27,8 @@ public class ArmIOReal implements ArmIO { private final SparkClosedLoopController onboardController = armMotor.getClosedLoopController(); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ArmConstants.MAX_VELOCITY, ArmConstants.MAX_ACCELERATION); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); - private TrapezoidProfile.State goal = new TrapezoidProfile.State(0, 0); - private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + private TrapezoidProfile.State goal; + private TrapezoidProfile.State setpoint; public ArmIOReal(){ config.smartCurrentLimit(ArmConstants.CURRENT_LIMIT); @@ -59,11 +60,19 @@ public ArmIOReal(){ armEncoder = armMotor.getAbsoluteEncoder(); armMotor.getEncoder().setPosition(getOffsetAngle()); + + goal = new TrapezoidProfile.State(getOffsetAngle(), 0); + setpoint = new TrapezoidProfile.State(); } @Override public void setGoal(double angle) { - goal = new TrapezoidProfile.State(angle, 0); + goal = new TrapezoidProfile.State(MathUtil.clamp(angle, ArmConstants.Sim.MIN_ANGLE, ArmConstants.Sim.MAX_ANGLE), 0); + } + + @Override + public double getGoal() { + return goal.position; } @Override @@ -78,11 +87,6 @@ public void setFFValues(double kS, double kG, double kV, double kA) { ffmodel = new ArmFeedforward(kS, kG, kV); } - @Override - public void hold() { - double ffvolts = ffmodel.calculate(getOffsetAngle(), 0); - setVoltage(ffvolts); - } @Override public void resetEncoder() { @@ -95,12 +99,19 @@ public void setVoltage(double voltage) { } public double getOffsetAngle() { - return armEncoder.getPosition() - ArmConstants.ARM_OFFSET; + double withoffset = armEncoder.getPosition() - ArmConstants.ARM_OFFSET; + if(withoffset > Math.PI) { + return -1 * (2 * Math.PI - withoffset); + } + else { + return withoffset; + } } @Override public void updateInputs(ArmIOInputs inputs) { inputs.angularPosition = getOffsetAngle(); + // inputs.angularPosition = armEncoder.getPosition(); inputs.angularVelocity = armEncoder.getVelocity(); inputs.current = armMotor.getOutputCurrent(); inputs.voltage = armMotor.getAppliedOutput() * armMotor.getBusVoltage(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index f2f1ef1..bc24230 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -23,7 +23,7 @@ public final class ElevatorConstants{ public static final double conversionFactor = Units.inchesToMeters(1.0/9.0 * Math.PI * 1.751); // gearing * pi * sprocket diameter - public static final int elevatorCurrentLimits = 40; //might need to adjust later + public static final int elevatorCurrentLimits = 60; //might need to adjust later diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 5b26053..65fe382 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -48,10 +48,10 @@ public ElevatorIOReal(){ config.encoder.positionConversionFactor(ElevatorConstants.conversionFactor); config.encoder.velocityConversionFactor(ElevatorConstants.conversionFactor / 60); - config.softLimit.forwardSoftLimitEnabled(true); - config.softLimit.reverseSoftLimitEnabled(true); - config.softLimit.forwardSoftLimit(ElevatorConstants.maxDistance); - config.softLimit.reverseSoftLimit(ElevatorConstants.minDistance); + // config.softLimit.forwardSoftLimitEnabled(true); + // config.softLimit.reverseSoftLimitEnabled(true); + // config.softLimit.forwardSoftLimit(ElevatorConstants.maxDistance); + // config.softLimit.reverseSoftLimit(ElevatorConstants.minDistance); config.closedLoop.p(0); config.closedLoop.i(0); @@ -59,7 +59,7 @@ public ElevatorIOReal(){ config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits); config.idleMode(IdleMode.kBrake); - + config.inverted(true); rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); config.follow(ElevatorConstants.rightDeviceID, true); @@ -85,10 +85,16 @@ public void updateMotionProfile() { setpoint = profile.calculate(0.02, setpoint, goal); } + @Override + public void hold() { + double ffvolts = ffmodel.calculate(0); + setVoltage(ffvolts); + } + @Override public void updateInputs(ElevatorIOInputs inputs) { inputs.elevatorRightCurrent = rightElevatorMotor.getOutputCurrent(); - inputs.elevatorRightAppliedVolts = rightElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage(); + inputs.elevatorRightAppliedVolts = rightElevatorMotor.getAppliedOutput() * rightElevatorMotor.getBusVoltage(); inputs.elevatorRightPositionMeters = rightEncoder.getPosition(); inputs.elevatorRightVelocityMetersPerSecond = rightEncoder.getVelocity(); // inputs.elevatorRightMotorPositionMeters = leftAbsoluteEncoder.getPosition(); From d919adc9128bb0db325bcf00b5e6f91be35f6adf Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Mon, 10 Feb 2025 20:37:32 -0500 Subject: [PATCH 104/110] We can score with setpoints (about 50% accuracy) --- src/main/java/frc/robot/RobotContainer.java | 40 ++++++++++--------- .../frc/robot/subsystems/CANCoderTest.java | 15 ------- .../java/frc/robot/subsystems/arm/Arm.java | 2 +- .../robot/subsystems/arm/ArmConstants.java | 6 +-- .../java/frc/robot/subsystems/arm/ArmIO.java | 1 + .../frc/robot/subsystems/arm/ArmIOReal.java | 27 ++++++++----- .../robot/subsystems/elevator/Elevator.java | 3 +- .../elevator/ElevatorConstants.java | 2 +- .../subsystems/elevator/ElevatorIOReal.java | 6 ++- 9 files changed, 49 insertions(+), 53 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/CANCoderTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e34c707..0f1384a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIORealVortex; import frc.robot.subsystems.intake.IntakeIOSim; -import frc.robot.subsystems.CANCoderTest; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmConstants; import frc.robot.subsystems.arm.ArmIOSim; @@ -37,7 +36,6 @@ public RobotContainer() throws IOException, ParseException { elevator = Elevator.initialize(new ElevatorIOReal()); intake = Intake.initialize(new IntakeIORealVortex()); Arm.initialize(new ArmIOReal()); - CANCoderTest canCoderTest = new CANCoderTest(); // drive = new Drive( // new GyroIOPigeon(), @@ -116,22 +114,22 @@ private void configureBindings() { ) ); - // Constants.OIConstants.operatorController.povRight().onTrue( - // Commands.sequence( - // Commands.parallel( - // Arm.getInstance().setGoal(ArmConstants.CORAL_L4), - // Elevator.getInstance().setGoal(ElevatorConstants.maxDistance) - // ), - // intake.intake().withTimeout(2) - // ) - // ); + Constants.OIConstants.operatorController.povRight().onTrue( + Commands.sequence( + Commands.parallel( + Commands.runOnce(() -> Arm.getInstance().setGoal(1.02)), + Elevator.getInstance().setGoal(0.57) + ) + ) + ); - // Constants.OIConstants.operatorController.povLeft().onTrue( - // Commands.parallel( - // Arm.getInstance().setGoal(ArmConstants.Sim.INIT_ANGLE), - // Elevator.getInstance().setGoal(ElevatorConstants.minDistance) - // ) - // ); + Constants.OIConstants.operatorController.povLeft().onTrue( + Commands.sequence( + Elevator.getInstance().setGoal(ElevatorConstants.minDistance).withTimeout(2), + Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8)), + Commands.waitSeconds(1.5) + ) + ); Constants.OIConstants.operatorController.povUp() @@ -149,9 +147,13 @@ private void configureBindings() { Constants.OIConstants.operatorController.b().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(0.5), () -> intake.setIntakeSpeed(0), intake)); - Constants.OIConstants.operatorController.y().whileTrue( + Constants.OIConstants.operatorController.y().onTrue( elevator.setGoal(ElevatorConstants.maxDistance/2) ); + + Constants.OIConstants.operatorController.a().onTrue( + elevator.setGoal(0) + ); // OIConstants.driverController.povDown().onTrue((Arm.getInstance().setGoal(0))); // OIConstants.driverController.povRight().onTrue((Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); @@ -162,7 +164,7 @@ private void configureBindings() { // OIConstants.operatorController.povRight().onTrue((elevator.setGoal(0.4))); // OIConstants.operatorController.povUp().onTrue((elevator.setGoal(0.69))); - OIConstants.driverController.leftBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(0))); + OIConstants.driverController.leftBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8))); OIConstants.driverController.rightBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); } diff --git a/src/main/java/frc/robot/subsystems/CANCoderTest.java b/src/main/java/frc/robot/subsystems/CANCoderTest.java deleted file mode 100644 index 637e484..0000000 --- a/src/main/java/frc/robot/subsystems/CANCoderTest.java +++ /dev/null @@ -1,15 +0,0 @@ -package frc.robot.subsystems; - -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.hardware.CANcoder; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class CANCoderTest extends SubsystemBase { - private CANcoder encoder = new CANcoder(19); - @Override - public void periodic() { - Logger.recordOutput("CANCoder Angle", encoder.getAbsolutePosition().getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 58dd9d3..a584415 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -29,7 +29,7 @@ private Arm(ArmIO io) { double down = MathUtil.applyDeadband(OIConstants.driverController.getRightTriggerAxis(), 0.1); double change = (up - down) * 0.1; - io.setGoal(inputs.goalAngle + change); + // io.setGoal(inputs.goalAngle + change); }, this) ); setFFMode(Constants.currentMode == Mode.SIM ? Gamepiece.SIM: Gamepiece.NONE); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 9ffb424..5de237d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -5,7 +5,7 @@ public class ArmConstants { public static final int CAN_ID = 14; public static final int CURRENT_LIMIT = 50; - public static final double ARM_OFFSET = Math.PI/2; + public static final double ARM_OFFSET = 1.8; public static final double CORAL_L1 = Units.degreesToRadians(-34.74); public static final double CORAL_L2 = Units.degreesToRadians(-54.1); // with elevator up @@ -24,8 +24,8 @@ public class ArmConstants { // Default FF public static final double DEFAULTkG = 1.02; public static final double DEFAULTkV = 1.01; - public static final double DEFAULTkA = 0.05; - public static final double DEFAULTkS = 0.1; + public static final double DEFAULTkA = 0.07; + public static final double DEFAULTkS = 0.3; // Algae FF public static final double ALGAEkG = 1.02; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 1991efa..5a8f209 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -14,6 +14,7 @@ public static class ArmIOInputs{ public double busVoltage = 0.0; public double relativeEncoderPosition = 0.0; public double relativeEncoderVelocity = 0.0; + public double setpointPosition = 0.0; } public default void updateInputs(ArmIOInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 1e80121..5ae77d0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -23,7 +23,7 @@ public class ArmIOReal implements ArmIO { private SparkFlexConfig config = new SparkFlexConfig(); private SparkAbsoluteEncoder armEncoder; - private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA); + private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA, 0.02); private final SparkClosedLoopController onboardController = armMotor.getClosedLoopController(); private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ArmConstants.MAX_VELOCITY, ArmConstants.MAX_ACCELERATION); private final TrapezoidProfile profile = new TrapezoidProfile(constraints); @@ -39,17 +39,17 @@ public ArmIOReal(){ config.absoluteEncoder.inverted(false); config.encoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); - config.encoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING); - - config.openLoopRampRate(1); + config.encoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60); // config.softLimit.forwardSoftLimitEnabled(true); // config.softLimit.reverseSoftLimitEnabled(false); // config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE); // config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE); - config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - config.closedLoop.p(0.0); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + config.closedLoop.positionWrappingEnabled(true); + config.closedLoop.positionWrappingInputRange(0, Math.PI); + config.closedLoop.p(5.0); config.closedLoop.i(0.0); config.closedLoop.d(0.0); @@ -62,12 +62,12 @@ public ArmIOReal(){ armMotor.getEncoder().setPosition(getOffsetAngle()); goal = new TrapezoidProfile.State(getOffsetAngle(), 0); - setpoint = new TrapezoidProfile.State(); + setpoint = new TrapezoidProfile.State(getOffsetAngle(), 0); } @Override public void setGoal(double angle) { - goal = new TrapezoidProfile.State(MathUtil.clamp(angle, ArmConstants.Sim.MIN_ANGLE, ArmConstants.Sim.MAX_ANGLE), 0); + goal = new TrapezoidProfile.State(angle, 0); } @Override @@ -77,9 +77,15 @@ public double getGoal() { @Override public void updateMotionProfile() { - double ffvolts = ffmodel.calculate(getOffsetAngle(), setpoint.velocity); - onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); + double prevVelocity = setpoint.velocity; + Logger.recordOutput("prev velocity", prevVelocity); setpoint = profile.calculate(0.02, setpoint, goal); + double acceleration = (setpoint.velocity - prevVelocity) / 0.02; + Logger.recordOutput("acceleration", acceleration); + double ffvolts = ffmodel.calculate(getOffsetAngle(), setpoint.velocity, acceleration); + Logger.recordOutput("arm ffvolts", ffvolts); + onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); + } @Override @@ -121,5 +127,6 @@ public void updateInputs(ArmIOInputs inputs) { inputs.appliedOutput = armMotor.getAppliedOutput(); inputs.relativeEncoderPosition = armMotor.getEncoder().getPosition(); inputs.relativeEncoderVelocity = armMotor.getEncoder().getVelocity(); + inputs.setpointPosition = setpoint.position; } } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index e2aaf4e..bc09229 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -34,8 +34,7 @@ private Elevator(ElevatorIO elevatorIO){ public Command setGoal(double goal){ return Commands.runOnce(() -> io.setGoal(goal), this) - .andThen(Commands.run(() -> io.updateMotionProfile(), this)) - .until(() -> MathUtil.isNear(goal, inputs.elevatorLeftPositionMeters, 0.008)); + .andThen(Commands.run(() -> io.updateMotionProfile(), this)); } public void moveElevator(double speed) { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index bc24230..af6e951 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -11,7 +11,7 @@ public final class ElevatorConstants{ public static final double kD = 0; //figure this out later public static final double kI = 0; //figure this out later - public static final double kP = 0; //figure this out later + public static final double kP = 10; //figure this out later public static final int rightDeviceID = 12; public static final int leftDeviceID = 13; public static final double ks = 0.01; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 65fe382..6da70ba 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -80,9 +80,11 @@ public void setGoal(double height) { @Override public void updateMotionProfile() { - double ffvolts = ffmodel.calculate(setpoint.velocity); - controller.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); + double prevVelocity = setpoint.velocity; setpoint = profile.calculate(0.02, setpoint, goal); + double acceleration = (setpoint.velocity - prevVelocity) / 0.02; + double ffvolts = ffmodel.calculate(setpoint.velocity, acceleration); + controller.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); } @Override From cf3a65acbc8abb42dac51c37addf5641efdcae1a Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Tue, 11 Feb 2025 16:35:30 -0500 Subject: [PATCH 105/110] ctre generated swerve --- src/main/java/frc/robot/RobotContainer.java | 57 +++- src/main/java/frc/robot/Telemetry.java | 124 ++++++++ .../frc/robot/generated/TunerConstants.java | 286 +++++++++++++++++ .../subsystems/CommandSwerveDrivetrain.java | 288 ++++++++++++++++++ tuner-project.json | 1 + 5 files changed, 755 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/Telemetry.java create mode 100644 src/main/java/frc/robot/generated/TunerConstants.java create mode 100644 src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java create mode 100644 tuner-project.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..bfaa306 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,70 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; + +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; public class RobotContainer { + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + private final Telemetry logger = new Telemetry(MaxSpeed); + + private final CommandXboxController joystick = new CommandXboxController(0); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + public RobotContainer() { configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> + drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + ) + ); + + joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); + joystick.b().whileTrue(drivetrain.applyRequest(() -> + point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) + )); + + // Run SysId routines when holding back/start and X/Y. + // Note that each routine should be run exactly once in a single log. + joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + // reset the field-centric heading on left bumper press + joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + + drivetrain.registerTelemetry(logger::telemeterize); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java new file mode 100644 index 0000000..edf1979 --- /dev/null +++ b/src/main/java/frc/robot/Telemetry.java @@ -0,0 +1,124 @@ +package frc.robot; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + private final double[] m_poseArray = new double[3]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; + + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + + /* Also write to log file */ + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } + + SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); + SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); + SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + fieldPub.set(m_poseArray); + + /* Telemeterize the module states to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java new file mode 100644 index 0000000..9e77c31 --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -0,0 +1,286 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; + +import frc.robot.subsystems.CommandSwerveDrivetrain; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.66).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.75); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 4.2; + + private static final double kDriveGearRatio = 6.720000000000001; + private static final double kSteerGearRatio = 13.371428571428572; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 20; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 22; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.164794921875); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.625); + private static final Distance kFrontLeftYPos = Inches.of(10.625); + + // Front Right + private static final int kFrontRightDriveMotorId = 3; + private static final int kFrontRightSteerMotorId = 4; + private static final int kFrontRightEncoderId = 23; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.0107421875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.625); + private static final Distance kFrontRightYPos = Inches.of(-10.625); + + // Back Left + private static final int kBackLeftDriveMotorId = 8; + private static final int kBackLeftSteerMotorId = 7; + private static final int kBackLeftEncoderId = 21; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.458984375); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.625); + private static final Distance kBackLeftYPos = Inches.of(10.625); + + // Back Right + private static final int kBackRightDriveMotorId = 6; + private static final int kBackRightSteerMotorId = 5; + private static final int kBackRightEncoderId = 24; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.07958984375); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.625); + private static final Distance kBackRightYPos = Inches.of(-10.625); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java new file mode 100644 index 0000000..d127487 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -0,0 +1,288 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; + +/** + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements + * Subsystem so it can easily be used in command-based projects. + */ +public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this + ) + ); + + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this + ) + ); + + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + }, + null, + this + ) + ); + + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, odometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + /** + * Runs the SysId Quasistatic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + /** + * Runs the SysId Dynamic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + @Override + public void periodic() { + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent(allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation + ); + m_hasAppliedOperatorPerspective = true; + }); + } + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + *

+ * Note that the vision measurement standard deviations passed into this method + * will continue to apply to future measurements until a subsequent call to + * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement + * in the form [x, y, theta]ᵀ, with units in meters and radians. + */ + @Override + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } +} diff --git a/tuner-project.json b/tuner-project.json new file mode 100644 index 0000000..c1658a3 --- /dev/null +++ b/tuner-project.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":22,"Name":"Front Left Encoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":2,"Name":"FrontLeft Turn","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":1,"Name":"FrontLeft Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.164794921875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":2,"ValidatedDriveId":1,"ValidatedEncoderId":22},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":23,"Name":"Front Right Encoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":4,"Name":"Front Right Turn","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":3,"Name":"Front Right Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.0107421875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":4,"ValidatedDriveId":3,"ValidatedEncoderId":23},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":21,"Name":"Back Left Encoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":7,"Name":"BackLeft Turn","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":8,"Name":"Back Left Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.458984375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":7,"ValidatedDriveId":8,"ValidatedEncoderId":21},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":24,"Name":"Back Right Encoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":5,"Name":"Back Right Turn","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":6,"Name":"Back Right Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.07958984375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":5,"ValidatedDriveId":6,"ValidatedEncoderId":24}],"SwerveOptions":{"kSpeedAt12Volts":4.749788892927424,"Gyro":{"Id":20,"Name":"Pigeon 2 (Device ID 10)","Model":"Pigeon 2","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":21.25,"HorizontalTrackSizeInches":21.25,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":8,"SwerveModuleConfiguration":{"ModuleBrand":8,"DriveRatio":6.720000000000001,"SteerRatio":13.371428571428572,"CouplingRatio":4.2,"CustomName":"X2 10T"},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"WestCoast Products (WCP)","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file From 77dd2b436d875426df9e598f3a095ed323182615 Mon Sep 17 00:00:00 2001 From: Thomas <119466406+ColinH0@users.noreply.github.com> Date: Wed, 12 Feb 2025 09:25:20 -0500 Subject: [PATCH 106/110] changes from meetig 2/11 uploaded by justin at outreach event --- src/main/java/frc/robot/RobotContainer.java | 69 ++++++++++++------- .../java/frc/robot/subsystems/arm/Arm.java | 28 +++++++- .../frc/robot/subsystems/arm/ArmIOReal.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 3 +- .../elevator/ElevatorConstants.java | 4 +- .../subsystems/elevator/ElevatorIOReal.java | 3 +- .../subsystems/intake/IntakeConstants.java | 2 +- 7 files changed, 78 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f1384a..dc94c8b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,7 @@ import org.json.simple.parser.ParseException; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.elevator.Elevator; @@ -23,6 +24,7 @@ import frc.robot.subsystems.arm.ArmConstants; import frc.robot.subsystems.arm.ArmIOSim; import frc.robot.subsystems.arm.ArmIOReal; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class RobotContainer { @@ -90,29 +92,47 @@ private void configureBindings() { //Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); - // OIConstants.operatorController.leftTrigger() - // .whileTrue(Commands.startEnd(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 1.5), - // () -> elevator.moveElevator(0), - // elevator)); + OIConstants.operatorController.leftTrigger() + .whileTrue(Commands.startEnd(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 1.5), + () -> elevator.moveElevator(0), + elevator)); - // OIConstants.operatorController.rightTrigger() - // .whileTrue(Commands.startEnd(() -> elevator.moveElevator(-OIConstants.operatorController.getRightTriggerAxis() / 1.5), - // () -> elevator.moveElevator(0), - // elevator)); + OIConstants.operatorController.rightTrigger() + .whileTrue(Commands.startEnd(() -> elevator.moveElevator(-OIConstants.operatorController.getRightTriggerAxis() / 1.5), + () -> elevator.moveElevator(0), + elevator)); - OIConstants.operatorController.rightBumper().whileTrue( - Commands.run( - () -> elevator.setVoltage(1), - elevator - ) - ); + // OIConstants.operatorController.rightBumper().whileTrue( + // Commands.run( + // () -> elevator.setVoltage(1), + // elevator + // ) + // ); + + // OIConstants.operatorController.leftBumper().whileTrue( + // Commands.run( + // () -> elevator.setVoltage(-1), + // elevator + // ) + // ); - OIConstants.operatorController.leftBumper().whileTrue( - Commands.run( - () -> elevator.setVoltage(-1), - elevator - ) - ); + + // OIConstants.operatorController.leftBumper().whileTrue( + // Arm.getInstance().sysIDRoutine().quasistatic(SysIdRoutine.Direction.kForward) + // ); + + // OIConstants.operatorController.rightBumper().whileTrue( + // Arm.getInstance().sysIDRoutine().quasistatic(SysIdRoutine.Direction.kReverse) + // ); + + // OIConstants.operatorController.leftTrigger().whileTrue( + // Arm.getInstance().sysIDRoutine().dynamic(SysIdRoutine.Direction.kForward) + // ); + + // OIConstants.operatorController.rightTrigger().whileTrue( + // Arm.getInstance().sysIDRoutine().dynamic(SysIdRoutine.Direction.kReverse) + // ); + Constants.OIConstants.operatorController.povRight().onTrue( Commands.sequence( @@ -125,7 +145,7 @@ private void configureBindings() { Constants.OIConstants.operatorController.povLeft().onTrue( Commands.sequence( - Elevator.getInstance().setGoal(ElevatorConstants.minDistance).withTimeout(2), + Elevator.getInstance().setGoal(ElevatorConstants.minDistance), Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8)), Commands.waitSeconds(1.5) ) @@ -141,10 +161,10 @@ private void configureBindings() { ) ); - Constants.OIConstants.operatorController.x().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(-0.5), + Constants.OIConstants.operatorController.x().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(-1), () -> intake.setIntakeSpeed(0), intake)); - Constants.OIConstants.operatorController.b().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(0.5), + Constants.OIConstants.operatorController.b().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(1), () -> intake.setIntakeSpeed(0), intake)); Constants.OIConstants.operatorController.y().onTrue( @@ -169,7 +189,6 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return Commands.sequence( - ); + return Commands.print("hi"); } } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index a584415..ad22e8f 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -1,5 +1,9 @@ package frc.robot.subsystems.arm; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import java.util.regex.MatchResult; import org.littletonrobotics.junction.Logger; @@ -8,9 +12,13 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Gamepiece; import frc.robot.Constants.Mode; @@ -21,6 +29,19 @@ public class Arm extends SubsystemBase { private ArmIO io; private static Arm instance; + private SysIdRoutine sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism(volts -> { + manualVoltage(volts.magnitude()); + }, + log -> { + log.motor("arm motor") + .voltage(Voltage.ofBaseUnits(inputs.voltage, Volts)) + .angularPosition(Angle.ofBaseUnits(inputs.relativeEncoderPosition, Radians)) + .angularVelocity(AngularVelocity.ofBaseUnits(inputs.relativeEncoderVelocity, RadiansPerSecond)); + }, this) + ); + private Arm(ArmIO io) { this.io = io; setDefaultCommand( @@ -28,8 +49,7 @@ private Arm(ArmIO io) { double up = MathUtil.applyDeadband(OIConstants.driverController.getLeftTriggerAxis(), 0.1); double down = MathUtil.applyDeadband(OIConstants.driverController.getRightTriggerAxis(), 0.1); double change = (up - down) * 0.1; - - // io.setGoal(inputs.goalAngle + change); + io.setGoal(inputs.goalAngle + change); }, this) ); setFFMode(Constants.currentMode == Mode.SIM ? Gamepiece.SIM: Gamepiece.NONE); @@ -59,6 +79,10 @@ public void resetAbsoluteEncoder() { io.resetEncoder(); } + public SysIdRoutine sysIDRoutine() { + return sysIdRoutine; + } + public void manualVoltage(double voltage) { io.setVoltage(voltage); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java index 5ae77d0..c5d6703 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -82,7 +82,7 @@ public void updateMotionProfile() { setpoint = profile.calculate(0.02, setpoint, goal); double acceleration = (setpoint.velocity - prevVelocity) / 0.02; Logger.recordOutput("acceleration", acceleration); - double ffvolts = ffmodel.calculate(getOffsetAngle(), setpoint.velocity, acceleration); + double ffvolts = ffmodel.calculate(armMotor.getEncoder().getPosition(), setpoint.velocity, acceleration); Logger.recordOutput("arm ffvolts", ffvolts); onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index bc09229..860fc77 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -34,7 +34,8 @@ private Elevator(ElevatorIO elevatorIO){ public Command setGoal(double goal){ return Commands.runOnce(() -> io.setGoal(goal), this) - .andThen(Commands.run(() -> io.updateMotionProfile(), this)); + .andThen(Commands.run(() -> io.updateMotionProfile(), this)) + .withTimeout(2.5); } public void moveElevator(double speed) { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index af6e951..a706c83 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -15,8 +15,8 @@ public final class ElevatorConstants{ public static final int rightDeviceID = 12; public static final int leftDeviceID = 13; public static final double ks = 0.01; - public static final double kg = 0.38; - public static final double kv = 6.84; //IS THIS RIGHT?!?!? + public static final double kg = 0.35; + public static final double kv = 7; //IS THIS RIGHT?!?!? public static final double MAX_ACCELERATION = 1; public static final double MAX_VELOCITY = 1; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 6da70ba..a5db6a9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -53,7 +53,7 @@ public ElevatorIOReal(){ // config.softLimit.forwardSoftLimit(ElevatorConstants.maxDistance); // config.softLimit.reverseSoftLimit(ElevatorConstants.minDistance); - config.closedLoop.p(0); + config.closedLoop.p(10); config.closedLoop.i(0); config.closedLoop.d(0); @@ -75,6 +75,7 @@ public void setVoltage(double voltage) { @Override public void setGoal(double height) { + setpoint = new TrapezoidProfile.State(leftEncoder.getPosition(), leftEncoder.getVelocity()); goal = new TrapezoidProfile.State(height, 0); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index e92c1d1..6985873 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -8,7 +8,7 @@ public final class IntakeConstants { public static final double overrideSpeed = -0.0; public static final double intakeCurrent = 0; - public static final int currentLimit = 30; + public static final int currentLimit = 45; public static final double currentThreshold = 25; //change later based on akit numbers for gamepiece public static final double initialThreshold = 25; From f77a6764f69525a9433c9551f208082cbb8cb850 Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Wed, 12 Feb 2025 11:34:50 -0500 Subject: [PATCH 107/110] merge clean up --- src/main/java/frc/robot/RobotContainer.java | 30 ++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c560ca1..8b6000f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -109,7 +109,34 @@ public RobotContainer() throws IOException, ParseException { configureBindings(); } - private void configureBindings() { + private void configureBindings() {{ + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> + drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + ) + ); + + joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); + joystick.b().whileTrue(drivetrain.applyRequest(() -> + point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) + )); + + // Run SysId routines when holding back/start and X/Y. + // Note that each routine should be run exactly once in a single log. + joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + // reset the field-centric heading on left bumper press + joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + + drivetrain.registerTelemetry(logger::telemeterize); // drive.setDefaultCommand( // DriveCommands.joystickDrive( // drive, @@ -214,6 +241,7 @@ private void configureBindings() { OIConstants.driverController.leftBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8))); OIConstants.driverController.rightBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); } +} public Command getAutonomousCommand() { return Commands.print("hi"); From b773ddc1a53cc6bb8286a03fb3f2b30a1c16b4c0 Mon Sep 17 00:00:00 2001 From: ozzie Date: Wed, 12 Feb 2025 11:34:53 -0500 Subject: [PATCH 108/110] No manual set goal --- src/main/java/frc/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index ad22e8f..aad06be 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -49,7 +49,7 @@ private Arm(ArmIO io) { double up = MathUtil.applyDeadband(OIConstants.driverController.getLeftTriggerAxis(), 0.1); double down = MathUtil.applyDeadband(OIConstants.driverController.getRightTriggerAxis(), 0.1); double change = (up - down) * 0.1; - io.setGoal(inputs.goalAngle + change); + // io.setGoal(inputs.goalAngle + change); }, this) ); setFFMode(Constants.currentMode == Mode.SIM ? Gamepiece.SIM: Gamepiece.NONE); From 0ea96628aec7488b078d1c64f48df4092aa3000e Mon Sep 17 00:00:00 2001 From: Thomas Jin <83102001+Bob1272001@users.noreply.github.com> Date: Wed, 12 Feb 2025 11:42:42 -0500 Subject: [PATCH 109/110] fixed button bindings and added arm requirement for run once. --- src/main/java/frc/robot/RobotContainer.java | 28 ++++++++++----------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b6000f..f9e4197 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,8 +50,6 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(MaxSpeed); - private final CommandXboxController joystick = new CommandXboxController(0); - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); @@ -115,26 +113,26 @@ private void configureBindings() {{ drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + drive.withVelocityX(-Constants.OIConstants.driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(-Constants.OIConstants.driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-Constants.OIConstants.driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) ) ); - joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - joystick.b().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) + Constants.OIConstants.driverController.a().whileTrue(drivetrain.applyRequest(() -> brake)); + Constants.OIConstants.driverController.b().whileTrue(drivetrain.applyRequest(() -> + point.withModuleDirection(new Rotation2d(-Constants.OIConstants.driverController.getLeftY(), -Constants.OIConstants.driverController.getLeftX())) )); // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. - joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + Constants.OIConstants.driverController.back().and(Constants.OIConstants.driverController.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + Constants.OIConstants.driverController.back().and(Constants.OIConstants.driverController.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + Constants.OIConstants.driverController.start().and(Constants.OIConstants.driverController.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + Constants.OIConstants.driverController.start().and(Constants.OIConstants.driverController.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); // reset the field-centric heading on left bumper press - joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + Constants.OIConstants.driverController.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); drivetrain.registerTelemetry(logger::telemeterize); // drive.setDefaultCommand( @@ -191,7 +189,7 @@ private void configureBindings() {{ Constants.OIConstants.operatorController.povRight().onTrue( Commands.sequence( Commands.parallel( - Commands.runOnce(() -> Arm.getInstance().setGoal(1.02)), + Commands.runOnce(() -> Arm.getInstance().setGoal(1.02), Arm.getInstance()), Elevator.getInstance().setGoal(0.57) ) ) @@ -200,7 +198,7 @@ private void configureBindings() {{ Constants.OIConstants.operatorController.povLeft().onTrue( Commands.sequence( Elevator.getInstance().setGoal(ElevatorConstants.minDistance), - Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8)), + Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8), Arm.getInstance()), Commands.waitSeconds(1.5) ) ); From fec1a397d91582e8ade3ef02c2010821e6352fa3 Mon Sep 17 00:00:00 2001 From: ozziexyz Date: Wed, 12 Feb 2025 13:15:48 -0500 Subject: [PATCH 110/110] cleaned up button bindings --- src/main/java/frc/robot/RobotContainer.java | 213 ++++++-------------- 1 file changed, 64 insertions(+), 149 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9e4197..3f0bde1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,16 +9,12 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.math.geometry.Rotation2d; import java.io.IOException; import org.json.simple.parser.ParseException; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -26,7 +22,6 @@ import frc.robot.subsystems.elevator.ElevatorConstants; import frc.robot.subsystems.elevator.ElevatorIOReal; import frc.robot.subsystems.elevator.ElevatorIOSim; -import frc.robot.Constants.Gamepiece; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIORealVortex; @@ -35,16 +30,15 @@ import frc.robot.subsystems.arm.ArmConstants; import frc.robot.subsystems.arm.ArmIOSim; import frc.robot.subsystems.arm.ArmIOReal; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity +public class RobotContainer { + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); @@ -52,9 +46,8 @@ public class RobotContainer { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - private Elevator elevator; - //private Drive drive; + // private Drive drive; private Intake intake; public RobotContainer() throws IOException, ParseException { @@ -63,185 +56,107 @@ public RobotContainer() throws IOException, ParseException { elevator = Elevator.initialize(new ElevatorIOReal()); intake = Intake.initialize(new IntakeIORealVortex()); Arm.initialize(new ArmIOReal()); - - // drive = new Drive( - // new GyroIOPigeon(), - // new ModuleIOTalonFX(0), - // new ModuleIOTalonFX(1), - // new ModuleIOTalonFX(2), - // new ModuleIOTalonFX(3)); - break; + case SIM: elevator = Elevator.initialize(new ElevatorIOSim()); intake = Intake.initialize(new IntakeIOSim()); Arm.initialize(new ArmIOSim()); - - // drive = new Drive( - // new GyroIO() { - // }, - // new ModuleIOSim(), - // new ModuleIOSim(), - // new ModuleIOSim(), - // new ModuleIOSim()); - break; default: - elevator = Elevator.initialize(new ElevatorIOSim()); intake = Intake.initialize(new IntakeIOSim()); - Arm.initialize(new ArmIOSim()); - - // drive = new Drive( - // new GyroIO() { - // }, - // new ModuleIOSim(), - // new ModuleIOSim(), - // new ModuleIOSim(), - // new ModuleIOSim()); + break; - } RobotVisualizer robotVisualizer = new RobotVisualizer(); configureBindings(); } - private void configureBindings() {{ - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> - drive.withVelocityX(-Constants.OIConstants.driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(-Constants.OIConstants.driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-Constants.OIConstants.driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) - ) - ); - - Constants.OIConstants.driverController.a().whileTrue(drivetrain.applyRequest(() -> brake)); - Constants.OIConstants.driverController.b().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-Constants.OIConstants.driverController.getLeftY(), -Constants.OIConstants.driverController.getLeftX())) - )); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - Constants.OIConstants.driverController.back().and(Constants.OIConstants.driverController.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - Constants.OIConstants.driverController.back().and(Constants.OIConstants.driverController.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - Constants.OIConstants.driverController.start().and(Constants.OIConstants.driverController.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - Constants.OIConstants.driverController.start().and(Constants.OIConstants.driverController.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - // reset the field-centric heading on left bumper press - Constants.OIConstants.driverController.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - - drivetrain.registerTelemetry(logger::telemeterize); - // drive.setDefaultCommand( - // DriveCommands.joystickDrive( - // drive, - // () -> Constants.OIConstants.driverController.getLeftY(), - // () -> Constants.OIConstants.driverController.getLeftX(), - // () -> -Constants.OIConstants.driverController.getRightX())); - - //Constants.OIConstants.driverController.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain + .applyRequest(() -> drive.withVelocityX(-Constants.OIConstants.driverController.getLeftY() * MaxSpeed) + .withVelocityY(-Constants.OIConstants.driverController.getLeftX() * MaxSpeed) + .withRotationalRate(-Constants.OIConstants.driverController.getRightX() * MaxAngularRate))); - OIConstants.operatorController.leftTrigger() - .whileTrue(Commands.startEnd(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 1.5), - () -> elevator.moveElevator(0), - elevator)); + // Constants.OIConstants.driverController.back().and(Constants.OIConstants.driverController.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + // Constants.OIConstants.driverController.back().and(Constants.OIConstants.driverController.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + // Constants.OIConstants.driverController.start().and(Constants.OIConstants.driverController.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + // Constants.OIConstants.driverController.start().and(Constants.OIConstants.driverController.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - OIConstants.operatorController.rightTrigger() - .whileTrue(Commands.startEnd(() -> elevator.moveElevator(-OIConstants.operatorController.getRightTriggerAxis() / 1.5), - () -> elevator.moveElevator(0), - elevator)); + Constants.OIConstants.driverController.a().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - // OIConstants.operatorController.rightBumper().whileTrue( - // Commands.run( - // () -> elevator.setVoltage(1), - // elevator - // ) - // ); + drivetrain.registerTelemetry(logger::telemeterize); - // OIConstants.operatorController.leftBumper().whileTrue( - // Commands.run( - // () -> elevator.setVoltage(-1), - // elevator - // ) - // ); + OIConstants.operatorController.leftTrigger() + .whileTrue( + Commands.startEnd(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 1.5), + () -> elevator.moveElevator(0), + elevator)); + OIConstants.operatorController.rightTrigger() + .whileTrue(Commands.startEnd( + () -> elevator.moveElevator(-OIConstants.operatorController.getRightTriggerAxis() / 1.5), + () -> elevator.moveElevator(0), + elevator)); // OIConstants.operatorController.leftBumper().whileTrue( - // Arm.getInstance().sysIDRoutine().quasistatic(SysIdRoutine.Direction.kForward) + // Arm.getInstance().sysIDRoutine().quasistatic(SysIdRoutine.Direction.kForward) // ); // OIConstants.operatorController.rightBumper().whileTrue( - // Arm.getInstance().sysIDRoutine().quasistatic(SysIdRoutine.Direction.kReverse) + // Arm.getInstance().sysIDRoutine().quasistatic(SysIdRoutine.Direction.kReverse) // ); // OIConstants.operatorController.leftTrigger().whileTrue( - // Arm.getInstance().sysIDRoutine().dynamic(SysIdRoutine.Direction.kForward) + // Arm.getInstance().sysIDRoutine().dynamic(SysIdRoutine.Direction.kForward) // ); // OIConstants.operatorController.rightTrigger().whileTrue( - // Arm.getInstance().sysIDRoutine().dynamic(SysIdRoutine.Direction.kReverse) + // Arm.getInstance().sysIDRoutine().dynamic(SysIdRoutine.Direction.kReverse) // ); - Constants.OIConstants.operatorController.povRight().onTrue( - Commands.sequence( - Commands.parallel( - Commands.runOnce(() -> Arm.getInstance().setGoal(1.02), Arm.getInstance()), - Elevator.getInstance().setGoal(0.57) - ) - ) - ); + Commands.sequence( + Commands.parallel( + Commands.runOnce(() -> Arm.getInstance().setGoal(1.02), Arm.getInstance()), + Elevator.getInstance().setGoal(0.57)))); Constants.OIConstants.operatorController.povLeft().onTrue( - Commands.sequence( - Elevator.getInstance().setGoal(ElevatorConstants.minDistance), - Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8), Arm.getInstance()), - Commands.waitSeconds(1.5) - ) - ); - - - Constants.OIConstants.operatorController.povUp() - .whileTrue( Commands.sequence( - intake.intake().until(() -> intake.GamePieceInitial()), - intake.intakeSlow().until(() -> intake.GamePieceFinal()), - Commands.run(() -> intake.setIntakeSpeed(0)) - ) - ); - - Constants.OIConstants.operatorController.x().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(-1), - () -> intake.setIntakeSpeed(0), intake)); - - Constants.OIConstants.operatorController.b().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(1), - () -> intake.setIntakeSpeed(0), intake)); - - Constants.OIConstants.operatorController.y().onTrue( - elevator.setGoal(ElevatorConstants.maxDistance/2) - ); - - Constants.OIConstants.operatorController.a().onTrue( - elevator.setGoal(0) - ); - - // OIConstants.driverController.povDown().onTrue((Arm.getInstance().setGoal(0))); - // OIConstants.driverController.povRight().onTrue((Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); - OIConstants.driverController.povUp().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L3))); + Elevator.getInstance().setGoal(ElevatorConstants.minDistance), + Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8), Arm.getInstance()), + Commands.waitSeconds(1.5))); + + // Constants.OIConstants.operatorController.povUp() + // .whileTrue( + // Commands.sequence( + // intake.intake().until(() -> intake.GamePieceInitial()), + // intake.intakeSlow().until(() -> intake.GamePieceFinal()), + // Commands.run(() -> intake.setIntakeSpeed(0)) + // ) + // ); + Constants.OIConstants.operatorController.x().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(-1), + () -> intake.setIntakeSpeed(0), intake)); - // OIConstants.operatorController.povDown().onTrue((elevator.setGoal(0.05))); - // OIConstants.operatorController.povRight().onTrue((elevator.setGoal(0.4))); - // OIConstants.operatorController.povUp().onTrue((elevator.setGoal(0.69))); + Constants.OIConstants.operatorController.b().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(1), + () -> intake.setIntakeSpeed(0), intake)); - OIConstants.driverController.leftBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8))); - OIConstants.driverController.rightBumper().onTrue(Commands.runOnce(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); + // OIConstants.driverController.povUp().onTrue(Commands.runOnce(() -> + // Arm.getInstance().setGoal(ArmConstants.CORAL_L3))); + // OIConstants.driverController.leftBumper().onTrue(Commands.runOnce(() -> + // Arm.getInstance().setGoal(-1.8))); + // OIConstants.driverController.rightBumper().onTrue(Commands.runOnce(() -> + // Arm.getInstance().setGoal(ArmConstants.CORAL_L2))); } -} public Command getAutonomousCommand() { - return Commands.print("hi"); + return Commands.runOnce(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L4), Arm.getInstance()); } }