diff --git a/src/main/java/frc/robot/commands/arm/ArmToPosition.java b/src/main/java/frc/robot/commands/arm/ArmToPosition.java index b5f5a895..6fb0ec9f 100644 --- a/src/main/java/frc/robot/commands/arm/ArmToPosition.java +++ b/src/main/java/frc/robot/commands/arm/ArmToPosition.java @@ -1,5 +1,6 @@ package frc.robot.commands.arm; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -11,7 +12,7 @@ public class ArmToPosition extends Command { Arm arm; DoubleSupplier positionDegrees; double targetPositionDegrees; - double timeMS; + Timer timer = new Timer(); public ArmToPosition(Arm arm, DoubleSupplier positionDegrees) { this.arm = arm; @@ -23,28 +24,24 @@ public ArmToPosition(Arm arm, DoubleSupplier positionDegrees) { @Override public void initialize() { targetPositionDegrees = positionDegrees.getAsDouble(); - timeMS = 0.0; + timer.restart(); if (Constants.debugCommands) { System.out.println( "START: " + this.getClass().getSimpleName() + " angle: " + targetPositionDegrees); } - } - @Override - public void execute() { arm.setAngle(targetPositionDegrees); } @Override public boolean isFinished() { if (Math.abs(arm.getAngle() - targetPositionDegrees) <= ArmConstants.pidAngleErrorInDegrees) { - timeMS += 20.0; - if (timeMS >= ArmConstants.pidSettlingTimeInMilliseconds) { + if (timer.get() >= ArmConstants.pidSettlingTimeInSeconds) { return true; } } else { - timeMS = 0.0; + timer.reset(); } return false; } diff --git a/src/main/java/frc/robot/commands/arm/ArmToPositionTP.java b/src/main/java/frc/robot/commands/arm/ArmToPositionTP.java deleted file mode 100644 index dc9d4962..00000000 --- a/src/main/java/frc/robot/commands/arm/ArmToPositionTP.java +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.arm; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.Constants; -import frc.robot.config.RobotConfig; -import frc.robot.subsystems.arm.Arm; -import java.util.function.DoubleSupplier; - -public class ArmToPositionTP extends Command { - private final Arm arm; - private TrapezoidProfile motionProfile; - private TrapezoidProfile.State initial; - private TrapezoidProfile.State current; - private TrapezoidProfile.State goal; - private DoubleSupplier positionDegrees; - private final Timer timer = new Timer(); - - /** Creates a new ArmToPositionTP. */ - public ArmToPositionTP(Arm arm, DoubleSupplier positionDegrees) { - this.arm = arm; - this.positionDegrees = positionDegrees; - - addRequirements((Subsystem) arm); - } - - @Override - public void initialize() { - motionProfile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints( - RobotConfig.ArmConstants.maxVelocityInDegreesPerSecond, - RobotConfig.ArmConstants.maxAccelerationInDegreesPerSecondSquared)); - - initial = new TrapezoidProfile.State(arm.getAngle(), arm.getVelocity()); - current = initial; - - goal = - new TrapezoidProfile.State( - MathUtil.clamp( - positionDegrees.getAsDouble(), - RobotConfig.ArmConstants.minAngleInDegrees, - RobotConfig.ArmConstants.maxAngleInDegrees), - 0); - timer.restart(); - - if (Constants.debugCommands) { - System.out.println( - "START: " - + this.getClass().getSimpleName() - + " angle: " - + initial.position - + " --> " - + goal.position); - } - } - - @Override - public void execute() { - current = motionProfile.calculate(timer.get(), initial, goal); - - // System.out.println(current.position); - arm.setAngle(current.position, current.velocity); - } - - @Override - public void end(boolean interrupted) { - if (interrupted) { - System.err.println("INTERRUPTED: " + this.getClass().getSimpleName()); - } - - if (Constants.debugCommands) { - System.out.println(" END: " + this.getClass().getSimpleName()); - } - } - - @Override - public boolean isFinished() { - return timer.hasElapsed(motionProfile.totalTime()); - } -} diff --git a/src/main/java/frc/robot/commands/drive/DriveToYaw.java b/src/main/java/frc/robot/commands/drive/DriveToYaw.java index 6f4ed2b2..a2302b0a 100644 --- a/src/main/java/frc/robot/commands/drive/DriveToYaw.java +++ b/src/main/java/frc/robot/commands/drive/DriveToYaw.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.Constants; @@ -16,7 +17,7 @@ public class DriveToYaw extends Command { PIDController turnPID = new PIDController( DriveConstants.rotatePidKp, DriveConstants.rotatePidKi, DriveConstants.rotatePidKd); - double timeMS; + Timer timer = new Timer(); public DriveToYaw(Drive drive, DoubleSupplier yawDegrees) { this.drive = drive; @@ -32,7 +33,7 @@ public void initialize() { targetYaw = this.yawDegrees.getAsDouble(); turnPID.reset(); turnPID.setSetpoint(targetYaw); - timeMS = 0.0; + timer.reset(); if (Constants.debugCommands) { System.out.println( "START: " @@ -54,12 +55,11 @@ public void execute() { @Override public boolean isFinished() { if (turnPID.atSetpoint()) { - timeMS += 20.0; - if (timeMS >= DriveConstants.pidSettlingTimeInMilliseconds) { + if (timer.get() >= DriveConstants.pidSettlingTimeInSeconds) { return true; } } else { - timeMS = 0.0; + timer.reset(); } return false; } diff --git a/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java b/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java index 6ecbf0f0..0f4f3a0b 100644 --- a/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java +++ b/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java @@ -1,6 +1,7 @@ package frc.robot.commands.shooter; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -12,7 +13,7 @@ public class SetShooterVelocity extends Command { Shooter shooter; DoubleSupplier velocityRPM; double targetVelocityRPM; - double timeMS; + Timer timer = new Timer(); public SetShooterVelocity(Shooter shooter, DoubleSupplier velocityRPM) { this.shooter = shooter; @@ -26,7 +27,7 @@ public void initialize() { System.out.println( "START: " + this.getClass().getSimpleName() + " velocity: " + velocityRPM.getAsDouble()); } - timeMS = 0.0; + timer.restart(); targetVelocityRPM = velocityRPM.getAsDouble(); shooter.runVelocity(targetVelocityRPM); } @@ -37,12 +38,11 @@ public boolean isFinished() { Units.radiansPerSecondToRotationsPerMinute(shooter.getCurrentSpeed()) - targetVelocityRPM) <= ShooterConstants.pidVelocityErrorInRPMS) { - timeMS += 20.0; - if (timeMS >= ShooterConstants.pidSettlingTimeInMilliseconds) { + if (timer.get() >= ShooterConstants.pidSettlingTimeInSeconds) { return true; } } else { - timeMS = 0.0; + timer.reset(); } return false; diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index bcabaaad..8f60cd33 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -48,7 +48,7 @@ public static class DriveConstants { public static double rotatePidKd = 0.0; public static double rotatePidErrorInDegrees = 2.0; public static double pidTimeoutInSeconds = 0.5; - public static double pidSettlingTimeInMilliseconds = 0.1; + public static double pidSettlingTimeInSeconds = 0.1; public static double slewRateLimiterX = 3; public static double slewRateLimiterY = 3; @@ -62,25 +62,26 @@ public static class ArmConstants { public static double absolutePositionOffset = 0; /* 0-1 */ public static double absoluteEncoderInversion = 1; /* 1 for none, -1 to invert */ - public static double pidKp = 0.1; + public static double pidKp = 0.12935; public static double pidKi = 0.0; - public static double pidKd = 0.0; + public static double pidKd = 0.0060174; public static double pidMaxOutput = 0.4; public static double pidMinOutput = -0.4; - public static double ffKs = 0.0; - public static double ffKv = 0.0; - public static double ffKa = 0.0; - public static double ffKg = 0.1; + public static double ffKs = 0.03402; + public static double ffKv = 0.072343; + public static double ffKa = 0.00036498; + public static double ffKg = 0.88138; public static double pidAngleErrorInDegrees = 2.0; - public static double pidSettlingTimeInMilliseconds = 0.1; + public static double pidSettlingTimeInSeconds = 0.0; public static double pidTimeoutInSeconds = 3.0; public static double maxAngleInDegrees = 90.0; public static double minAngleInDegrees = 0.0; public static double maxVelocityInDegreesPerSecond = 90; - public static double maxAccelerationInDegreesPerSecondSquared = 720; + public static double maxAccelerationInDegreesPerSecondSquared = + maxVelocityInDegreesPerSecond * 16; public static double intakeAngleInDegrees = 1; public static double ejectAngleInDegrees = 15; @@ -105,7 +106,7 @@ public static class ShooterConstants { /* PID */ public static double pidVelocityErrorInRPMS = 300; - public static double pidSettlingTimeInMilliseconds = 0.1; + public static double pidSettlingTimeInSeconds = 0.0; public static double pidTimeoutInSeconds = 2.0; public static double pidKp = 0.043566; public static double pidKi = 0.0; diff --git a/src/main/java/frc/robot/config/RobotConfigInferno.java b/src/main/java/frc/robot/config/RobotConfigInferno.java index 46ce775c..a54e93c2 100644 --- a/src/main/java/frc/robot/config/RobotConfigInferno.java +++ b/src/main/java/frc/robot/config/RobotConfigInferno.java @@ -36,7 +36,7 @@ public RobotConfigInferno() { DriveConstants.rotatePidKd = 0.001; DriveConstants.rotatePidErrorInDegrees = 0.5; DriveConstants.pidTimeoutInSeconds = 2; - DriveConstants.pidSettlingTimeInMilliseconds = 0.5; + DriveConstants.pidSettlingTimeInSeconds = 0.5; drive = new DriveSwerveYAGSL("yagsl/inferno"); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index f60b56a9..b2954da8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -46,7 +46,6 @@ public ShooterSubsystem(ShooterIO io) { Units.rotationsPerMinuteToRadiansPerSecond(ShooterConstants.maxVelocityInRPMs), Units.rotationsPerMinuteToRadiansPerSecond( ShooterConstants.maxAccelerationInRPMsSquared)))); - setGoal(0); this.io = io; useSoftwarePidVelocityControl = !io.supportsHardwarePid(); @@ -103,25 +102,21 @@ public double getMeasurement() { @Override // Sets the voltage to volts. the volts value is -12 to 12 public void runVoltage(double volts) { - if (targetVoltage != volts) { - targetVoltage = volts; - targetVelocityRadPerSec = 0; - this.targetVelocityRPM = ShooterConstants.maxVelocityInRPMs * (volts / 12.0); - disable(); // disable PID control - io.setVoltage(targetVoltage); - } + targetVoltage = volts; + targetVelocityRadPerSec = 0; + this.targetVelocityRPM = ShooterConstants.maxVelocityInRPMs * (volts / 12.0); + disable(); // disable PID control + io.setVoltage(targetVoltage); } @Override public void runVelocity(double velocityRPM) { velocityRPM = MathUtil.clamp(velocityRPM, 0, ShooterConstants.maxVelocityInRPMs); - if (this.targetVelocityRPM != velocityRPM) { - targetVoltage = -1; - this.targetVelocityRPM = velocityRPM; - targetVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - setGoal(targetVelocityRadPerSec); - enable(); - } + targetVoltage = -1; + this.targetVelocityRPM = velocityRPM; + targetVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); + setGoal(targetVelocityRadPerSec); + enable(); } @Override