diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat index 4efbb02..82eade7 100644 Binary files a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat and b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat differ diff --git a/logs/Log_24-10-10_16-16-33.wpilog b/logs/Log_24-10-10_16-16-33.wpilog new file mode 100644 index 0000000..0027a00 Binary files /dev/null and b/logs/Log_24-10-10_16-16-33.wpilog differ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a82210e..a390c3f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,7 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final Mode currentMode = Mode.REAL; + public static final Mode currentMode = Mode.SIM; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b9fb89c..5580f85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,6 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -103,8 +102,7 @@ public RobotContainer() { break; } - - //This can go away + // This can go away // Set up auto routines // NamedCommands.registerCommand( @@ -154,7 +152,9 @@ private void configureButtonBindings() { drive) .ignoringDisable(true)); - controller.y().onTrue(Commands.runEnd(() -> pivot.setGoal(45), () -> pivot.setGoal(0))); + controller + .y() + .whileTrue(Commands.runEnd(() -> pivot.setGoal(Math.PI / 4), () -> pivot.setGoal(0), pivot)); } /** diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java index 4fae70f..866ed0c 100644 --- a/src/main/java/frc/robot/subsystems/pivot/Pivot.java +++ b/src/main/java/frc/robot/subsystems/pivot/Pivot.java @@ -39,7 +39,7 @@ public static Pivot getInstance() { } public static Pivot initialize(PivotIO io) { - if (Pivot.pivotInstance == null) { + if (Pivot.pivotInstance == null) { pivotInstance = new Pivot(io); } return pivotInstance; @@ -48,6 +48,7 @@ public static Pivot initialize(PivotIO io) { @Override public void periodic() { io.updateInputs(inputs); + io.setVoltage(inputs.appliedVolts); Logger.processInputs("Pivot", inputs); } @@ -56,7 +57,7 @@ public void runVoltage(double volts) { } public void setGoal(double goal) { - goalRad = goal; + io.setAngle(goal); } public void stop() { diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java index 8e0f346..c46129f 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java @@ -8,7 +8,7 @@ public interface PivotIO { public static class PivotIOInputs { public double positionRad = 0.0; public double appliedVolts = 0.0; - public double velocityRevolutionsPerSec = 0.0; + public double velocityRadPerSec = 0.0; public double[] currentAmps = new double[] {}; } diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index 70d0a7f..319d6e6 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -40,7 +40,7 @@ public PivotIOReal() { @Override public void updateInputs(PivotIOInputs inputs) { inputs.positionRad = pivotEncoderRight.getPosition() * 2 * Math.PI; - inputs.velocityRevolutionsPerSec = pivotEncoderRight.getVelocity(); + inputs.velocityRadPerSec = pivotEncoderRight.getVelocity() * 2 * Math.PI / 60; inputs.appliedVolts = appliedVolts; if (closedLoop) { diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java index 6133641..bbf0015 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java @@ -7,19 +7,23 @@ public class PivotIOSim implements PivotIO { private SingleJointedArmSim sim = - new SingleJointedArmSim(DCMotor.getNEO(1), 1.5, 0.004, 0, 0, 0, false, 0); - private PIDController pid = new PIDController(1.0, 0.0, 0.0); + new SingleJointedArmSim(DCMotor.getNEO(1), 1.5, 0.004, 0.2, 0, Math.PI, false, 0); + private PIDController pid = new PIDController(0.2, 0.0, 0.0); - private boolean closedLoop = false; + private boolean closedLoop = true; private double ffVolts = 0.0; private double appliedVolts = 0.0; @Override public void updateInputs(PivotIOInputs inputs) { - if (closedLoop) { - appliedVolts = MathUtil.clamp(pid.calculate(sim.getAngleRads()) + ffVolts, -12.0, 12.0); - sim.setInputVoltage(appliedVolts); - } + sim.update(0.02); + + appliedVolts = MathUtil.clamp(pid.calculate(sim.getAngleRads()) + ffVolts, -12.0, 12.0); + sim.setInputVoltage(appliedVolts); + + inputs.positionRad = sim.getAngleRads(); + inputs.appliedVolts = appliedVolts; + inputs.velocityRadPerSec = sim.getVelocityRadPerSec(); } @Override