diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1c8a78c..8481188 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,6 +34,7 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ + public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; @@ -155,6 +156,8 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + + robotContainer.arm.setPosition(5); } /** This function is called periodically during operator control. */ @@ -185,4 +188,5 @@ public static boolean isBlue() { // defaults to blue when unknown return !DriverStation.getAlliance().isPresent() || DriverStation.getAlliance().get() != DriverStation.Alliance.Red; } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ddb9495..5e3ea42 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,11 +1,12 @@ package frc.robot; -import static frc.robot.constants.Constants.tuningMode; import static frc.robot.constants.Constants.OperatorConstants.*; import frc.robot.constants.Constants; import frc.robot.constants.Constants.OperatorConstants; import frc.robot.constants.Constants.ShuffleboardConstants; +import frc.robot.constants.IntakeConstants; +import frc.robot.constants.ShooterConstants; import frc.robot.io.GyroIO; import frc.robot.io.GyroIOPigeon2; import frc.robot.io.GyroIOSim; @@ -14,16 +15,23 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.SystemAlerter; +import frc.robot.subsystems.Vision; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import frc.robot.commands.arm.ManualArmControl; +import frc.robot.commands.arm.SetArmPosition; +import frc.robot.commands.intake.IntakeWithArm; import frc.robot.commands.intake.SetIntakeSpeed; +import frc.robot.commands.shooter.SetShooterVelocity; import frc.robot.commands.shooter.StopShooter; import frc.robot.commands.swerve.SwerveAngleOffsetCalibration; import frc.robot.commands.swerve.TeleopSwerve; import frc.robot.commands.swerve.XStance; +import frc.robot.commands.vision.GamePieceVision; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Relay.Value; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.SendableCameraWrapper; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -73,10 +81,10 @@ public static RobotContainer getInstance() { : new GyroIOSim(); /* subsystems */ - // public final Vision vision = new Vision(); + public final Vision vision = new Vision(); public final Arm arm = new Arm(); public final Intake intake = new Intake(); - public final Swerve swerve = new Swerve(gyro/* , vision */); + public final Swerve swerve = new Swerve(gyro, vision); public final Shooter shooter = new Shooter(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -122,6 +130,8 @@ public RobotContainer() { autoChooser.addOption("sysid swerve drive", sysIdTestSet(swerve.driveSysIdRoutine)); autoChooser.addOption("sysid swerve angle", sysIdTestSet(swerve.angleSysIdRoutine)); autoChooser.addOption("sysid shooter", sysIdTestSet(shooter.sysIdRoutine)); + + vision.setPriorityId(7); } // shouldn't require tuningMode as we might run it randomly in comp @@ -139,32 +149,55 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { + // *DRIVER CONTROLS* + // var teleopSwerve = new TeleopSwerve( () -> -driverController.getLeftY(), () -> -driverController.getLeftX(), () -> -driverController.getRightX(), - driverController.getHID()::getLeftBumper, // override speed - () -> driverController.getLeftTriggerAxis() > OperatorConstants.boolTriggerThreshold, // preceise mode + () -> false, // no overdrive functionality + // driverController.getHID()::getLeftBumper, // override speed + () -> driverController.getLeftTriggerAxis() > OperatorConstants.boolTriggerThreshold, // precise mode swerve); swerve.setDefaultCommand(teleopSwerve); driverController.rightTrigger(OperatorConstants.boolTriggerThreshold) .whileTrue(teleopSwerve.holdToggleFieldRelative()); - // driverController.rightBumper() - // .whileTrue(teleopSwerve.holdRotateAroundPiece()); - + // right bumper -> rotate to speaker apriltag + // driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve)); + // left bumper -> rotate to note + driverController.leftBumper().whileTrue(new GamePieceVision(vision, swerve)); driverController.a().onTrue(teleopSwerve.toggleFieldRelative()); + // b -> trap/climb align maybe? driverController.x().whileTrue(new XStance(swerve)); driverController.y().onTrue(teleopSwerve.zeroYaw()); - // operatorController.y().onTrue(new SetShooterVelocity(shooter, shooterVelocity)); + // dpad left -> call for coopertition (lights) + // dpad right -> call for amplify (lights) + + // *OPERATOR CONTROLS* + // + operatorController.a() + .onTrue(Commands.parallel(new SetArmPosition(arm, 84), new SetShooterVelocity(shooter, 1750))); + operatorController.b().onTrue(Commands.parallel(new SetArmPosition(arm, -1.5), + new SetShooterVelocity(shooter, ShooterConstants.subwooferVelocity_rpm))); operatorController.x().onTrue(new StopShooter(shooter)); - operatorController.rightBumper().whileTrue(new SetIntakeSpeed(intake, false)); + // y -> set shooter speed and arm angle based on limelight + operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4)); + operatorController.rightBumper().onTrue(new SetArmPosition(arm, 84)); + operatorController.leftTrigger(boolTriggerThreshold).whileTrue(new IntakeWithArm(intake, arm)); operatorController.rightTrigger(boolTriggerThreshold).whileTrue(new SetIntakeSpeed(intake, true)); + // dpad up -> climber up + // dpad down -> climber down + + operatorController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true)); + operatorController.rightStick().whileTrue(new ManualArmControl(arm, operatorController::getLeftY)); + // *TESTING CONTROLS* + // // When in tuning mode, create multiple testing options on shuffleboard as well as bind commands to a unique // 'testing' controller - if (tuningMode) { + if (Constants.tuningMode) { var indexSpeedEntry = Shuffleboard.getTab("Test").add("Index Speed", 0) .withWidget(BuiltInWidgets.kNumberSlider) .withProperties(Map.of("min", -1, "max", 1)).getEntry(); @@ -175,19 +208,30 @@ private void configureBindings() { .withWidget(BuiltInWidgets.kNumberSlider) .withProperties(Map.of("min", -1, "max", 1)).getEntry(); var shooterVelocityEntry = Shuffleboard.getTab("Test").add("Shooter Velocity", 0).getEntry(); + var armPositionEntry = Shuffleboard.getTab("Test").add("Arm Position (degrees)", 0).getEntry(); testController.x().onTrue(new StopShooter(shooter)); + testController.y() + .onTrue(Commands.runOnce(() -> arm.setPosition(armPositionEntry.getDouble(boolTriggerThreshold)), arm)); testController.b() .onTrue(new InstantCommand(() -> shooter.setVelocity(shooterVelocityEntry.getDouble(0)), shooter)); testController.a() .onTrue(new InstantCommand(() -> shooter.setPercentOutput(shooterSpeedEntry.getDouble(0)), shooter)); + testController.rightBumper() .whileTrue(Commands - .runEnd(() -> intake.setMotorVoltage(intakeSpeedEntry.getDouble(0) * 12), () -> intake.stopMotor(), intake) + .runEnd(() -> intake.setMotorSpeed(intakeSpeedEntry.getDouble(0)), () -> intake.stopMotor(), intake) .until(() -> intake.inputs.noteSensor).unless(() -> intake.inputs.noteSensor)); testController.leftBumper().whileTrue( - new StartEndCommand(() -> intake.setMotorVoltage(indexSpeedEntry.getDouble(0) * 12), () -> intake.stopMotor(), + new StartEndCommand(() -> intake.setMotorSpeed(indexSpeedEntry.getDouble(0)), () -> intake.stopMotor(), intake)); + + testController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true)); + testController.rightStick().whileTrue(new ManualArmControl(arm, testController::getLeftY)); + + testController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm)); + testController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm)); + testController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); } } diff --git a/src/main/java/frc/robot/commands/arm/AutoAim.java b/src/main/java/frc/robot/commands/arm/AutoAim.java new file mode 100644 index 0000000..09241b6 --- /dev/null +++ b/src/main/java/frc/robot/commands/arm/AutoAim.java @@ -0,0 +1,56 @@ +// 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 frc.robot.constants.VisionConstants; +import frc.robot.io.FieldVisionIOInputsAutoLogged; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Vision; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.Optional; + +public class AutoAim extends Command { + private Arm arm; + private Vision vision; // needed just to set the priority tag to stare at + private FieldVisionIOInputsAutoLogged fieldvisionio; // the data we take from + Optional alliance = DriverStation.getAlliance(); + + /** Creates a new AutoAim. */ + public AutoAim(Arm arm, Vision vision) { + this.arm = arm; + this.vision = vision; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if (alliance.get() == Alliance.Blue) { + vision.setPriorityId(VisionConstants.blueSpeakerTag); + } + if (alliance.get() == Alliance.Red) { + vision.setPriorityId(VisionConstants.redSpeakerTag); + } + } + + @Override + public void execute() { + if (vision.findDistance() > 1) + ; + // add if statements for the range now + } + + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/arm/ManualArmControl.java b/src/main/java/frc/robot/commands/arm/ManualArmControl.java new file mode 100644 index 0000000..138f362 --- /dev/null +++ b/src/main/java/frc/robot/commands/arm/ManualArmControl.java @@ -0,0 +1,53 @@ +package frc.robot.commands.arm; + +import frc.robot.constants.ArmConstants; +import frc.robot.subsystems.Arm; +import frc.robot.util.ControllerUtil; + +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.function.DoubleSupplier; + +public class ManualArmControl extends Command { + public DoubleSupplier speedSupplier; + public Arm arm; + + /** + * Constructs a ManualArmControl command, using the provided function to supply speed numbers + * @param arm Arm subsystem to be passed in + * @param speedSupplier function that supplies the speed used to run the arm + */ + public ManualArmControl(Arm arm, DoubleSupplier speedSupplier) { + this.arm = arm; + this.speedSupplier = speedSupplier; + addRequirements(arm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // apply deadband to input. Run stationary feedforward ("stopped") if banded input is 0 + double deadbandInput = ControllerUtil.scaleAxis(speedSupplier.getAsDouble()); + if (deadbandInput == 0) { + arm.stop(); + } else { // else, scale banded input by max manual output and then convert to volts (linearly with max of 12 V) + arm.setMotorVoltage(-(ArmConstants.manualMaxOutput * deadbandInput) * 12); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + arm.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/arm/setArmPosition.java b/src/main/java/frc/robot/commands/arm/SetArmPosition.java similarity index 52% rename from src/main/java/frc/robot/commands/arm/setArmPosition.java rename to src/main/java/frc/robot/commands/arm/SetArmPosition.java index a1c1b21..b36c87a 100644 --- a/src/main/java/frc/robot/commands/arm/setArmPosition.java +++ b/src/main/java/frc/robot/commands/arm/SetArmPosition.java @@ -4,18 +4,24 @@ package frc.robot.commands.arm; +import frc.robot.constants.ArmConstants; import frc.robot.subsystems.Arm; import edu.wpi.first.wpilibj2.command.Command; -public class setArmPosition extends Command { +public class SetArmPosition extends Command { private Arm arm; protected double targetPosition_rad; // did I do this right? - /** Creates a new setArmPosition. */ - public setArmPosition(Arm arm, double targetPosition_rad) { + /** + * Command to set the arm position, cancels the command and moves on + * after being within the range given by {@link ArmConstants#accuracyTolerance_deg accuracyTolerance} + * @param arm Arm subsystem + * @param targetPosition_deg Position in degrees to set the arm to (0 is horizontal to robot base) + */ + public SetArmPosition(Arm arm, double targetPosition_deg) { this.arm = arm; - this.targetPosition_rad = targetPosition_rad; + this.targetPosition_rad = targetPosition_deg; addRequirements(arm); } @@ -29,7 +35,10 @@ public void execute() {} @Override public void end(boolean interrupted) { - arm.stopArm(); + // only stop if interrupted. Otherwise, continue adjusting arm position just move on to another command + if (interrupted) { + arm.stop(); + } } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java new file mode 100644 index 0000000..ddae6b0 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -0,0 +1,44 @@ +package frc.robot.commands.intake; + +import frc.robot.constants.IntakeConstants; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.Command; + +public class IntakeWithArm extends Command { + private Intake intake; + private Arm arm; + + public IntakeWithArm(Intake intake, Arm arm) { + this.intake = intake; + this.arm = arm; + addRequirements(intake, arm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if (!intake.inputs.noteSensor) { + arm.setPosition(-1.5); + intake.setMotorSpeed(IntakeConstants.intakeSpeed); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + arm.setPosition(4); + intake.stopMotor(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return intake.inputs.noteSensor; + } +} diff --git a/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java b/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java index ac64a9c..547faf9 100644 --- a/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java +++ b/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java @@ -41,8 +41,9 @@ public SetIntakeSpeed(Intake intake, boolean isIndexing) { public void initialize() { // Run motor only if we are either indexing or don't detect a note. Or, if we are intaking (not indexing) and we // detect a note, don't run - if (isIndexing || !intake.detectNote()) - intake.setMotorSpeed(speed); + if (isIndexing || !intake.isHoldingNote()) + ; + intake.setMotorSpeed(speed); } // Called every time the scheduler runs while the command is scheduled. @@ -60,6 +61,6 @@ public void end(boolean interrupted) { public boolean isFinished() { // If we are intaking and we detect a note, cancel automatically. If we are indexing, the command will only be // canceled on interrupt - return (!isIndexing && intake.detectNote()); + return (!isIndexing && intake.inputs.noteSensor); } } diff --git a/src/main/java/frc/robot/commands/swerve/TeleopSwerve.java b/src/main/java/frc/robot/commands/swerve/TeleopSwerve.java index d588c3e..017089d 100644 --- a/src/main/java/frc/robot/commands/swerve/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/swerve/TeleopSwerve.java @@ -1,12 +1,11 @@ package frc.robot.commands.swerve; -import frc.robot.constants.Constants.OperatorConstants; import frc.robot.constants.SwerveConstants; import frc.robot.subsystems.Swerve; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import frc.robot.util.ControllerUtil; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; @@ -121,13 +120,13 @@ public void execute() { driveMax *= SwerveConstants.maxVelocity_mps; // turn percent into velocity translation = new Translation2d( - driveLimiter.calculate(driveMax * modifyAxis(driveSupplier.getAsDouble())), - strafeLimiter.calculate(driveMax * modifyAxis(strafeSupplier.getAsDouble()))); + driveLimiter.calculate(driveMax * ControllerUtil.scaleAxis(driveSupplier.getAsDouble())), + strafeLimiter.calculate(driveMax * ControllerUtil.scaleAxis(strafeSupplier.getAsDouble()))); Logger.recordOutput("TeleopSwerve/translationX_mps", translation.getX()); Logger.recordOutput("TeleopSwerve/translationY_mps", translation.getY()); - rotation = modifyAxis(turnSupplier.getAsDouble()) + rotation = ControllerUtil.scaleAxis(turnSupplier.getAsDouble()) * SwerveConstants.maxAngularVelocity_radps * turnMaxPercent; @@ -140,21 +139,4 @@ public void execute() { public void end(boolean interrupted) { swerve.stop(); } - - // private static final double cubicWeight = 0.3; - // private static final double weightExponent = 6.5; - // private static final double minOutput = 0.1; - - // custom input scaling - // @see https://desmos.com/calculator/7wy4gmgdpv - private static double modifyAxis(double value) { - value = MathUtil.applyDeadband(value, OperatorConstants.xboxStickDeadband); - return Math.copySign(value * value, value); - - // double absValue = Math.abs(value); - // return (absValue < OperatorConstants.xboxStickDeadband) ? 0 - // : Math.copySign( - // (cubicWeight * Math.pow(absValue, weightExponent) + (1 - cubicWeight) * absValue + minOutput) / (1 + minOutput), - // value); - } } diff --git a/src/main/java/frc/robot/commands/vision/AprilTagVision.java b/src/main/java/frc/robot/commands/vision/AprilTagVision.java new file mode 100644 index 0000000..7a4c440 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/AprilTagVision.java @@ -0,0 +1,86 @@ +// 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.vision; + +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.Vision; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; + +import org.littletonrobotics.junction.Logger; + +import java.util.Optional; + +public class AprilTagVision extends Command { + /** Creates a new AprilTagVision. */ + private Vision vision; + private Swerve swerve; + private boolean isAligned; + private double robotRotate_radps; + Optional alliance = DriverStation.getAlliance(); + public double rotationalOffset = Math.atan2(VisionConstants.limelightXOffset, vision.findDistance()); // inverse + // tangent + // of + // X/y + + public AprilTagVision(Vision vision, Swerve swerve) { + addRequirements(swerve); + this.vision = vision; + this.swerve = swerve; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if (alliance.get() == Alliance.Blue) { + vision.setPriorityId(VisionConstants.blueSpeakerTag); + } + if (alliance.get() == Alliance.Red) { + vision.setPriorityId(VisionConstants.redSpeakerTag); + } + robotRotate_radps = 0; + isAligned = false; + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if ((vision.fieldInputs.targetAprilTag == VisionConstants.blueSpeakerTag) && (alliance.get() == Alliance.Blue) + || (vision.fieldInputs.targetAprilTag == VisionConstants.redSpeakerTag) && (alliance.get() == Alliance.Red)) { + isAligned = (vision.pieceInputs.crosshairToTargetErrorX_rad < VisionConstants.crosshairGamePieceBoundRotateX_rad) + && (vision.fieldInputs.crosshairToTargetErrorX_rad > -VisionConstants.crosshairGamePieceBoundRotateX_rad); + } + Logger.recordOutput("Fieldvision/isAligned", isAligned); + + if (vision.fieldInputs.crosshairToTargetErrorX_rad < -VisionConstants.crosshairGamePieceBoundRotateX_rad) { + robotRotate_radps = VisionConstants.AprilTagRotateKp + * vision.fieldInputs.crosshairToTargetErrorX_rad // why do we use Proportional limit * error - derivative + - VisionConstants.AprilTagRotateKd; + } else if (VisionConstants.crosshairGamePieceBoundRotateX_rad < vision.fieldInputs.crosshairToTargetErrorX_rad) { + robotRotate_radps = VisionConstants.AprilTagRotateKp + * vision.fieldInputs.crosshairToTargetErrorX_rad + + VisionConstants.AprilTagRotateKd; + } + robotRotate_radps *= -1; // Rotate opposite of error + + System.out.println("Swerve --> Shooting Speaker"); + swerve.drive(new Translation2d(0, 0), robotRotate_radps - rotationalOffset, false); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/vision/GamePieceVision.java b/src/main/java/frc/robot/commands/vision/GamePieceVision.java new file mode 100644 index 0000000..44a2b7d --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/GamePieceVision.java @@ -0,0 +1,82 @@ +// 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.vision; + +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.Vision; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; + +import org.littletonrobotics.junction.Logger; + +public class GamePieceVision extends Command { + private Vision vision; + private Swerve swerve; + private double robotRotate_radps; + private boolean isAligned; + private PIDController pidController = new PIDController(VisionConstants.gamePieceRotateKp, 0, + VisionConstants.gamePieceRotateKd); + + /** Creates a new GamePieceVision. */ + public GamePieceVision(Vision vision, Swerve swerve) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(swerve); + this.vision = vision; + this.swerve = swerve; + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + robotRotate_radps = 0; + isAligned = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + isAligned = (vision.pieceInputs.crosshairToTargetErrorX_rad < VisionConstants.crosshairGamePieceBoundRotateX_rad) + && (vision.pieceInputs.crosshairToTargetErrorX_rad > -VisionConstants.crosshairGamePieceBoundRotateX_rad); + Logger.recordOutput("GamePieceVision/isAligned", isAligned); + + // When we see a ground GamePiece, we will rotate to it + // if (vision.pieceInputs.crosshairToTargetErrorX_rad < -VisionConstants.crosshairGamePieceBoundRotateX_rad) { + // robotRotate_radps = VisionConstants.gamePieceRotateKp + // * vision.pieceInputs.crosshairToTargetErrorX_rad + // - VisionConstants.gamePieceRotateKd; + // } else if (VisionConstants.crosshairGamePieceBoundRotateX_rad < vision.pieceInputs.crosshairToTargetErrorX_rad) { + // robotRotate_radps = VisionConstants.gamePieceRotateKp + // * vision.pieceInputs.crosshairToTargetErrorX_rad + // + VisionConstants.gamePieceRotateKd; + // } + // robotRotate_radps *= -1; // Rotate opposite of error + if (!MathUtil.isNear(0, vision.pieceInputs.crosshairToTargetErrorX_rad, + VisionConstants.crosshairGamePieceBoundRotateX_rad)) { + robotRotate_radps = pidController.calculate(vision.pieceInputs.crosshairToTargetErrorX_rad, 0); + } else { + robotRotate_radps = 0; + } + // Generate a continuously updated rotation to GamePiece + System.out.println("Swerve --> GamePiece"); + Logger.recordOutput("Vision/targetRotation", robotRotate_radps); + swerve.drive(new Translation2d(0, 0), robotRotate_radps, false); // TELL DRIVE THAT THEY ARE ALWAYS ROBOT ORIENTED + // WHEN ALIGN TO NOTE + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index e432ae9..3e2c78e 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -1,27 +1,36 @@ package frc.robot.constants; public final class ArmConstants { - public static final int motorId = 2; // placeholder + public static final int leadMotorId = 29; + public static final int followerMotorId = 30; + + public static final double shaftEncoderOffset_deg = 4.22; // needs to be tuned // pid - public static final double kP = 0; + public static final double kP = .000095; public static final double kI = 0; - public static final double kD = 0; + public static final double kD = .00000165; + + public static final double maxOutput = .5; + public static final double minOutput = -maxOutput; - public static final double minOutput = -1; - public static final double maxOutput = 1; + public static final double manualMaxOutput = .4; // feedforward public static final double kS = 0; - public static final double kG = 0; + public static final double kG = .5; public static final double kV = 0; public static final double kA = 0; - public static final double verticalArmPos_Rad = 1.5708; + public static final double verticalArmPos_deg = 90; + + // smartmotion + public static double maxAccel_rpmps = 7200; + public static double maxVelocity_rpm = 3800; - // Conversion - public static final int gearRatio = 1; // 1 IS PLACEHOLDER + // Conversion (not needed) + // public static final int gearRatio = 1; // 1 IS PLACEHOLDER // closeEnough - public static final double closeVar = 0.1; + public static final double accuracyTolerance_deg = .6; } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 5a515d5..3a4e92a 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -4,6 +4,6 @@ public class IntakeConstants { public static final int motorId = 31; public static final int sensorId = 0; - public static final double intakeSpeed = 0.80; - public static final double indexSpeed = 0.50; + public static final double intakeSpeed = 0.95; + public static final double indexSpeed = 0.85; } diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java new file mode 100644 index 0000000..ed65fec --- /dev/null +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -0,0 +1,18 @@ +package frc.robot.constants; + +import java.util.HashMap; + +public final class ScoringConstants { + public record ScoringPosition(double armAngle_deg, double shooterSpeed_rpm) {}; + + public static final HashMap scoringMap = new HashMap<>(); + static { + scoringMap.put(1.445, new ScoringPosition(-1, 3200)); + scoringMap.put(1.718, new ScoringPosition(3, 3200)); + scoringMap.put(1.817, new ScoringPosition(5, 3250)); + scoringMap.put(2.048, new ScoringPosition(8, 3250)); + scoringMap.put(2.426, new ScoringPosition(9, 3250)); + scoringMap.put(2.874, new ScoringPosition(16, 3350)); + scoringMap.put(3.227, new ScoringPosition(19, 3450)); + } +} diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 355757a..749df28 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -1,14 +1,16 @@ package frc.robot.constants; public final class ShooterConstants { - public static final int leadMotorId = 30; - public static final int followerMotorId = 29; + public static final int leadMotorId = 32; + public static final int followerMotorId = 33; - public static final double kP = .0005627; - public static final double kD = 0; + public static final double kP = .00007018; + public static final double kD = .0000025; - public static final double kS = .16982; - public static final double kV = .12824; - public static final double kA = .01449; - public static final double shooterVelocity = 1166; + public static final double kS = 0.026257; + public static final double kV = .00224; + public static final double kA = .008631; + + public static final double maxVelocity_rpm = 5000; + public static final double subwooferVelocity_rpm = 3250; } diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index 29085ef..7e03242 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -7,10 +7,10 @@ public final class SwerveConstants { // FL, FR, BL, BR (matches AdvantageScope convention) public static final SwerveModuleConstants moduleConstants[] = { - new SwerveModuleConstants(21, 22, .73), + new SwerveModuleConstants(21, 22, .722), new SwerveModuleConstants(23, 24, 1.84), - new SwerveModuleConstants(25, 26, 3.68), - new SwerveModuleConstants(27, 28, 2.90) + new SwerveModuleConstants(25, 26, 3.648), + new SwerveModuleConstants(27, 28, 2.917) }; // the left-to-right distance between the drivetrain wheels, should be measured from center to center diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java new file mode 100644 index 0000000..ba22c6f --- /dev/null +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -0,0 +1,25 @@ +package frc.robot.constants; + +public class VisionConstants { + /* AprilTagVision */ + public static final double poseError_m = 1; // comparing visionPose to pose + public static final double botpose_fieldOffsetX_m = 0.025; // realife offset to pathplanner app + /* ObjectDetectionVision */ + public static final double gamePieceRotateKp = 1.75; + public static final double gamePieceRotateKd = 1.4; + + public static final double AprilTagRotateKp = 1.2; + public static final double AprilTagRotateKd = 0.34; + + public static final double crosshairGamePieceBoundRotateX_rad = Math.toRadians(1.5); + + public static final int redSpeakerTag = 4; + public static final int blueSpeakerTag = 7; + + public static final double limelightMountAngleDegrees = 50; // placeholder + public static final double LensHeight = 7.5; // placeholder + public static final double goalHeight = 53.63; // Speaker tag center height + + public static final double limelightXOffset = 0; // offset the limelight is from the center of the robot + +} diff --git a/src/main/java/frc/robot/io/FieldVisionIO.java b/src/main/java/frc/robot/io/FieldVisionIO.java new file mode 100644 index 0000000..030ed5b --- /dev/null +++ b/src/main/java/frc/robot/io/FieldVisionIO.java @@ -0,0 +1,97 @@ +package frc.robot.io; + +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; + +import org.littletonrobotics.junction.AutoLog; + +public class FieldVisionIO { + @AutoLog + public static class FieldVisionIOInputs { + public int pipeline = 0; + public boolean hasTarget = false; + public int targetAprilTag = 0; + public double ty = 0; + + public double crosshairToTargetErrorX_rad = 0.0; + public double crosshairToTargetErrorY_rad = 0.0; + public double targetArea = 0.0; + + public double targetspaceTranslationX_m = 0.0; + public double targetspaceTranslationY_m = 0.0; + public double targetspaceTranslationZ_m = 0.0; + public double targetspaceRotationX_rad = 0.0; + public double targetspaceRotationY_rad = 0.0; + public double targetspaceRotationZ_rad = 0.0; + + public double fieldspaceTranslationX_m = 0.0; + public double fieldspaceTranslationY_m = 0.0; + public double fieldspaceTranslationZ_m = 0.0; + public double fieldspaceRotationX_rad = 0.0; + public double fieldspaceRotationY_rad = 0.0; + public double fieldspaceRotationZ_rad = 0.0; + + public double fieldspaceTotalLatency_s = 0.0; + } + + private NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-fv"); + + private NetworkTableEntry pipelineEntry = table.getEntry("pipeline"); + private NetworkTableEntry tvEntry = table.getEntry("tv"); + private NetworkTableEntry tidEntry = table.getEntry("tid"); + + private NetworkTableEntry txEntry = table.getEntry("tx"); + private NetworkTableEntry tyEntry = table.getEntry("ty"); + private NetworkTableEntry taEntry = table.getEntry("ta"); + + private NetworkTableEntry clEntry = table.getEntry("cl"); + private NetworkTableEntry tlEntry = table.getEntry("tl"); + + private NetworkTableEntry targetspaceEntry = table.getEntry("botpose_targetspace"); + private double[] targetspaceCache = new double[6]; // array that will hold all the positions + + private NetworkTableEntry botpose_wpiblueEntry = table.getEntry("botpose_wpiblue"); + private double[] botpose_wpiblueCache = new double[7]; + + private LinearFilter targetSpaceXFilter = LinearFilter.movingAverage(40); + private LinearFilter targetSpaceZFilter = LinearFilter.movingAverage(40); + + public void updateInputs(FieldVisionIOInputs inputs) { + inputs.pipeline = pipelineEntry.getNumber(inputs.pipeline).intValue(); + inputs.hasTarget = tvEntry.getDouble(0) == 1; + inputs.targetAprilTag = tidEntry.getNumber(inputs.targetAprilTag).intValue(); + inputs.crosshairToTargetErrorX_rad = Math.toRadians(txEntry.getDouble(inputs.crosshairToTargetErrorX_rad)); + inputs.crosshairToTargetErrorY_rad = Math.toRadians(tyEntry.getDouble(inputs.crosshairToTargetErrorX_rad)); + inputs.targetArea = taEntry.getDouble(inputs.targetArea); + inputs.ty = tyEntry.getDouble(inputs.ty); + + targetspaceCache = targetspaceEntry.getDoubleArray(targetspaceCache); + if (targetspaceCache.length > 0) { + inputs.targetspaceTranslationX_m = targetSpaceXFilter.calculate(targetspaceCache[0]); + inputs.targetspaceTranslationY_m = targetspaceCache[1]; + inputs.targetspaceTranslationZ_m = targetSpaceZFilter.calculate(targetspaceCache[2]); + inputs.targetspaceRotationX_rad = Math.toRadians(targetspaceCache[3]); + inputs.targetspaceRotationY_rad = Math.toRadians(targetspaceCache[4]); + inputs.targetspaceRotationZ_rad = Math.toRadians(targetspaceCache[5]); + } else + DriverStation.reportWarning("invalid targetspace array from limelight", true); + + if (botpose_wpiblueCache.length > 0) { + botpose_wpiblueCache = botpose_wpiblueEntry.getDoubleArray(botpose_wpiblueCache); + inputs.fieldspaceTranslationX_m = botpose_wpiblueCache[0]; + inputs.fieldspaceTranslationY_m = botpose_wpiblueCache[1]; + inputs.fieldspaceTranslationZ_m = botpose_wpiblueCache[2]; + inputs.fieldspaceRotationX_rad = Math.toRadians(botpose_wpiblueCache[3]); + inputs.fieldspaceRotationY_rad = Math.toRadians(botpose_wpiblueCache[4]); + inputs.fieldspaceRotationZ_rad = Math.toRadians(botpose_wpiblueCache[5]); + inputs.fieldspaceTotalLatency_s = botpose_wpiblueCache[6] / 1000; + } else { + DriverStation.reportWarning("(Field Vision) invalid wpiblue array from limelight", true); + inputs.fieldspaceTotalLatency_s = (tlEntry.getDouble(0) - clEntry.getDouble(0)) / 1000; + } + } + +} diff --git a/src/main/java/frc/robot/io/PieceVisionIO.java b/src/main/java/frc/robot/io/PieceVisionIO.java new file mode 100644 index 0000000..b349323 --- /dev/null +++ b/src/main/java/frc/robot/io/PieceVisionIO.java @@ -0,0 +1,97 @@ +// 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.io; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; + +import org.littletonrobotics.junction.AutoLog; + +/** Add your docs here. */ +public class PieceVisionIO { + @AutoLog + public static class PieceVisionIOInputs { + public int pipeline = 0; + public boolean hasTarget = false; + public int targetAprilTag = 0; + + public double crosshairToTargetErrorX_rad = 0.0; + public double crosshairToTargetErrorY_rad = 0.0; + public double targetArea = 0.0; + + public double targetspaceTranslationX_m = 0.0; + public double targetspaceTranslationY_m = 0.0; + public double targetspaceTranslationZ_m = 0.0; + public double targetspaceRotationX_rad = 0.0; + public double targetspaceRotationY_rad = 0.0; + public double targetspaceRotationZ_rad = 0.0; + + // This stuff is useless, but is still stuf that will be logged bc why not + public double fieldspaceTranslationX_m = 0.0; + public double fieldspaceTranslationY_m = 0.0; + public double fieldspaceTranslationZ_m = 0.0; + public double fieldspaceRotationX_rad = 0.0; + public double fieldspaceRotationY_rad = 0.0; + public double fieldspaceRotationZ_rad = 0.0; + + public double fieldspaceTotalLatency_s = 0.0; + } + + // Network table declarations + private NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-gpv"); + + private NetworkTableEntry pipelineEntry = table.getEntry("pipeline"); + private NetworkTableEntry tvEntry = table.getEntry("tv"); + private NetworkTableEntry tidEntry = table.getEntry("tid"); + + private NetworkTableEntry txEntry = table.getEntry("tx"); + private NetworkTableEntry tyEntry = table.getEntry("ty"); + private NetworkTableEntry taEntry = table.getEntry("ta"); + + private NetworkTableEntry clEntry = table.getEntry("cl"); + private NetworkTableEntry tlEntry = table.getEntry("tl"); + + private NetworkTableEntry targetspaceEntry = table.getEntry("targetspace"); + private double[] targetspaceCache = new double[6]; // array that will hold all the positions + + private NetworkTableEntry botpose_wpiblueEntry = table.getEntry("botpose_wpiblue"); + private double[] botpose_wpiblueCache = new double[7]; + + public void updateInputs(PieceVisionIOInputs inputs) { + inputs.pipeline = pipelineEntry.getNumber(inputs.pipeline).intValue(); + inputs.hasTarget = tvEntry.getDouble(0) == 1; + inputs.targetAprilTag = tidEntry.getNumber(inputs.targetAprilTag).intValue(); + inputs.crosshairToTargetErrorX_rad = Math.toRadians(txEntry.getDouble(inputs.crosshairToTargetErrorX_rad)); + inputs.crosshairToTargetErrorY_rad = Math.toRadians(tyEntry.getDouble(inputs.crosshairToTargetErrorY_rad)); + inputs.targetArea = taEntry.getDouble(inputs.targetArea); + + targetspaceCache = targetspaceEntry.getDoubleArray(targetspaceCache); + if (targetspaceCache.length > 0) { + inputs.targetspaceTranslationX_m = targetspaceCache[0]; + inputs.targetspaceTranslationY_m = targetspaceCache[1]; + inputs.targetspaceTranslationZ_m = targetspaceCache[2]; + inputs.targetspaceRotationX_rad = Math.toRadians(targetspaceCache[3]); + inputs.targetspaceRotationY_rad = Math.toRadians(targetspaceCache[4]); + inputs.targetspaceRotationZ_rad = Math.toRadians(targetspaceCache[5]); + } else + DriverStation.reportWarning("invalid targetspace array from limelight", true); + + if (botpose_wpiblueCache.length > 0) { + botpose_wpiblueCache = botpose_wpiblueEntry.getDoubleArray(botpose_wpiblueCache); + inputs.fieldspaceTranslationX_m = botpose_wpiblueCache[0]; + inputs.fieldspaceTranslationY_m = botpose_wpiblueCache[1]; + inputs.fieldspaceTranslationZ_m = botpose_wpiblueCache[2]; + inputs.fieldspaceRotationX_rad = Math.toRadians(botpose_wpiblueCache[3]); + inputs.fieldspaceRotationY_rad = Math.toRadians(botpose_wpiblueCache[4]); + inputs.fieldspaceRotationZ_rad = Math.toRadians(botpose_wpiblueCache[5]); + inputs.fieldspaceTotalLatency_s = botpose_wpiblueCache[6] / 1000; + } else { + DriverStation.reportWarning("(Piece Vision)invalid wpiblue array from limelight", true); + inputs.fieldspaceTotalLatency_s = (tlEntry.getDouble(0) - clEntry.getDouble(0)) / 1000; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 61f062d..44644c3 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -2,16 +2,24 @@ import static frc.robot.constants.ArmConstants.*; +import frc.robot.util.Math102; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.AccelStrategy; import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.AutoLogOutput; @@ -20,31 +28,72 @@ import lombok.Getter; public class Arm extends SubsystemBase { - private CANSparkMax motor = new CANSparkMax(motorId, MotorType.kBrushless); - private SparkPIDController pidController = motor.getPIDController(); - private RelativeEncoder encoder = motor.getEncoder(); + private CANSparkMax leadMotor = new CANSparkMax(leadMotorId, MotorType.kBrushless); + private CANSparkMax followerMotor = new CANSparkMax(followerMotorId, MotorType.kBrushless); + private SparkPIDController pidController = leadMotor.getPIDController(); + private RelativeEncoder motorEncoder = leadMotor.getEncoder(); // built-in encoder in the lead NEO + // throughbore encoder on hex shaft + private AbsoluteEncoder shaftEncoder = leadMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); private ArmFeedforward feedforwardController = new ArmFeedforward(kS, kG, kV, kA); + private Relay climberRelay = new Relay(0); + @Getter @AutoLogOutput - private double targetPosition_rad = 0; + private double targetPosition_deg = 0; public Arm() { - motor.setIdleMode(IdleMode.kBrake); - + leadMotor.restoreFactoryDefaults(); + leadMotor.setIdleMode(IdleMode.kBrake); + leadMotor.setInverted(false); + leadMotor.setSmartCurrentLimit(45); + leadMotor.setSecondaryCurrentLimit(65); + leadMotor.enableVoltageCompensation(12); + + leadMotor.setSoftLimit(SoftLimitDirection.kForward, (float) (110 + shaftEncoderOffset_deg)); + leadMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) (-1.5 + shaftEncoderOffset_deg)); + leadMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + leadMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + + followerMotor.restoreFactoryDefaults(); + followerMotor.setIdleMode(IdleMode.kBrake); + followerMotor.setSmartCurrentLimit(45); + followerMotor.setSecondaryCurrentLimit(65); + followerMotor.enableVoltageCompensation(12); + followerMotor.follow(leadMotor, true); + + // revolutions * deg / rev = deg + shaftEncoder.setPositionConversionFactor(360); + // rev / sec * sec / min = RPM + shaftEncoder.setVelocityConversionFactor(60); + shaftEncoder.setInverted(true); + shaftEncoder.setZeroOffset(0); + + pidController.setFeedbackDevice(shaftEncoder); + // Smart motion applies a velocity and acceleration limiter as it travels to the target position. More info can be + // found here: + // https://github.com/REVrobotics/SPARK-MAX-Examples/blob/master/Java/Smart%20Motion%20Example/src/main/java/frc/robot/Robot.java + pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 0); + pidController.setSmartMotionMaxAccel(maxAccel_rpmps, 0); + pidController.setSmartMotionMaxVelocity(maxVelocity_rpm, 0); + pidController.setSmartMotionAllowedClosedLoopError(accuracyTolerance_deg, 0); + pidController.setOutputRange(minOutput, maxOutput); + // since we are using smartmotion, the PID numbers are for velocity control, not position. pidController.setP(kP); pidController.setD(kD); - pidController.setI(kI); - - motor.setSmartCurrentLimit(50); - motor.setSecondaryCurrentLimit(65); - - encoder.setPositionConversionFactor((1 / gearRatio) * 2 * Math.PI); // motor revolutions * arm rev / motor rev * arm - // radians / arm rev = arm radians - encoder.setVelocityConversionFactor((1 / gearRatio) * (2 * Math.PI) * (1 / 60)); // RPM * arm rev / motor rev * - // radians / - // revolution * min / sec = rad/s + // Treats 0 and 360 degrees as the same number, so going from one side of 0 to the other doesnt make it do a 360 + pidController.setPositionPIDWrappingEnabled(true); + pidController.setPositionPIDWrappingMinInput(0); + pidController.setPositionPIDWrappingMaxInput(360); + + // if (tuningMode) { + // new AutoSetterTunableNumber("Arm/kP", kP, (value) -> pidController.setP(value)); + // new AutoSetterTunableNumber("Arm/kD", kD, (value) -> pidController.setD(value)); + // } + + leadMotor.burnFlash(); + followerMotor.burnFlash(); } @Override @@ -55,39 +104,67 @@ public void periodic() { @AutoLog public static class ArmIOInputs { - public double current_A = 0.0; - public double voltage_V = 0.0; - public double temperature_C = 0.0; - public double position_rad = 0.0; - public double velocity_radps = 0.0; + public double leadCurrent_A = 0.0; + public double leadBusVoltage_V = 0.0; + public double leadAppliedVoltage_V = 0.0; + public double leadTemperature_C = 0.0; + public double leadPercentOutput = 0.0; + + public double followerCurrent_A = 0.0; + public double followerBusVoltage_V = 0.0; + public double followerAppliedVoltage_V = 0.0; + public double followerTemperature_C = 0.0; + public double followerPercentOutput = 0.0; + + public double shaftPosition_deg = 0.0; + public double shaftVelocity_rpm = 0.0; + public double motorVelocity_rpm = 0.0; } public final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); private void updateInputs(ArmIOInputs inputs) { - inputs.current_A = motor.getOutputCurrent(); - inputs.voltage_V = motor.getBusVoltage(); - inputs.temperature_C = motor.getMotorTemperature(); - inputs.position_rad = encoder.getPosition(); - inputs.velocity_radps = encoder.getVelocity(); + inputs.leadCurrent_A = leadMotor.getOutputCurrent(); + inputs.leadBusVoltage_V = leadMotor.getBusVoltage(); + inputs.leadAppliedVoltage_V = 12 * leadMotor.getAppliedOutput(); + inputs.leadTemperature_C = leadMotor.getMotorTemperature(); + inputs.leadPercentOutput = leadMotor.getAppliedOutput(); + + inputs.followerCurrent_A = followerMotor.getOutputCurrent(); + inputs.followerBusVoltage_V = followerMotor.getBusVoltage(); + inputs.leadAppliedVoltage_V = 12 * followerMotor.getAppliedOutput(); + inputs.followerTemperature_C = followerMotor.getMotorTemperature(); + inputs.followerPercentOutput = followerMotor.getAppliedOutput(); + + inputs.shaftPosition_deg = Math102.truncate(shaftEncoder.getPosition() - shaftEncoderOffset_deg, 2); + inputs.shaftVelocity_rpm = Math102.truncate(shaftEncoder.getVelocity(), 2); + inputs.motorVelocity_rpm = Math102.truncate(motorEncoder.getVelocity(), 2); + } + + public void setPosition(double position_deg) { + targetPosition_deg = position_deg; + pidController.setReference(targetPosition_deg + shaftEncoderOffset_deg, ControlType.kSmartMotion, 0, + feedforwardController.calculate(Units.degreesToRadians(targetPosition_deg), 0)); + } + + public void setMotorVoltage(double voltage_V) { + Logger.recordOutput("Arm/targetVoltage_V", voltage_V); + pidController.setReference(voltage_V, ControlType.kVoltage, 0, + feedforwardController.calculate(Units.degreesToRadians(inputs.shaftPosition_deg), 0)); } - public void setPosition(double position_rad) { - targetPosition_rad = position_rad; - pidController.setReference(targetPosition_rad, ControlType.kPosition, 0, - feedforwardController.calculate(targetPosition_rad, 0)); + public void setClimberRelay(Relay.Value value) { + climberRelay.set(value); + Logger.recordOutput("Arm/relaySetting", value); } - public void stopArm() { - pidController.setReference(0, ControlType.kDutyCycle, 0, feedforwardController.calculate(inputs.position_rad, 0)); + public void stop() { + pidController.setReference(0, ControlType.kDutyCycle, 0, + feedforwardController.calculate(Units.degreesToRadians(inputs.shaftPosition_deg), 0)); // Set } public boolean closeEnough() { - if (MathUtil.isNear(targetPosition_rad, inputs.position_rad, closeVar)) { - return true; - } else { - return false; - } + return MathUtil.isNear(targetPosition_deg, inputs.shaftPosition_deg, accuracyTolerance_deg); } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 098d97e..1081caf 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,6 +2,7 @@ import static frc.robot.constants.IntakeConstants.*; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -10,29 +11,47 @@ import com.revrobotics.CANSparkMax; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import lombok.Getter; +import lombok.Setter; + public class Intake extends SubsystemBase { private final CANSparkMax motor = new CANSparkMax(motorId, MotorType.kBrushless); private final DigitalInput noteSensor = new DigitalInput(sensorId); + @Getter + @Setter + @AutoLogOutput + private boolean holdingNote = false; + + private boolean cachedNoteSensor = false; + public Intake() { + motor.restoreFactoryDefaults(); motor.setIdleMode(IdleMode.kBrake); + motor.enableVoltageCompensation(12); + motor.burnFlash(); + } @Override public void periodic() { updateInputs(inputs); Logger.processInputs(getName(), inputs); - } - public void setMotorSpeed(double speed) { - motor.set(speed); + if ((inputs.noteSensor != cachedNoteSensor) && inputs.noteSensor) { + holdingNote = !holdingNote; + } + + cachedNoteSensor = inputs.noteSensor; } - public void setMotorVoltage(double voltage_V) { - motor.setVoltage(voltage_V); + public void setMotorSpeed(double speed) { + speed = MathUtil.clamp(speed, -1, 1); + motor.setVoltage(speed * 12); } @AutoLog @@ -54,11 +73,6 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.noteSensor = !noteSensor.get(); } - public boolean detectNote() { - return inputs.noteSensor; - - } - public void stopMotor() { motor.stopMotor(); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index d25b45a..5cae79e 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -6,6 +6,7 @@ import frc.robot.util.AutoSetterTunableNumber; import frc.robot.util.SparkUtil; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -35,6 +36,8 @@ public class Shooter extends SubsystemBase { private double targetVelocity_rpm = 0.0; public Shooter() { + leadMotor.restoreFactoryDefaults(); + followerMotor.restoreFactoryDefaults(); if (tuningMode) { new AutoSetterTunableNumber("Shooter/kP", kP, (double value) -> pidController.setP(value)); new AutoSetterTunableNumber("Shooter/kD", kD, (double value) -> pidController.setD(value)); @@ -47,18 +50,22 @@ public Shooter() { pidController.setD(kD); leadMotor.setIdleMode(IdleMode.kCoast); + leadMotor.enableVoltageCompensation(12); followerMotor.setIdleMode(IdleMode.kCoast); + followerMotor.enableVoltageCompensation(12); followerMotor.follow(leadMotor); // only fetch position when tuning SparkUtil.setPeriodicFrames(leadMotor, true, true, tuningMode, false, false, false, false); SparkUtil.setPeriodicFrames(followerMotor, false, false, false, false, false, false, false); + leadMotor.burnFlash(); + followerMotor.burnFlash(); } public void setVelocity(double velocity_rpm) { - targetVelocity_rpm = velocity_rpm; - pidController.setReference(velocity_rpm, ControlType.kVelocity, 0, feedforward.calculate(velocity_rpm)); + targetVelocity_rpm = MathUtil.clamp(velocity_rpm, 0, maxVelocity_rpm); + pidController.setReference(targetVelocity_rpm, ControlType.kVelocity, 0, feedforward.calculate(targetVelocity_rpm)); } public void setPercentOutput(double speed) { @@ -79,12 +86,14 @@ public void periodic() { public static class ShooterIOInputs { // public double leadPosition_rot = 0.0; public double leadVelocity_rpm = 0.0; - public double leadVoltage_V = 0.0; + public double leadBusVoltage_V = 0.0; + public double leadAppliedVoltage_V = 0.0; public double leadCurrent_A = 0.0; public double leadTemperature_C = 0.0; public double leadPercentOutput = 0.0; - public double followerVoltage_V = 0.0; + public double followerBusVoltage_V = 0.0; + public double followerAppliedVoltage_V = 0.0; public double followerCurrent_A = 0.0; public double followerTemperature_C = 0.0; public double followerPercentOutput = 0.0; @@ -95,13 +104,15 @@ public static class ShooterIOInputs { private void updateInputs(ShooterIOInputs inputs) { // inputs.leadPosition_rot = encoder.getPosition(); inputs.leadVelocity_rpm = encoder.getVelocity(); - inputs.leadVoltage_V = leadMotor.getBusVoltage(); + inputs.leadBusVoltage_V = leadMotor.getBusVoltage(); + inputs.leadAppliedVoltage_V = leadMotor.getAppliedOutput() * 12; inputs.leadCurrent_A = leadMotor.getOutputCurrent(); inputs.leadTemperature_C = leadMotor.getMotorTemperature(); inputs.leadPercentOutput = leadMotor.getAppliedOutput(); // important to log to ensure proper functionality - inputs.followerVoltage_V = followerMotor.getBusVoltage(); + inputs.followerBusVoltage_V = followerMotor.getBusVoltage(); + inputs.followerAppliedVoltage_V = followerMotor.getAppliedOutput() * 12; inputs.followerCurrent_A = followerMotor.getOutputCurrent(); inputs.followerTemperature_C = followerMotor.getMotorTemperature(); inputs.followerPercentOutput = followerMotor.getAppliedOutput(); @@ -115,7 +126,7 @@ private void updateInputs(ShooterIOInputs inputs) { }, log -> { log.motor("shooter-lead") - .voltage(Units.Volts.of(inputs.leadVoltage_V * inputs.leadPercentOutput)) + .voltage(Units.Volts.of(inputs.leadAppliedVoltage_V)) .angularPosition(Units.Rotations.of(encoder.getPosition())) .angularVelocity(Units.RPM.of(inputs.leadVelocity_rpm)); }, diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index abf3e1d..00a7ff4 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -11,6 +11,7 @@ import frc.robot.subsystems.swerve.SwerveModuleIOSim; import frc.robot.util.LocalADStarAK; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -63,11 +64,11 @@ public class Swerve extends SubsystemBase { public GyroIO gyroIO; public GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - // private Vision vision; + private Vision vision; private double translationY; private double translationX; - public Swerve(GyroIO gyroIO/* , Vision vision */) { + public Swerve(GyroIO gyroIO, Vision vision) { modules = new SwerveModule[moduleConstants.length]; int moduleNumber = 0; for (var mod : moduleConstants) { @@ -77,6 +78,8 @@ public Swerve(GyroIO gyroIO/* , Vision vision */) { moduleNumber++; } + this.vision = vision; + this.gyroIO = gyroIO; zeroYaw(); @@ -241,28 +244,35 @@ public void periodic() { var pose = poseEstimator.getEstimatedPosition(); translationY = pose.getY(); translationX = pose.getX(); - // todo: estimate without using gyro? - // // Every 0.02s, updating pose2d - // if (vision.inputs.pipeline == pipeline.AprilTag.value && vision.isPipelineReady() - // && vision.inputs.target == true) { - // visionSeenCount++; - // if (visionSeenCount > 2) { // fixme: temporary - // var visionPose = new Pose2d(vision.inputs.botpose_fieldspaceTranslationX_m, - // vision.inputs.botpose_fieldspaceTranslationY_m, - // new Rotation2d(vision.inputs.botpose_fieldspaceRotationZ_rad)); - // Logger.recordOutput("Odometry/VisionPose", visionPose); - // // var distance = Math.hypot( - // // Math.abs(translationX - vision.inputs.botpose_fieldspaceTranslationX_m), - // // Math.abs(translationY - vision.inputs.botpose_fieldspaceTranslationY_m)); - // poseEstimator.addVisionMeasurement(visionPose, timer.get() - vision.inputs.botpose_latency_s); - // // VecBuilder.fill(distance / 2, distance / 2, 100)); - // } - // } else - // visionSeenCount = 0; + // Rotation2d rotation = new Rotation2d(vision.fieldInputs.fieldspaceRotationX_rad); + // double translationX = vision.fieldInputs.fieldspaceTranslationY_m; + // double translationY = vision.fieldInputs.fieldspaceTranslationX_m; + // Pose2d visionPose = new Pose2d(translationX, translationY, rotation); + // poseEstimator.addVisionMeasurement(visionPose, timer.get() - vision.fieldInputs.fieldspaceTotalLatency_s); + // todo: estimate without using gyro? + // Every 0.02s, updating pose2d + if (vision.fieldInputs.hasTarget == true) { + visionSeenCount++; + if (visionSeenCount > 2) { // The if statement is used to eliminate "hallucinations". Schizophrenic robot smh + var visionPose = new Pose2d(vision.fieldInputs.fieldspaceTranslationX_m, + vision.fieldInputs.fieldspaceTranslationY_m, + new Rotation2d(vision.fieldInputs.fieldspaceRotationX_rad)); // continuation of lines 259-261. Create a new + // pose2d + // using X,Y and rotation + Logger.recordOutput("Odometry/VisionPose", visionPose); // log the odometry to advantage kit + var distance = Math.hypot( // use pythagorean theorem to find the distance from the last position + Math.abs(translationX - vision.fieldInputs.fieldspaceTranslationX_m), + Math.abs(translationY - vision.fieldInputs.fieldspaceTranslationY_m)); + poseEstimator.addVisionMeasurement(visionPose, + timer.get() - vision.fieldInputs.fieldspaceTotalLatency_s); // remove lag from the time in the calculation of + // estimated pose + VecBuilder.fill(distance / 2, distance / 2, 100); + } + } else + visionSeenCount = 0; Logger.recordOutput("Odometry/Robot", pose); - // update field pose for (int i = 0; i < modules.length; i++) { modulePoses[i] = new Pose2d( @@ -301,7 +311,7 @@ public void periodic() { log -> { for (int i = 0; i < modules.length; i++) log.motor("drive" + i) - .voltage(Units.Volts.of(modules[i].inputs.driveVoltage_V)) + .voltage(Units.Volts.of(modules[i].inputs.driveAppliedVoltage_V)) .linearPosition(Units.Meters.of(modules[i].inputs.driveDistance_m)) .linearVelocity(Units.MetersPerSecond.of(modules[i].inputs.driveVelocity_mps)); }, @@ -328,7 +338,7 @@ public void periodic() { log -> { for (int i = 0; i < modules.length; i++) log.motor("angle" + i) - .voltage(Units.Volts.of(modules[i].inputs.angleVoltage_V)) + .voltage(Units.Volts.of(modules[i].inputs.angleAppliedVoltage_V)) .linearPosition(Units.Meters.of(modules[i].inputs.angleAbsolutePosition_rad)) .linearVelocity(Units.MetersPerSecond.of(modules[i].inputs.angleVelocity_radps)); }, diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java new file mode 100644 index 0000000..292db90 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -0,0 +1,103 @@ +package frc.robot.subsystems; + +import frc.robot.constants.ScoringConstants; +import frc.robot.constants.ScoringConstants.ScoringPosition; +import frc.robot.constants.VisionConstants; +import frc.robot.io.FieldVisionIO; +import frc.robot.io.FieldVisionIOInputsAutoLogged; +import frc.robot.io.PieceVisionIO; +import frc.robot.io.PieceVisionIOInputsAutoLogged; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.littletonrobotics.junction.Logger; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.stream.Collectors; + +public class Vision extends SubsystemBase { + private FieldVisionIO fieldIO = new FieldVisionIO(); + private PieceVisionIO pieceIO = new PieceVisionIO(); + public FieldVisionIOInputsAutoLogged fieldInputs = new FieldVisionIOInputsAutoLogged(); + public PieceVisionIOInputsAutoLogged pieceInputs = new PieceVisionIOInputsAutoLogged(); + private NetworkTableEntry priorityIdEntry = NetworkTableInstance.getDefault().getTable("limelight-fv") + .getEntry("priorityid"); + + public Vision() {} + + @Override + public void periodic() { + fieldIO.updateInputs(fieldInputs); + pieceIO.updateInputs(pieceInputs); + Logger.processInputs(getName() + "/field", fieldInputs); + Logger.processInputs(getName() + "/piece", pieceInputs); + } + + public void setPriorityId(int id) { + priorityIdEntry.setNumber(id); + } + + public double findDistance() { + double targetOffsetAngle_Vertical = fieldInputs.ty; + double angleToGoalDegrees = VisionConstants.limelightMountAngleDegrees + targetOffsetAngle_Vertical; + double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); + + double distance = (VisionConstants.goalHeight - VisionConstants.LensHeight) / Math.tan(angleToGoalRadians); + + return distance; + } + + public ScoringPosition estimateScoringPosition_math() { + double distance = Math.hypot(fieldInputs.targetspaceTranslationX_m, fieldInputs.targetspaceTranslationZ_m); + + double angle = -20 + (14.9 * distance) + (-.889 * Math.pow(distance, 2)); + double speed = 2515 + (990 * distance) + (-473 * Math.pow(distance, 2)) + (79.4 * Math.pow(distance, 3)); + + return new ScoringPosition(angle, speed); + } + + public ScoringPosition estimateScoringPosition_map() { + double distance = Math.hypot(fieldInputs.targetspaceTranslationX_m, fieldInputs.targetspaceTranslationZ_m); + + // sort the keys with stream methods then collect into a set + ArrayList keys = ScoringConstants.scoringMap.keySet().stream().sorted() + .collect(Collectors.toCollection(ArrayList::new)); + // get closest value + double closestKey_m = keys.stream().min(Comparator.comparing(i -> Math.abs(i - distance))).orElseThrow() + .doubleValue(); + + // instantiate bounds to be used for linear interpolation + double upperKey_m, lowerKey_m; + if (closestKey_m < distance) { + // the closest value is less than the actual distance, set it as the lower bound and set the upper bound as the + // next one up + lowerKey_m = closestKey_m; + upperKey_m = keys.get(keys.indexOf(closestKey_m) + 1); + } else { + // opposite if the closest value is above the actual distance + upperKey_m = closestKey_m; + lowerKey_m = keys.get(keys.indexOf(closestKey_m) - 1); + } + double interpolationDistance = (distance - lowerKey_m) / (upperKey_m - lowerKey_m); + + var lowerPosition = ScoringConstants.scoringMap.get(lowerKey_m); + var upperPosition = ScoringConstants.scoringMap.get(upperKey_m); + + // interpolate between the two positions to get the estimated angle and speed + double angle = MathUtil.interpolate(lowerPosition.armAngle_deg(), upperPosition.armAngle_deg(), + interpolationDistance); + double speed = MathUtil.interpolate(lowerPosition.shooterSpeed_rpm(), upperPosition.shooterSpeed_rpm(), + interpolationDistance); + + return new ScoringPosition(angle, speed); + } +} + +/* + * The point of this subsystem is to take all the inputs from VisionIO and log them. + * From there we can take values from here and apply them elsewhere + */ \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 0fbc252..15c5f4f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -2,7 +2,7 @@ import static frc.robot.constants.SwerveConstants.maxVelocity_mps; -import frc.robot.util.Conversions; +import frc.robot.util.Math102; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -126,7 +126,7 @@ public void periodic() { lastAngle = angle; Logger.recordOutput("SwerveModule " + moduleNumber + "/targetAngle_rad", - Conversions.angleModulus2pi(angle.getRadians())); + Math102.angleModulus2pi(angle.getRadians())); } public void setDriveBrakeMode(boolean enable) { diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIO.java index a4c63ce..f5bf532 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIO.java @@ -11,14 +11,16 @@ public static class SwerveModuleIOInputs { // double drivePosition_rad = 0.0; public double driveDistance_m = 0.0; public double driveVelocity_mps = 0.0; - public double driveVoltage_V = 0.0; + public double driveAppliedVoltage_V = 0.0; + public double driveBusVoltage_V = 0.0; public double driveCurrent_A = 0.0; public double driveTemperature_C = 0.0; public double anglePosition_rad = 0.0; public double angleAbsolutePosition_rad = 0.0; public double angleVelocity_radps = 0.0; - public double angleVoltage_V = 0.0; + public double angleAppliedVoltage_V = 0.0; + public double angleBusVoltage_V = 0.0; public double angleCurrent_A = 0.0; public double angleTemperature_C = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIOReal.java index 38e391a..1bea211 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIOReal.java @@ -4,7 +4,7 @@ import static frc.robot.constants.SwerveConstants.*; import frc.robot.util.AutoSetterTunableNumber; -import frc.robot.util.Conversions; +import frc.robot.util.Math102; import frc.robot.util.SparkUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -92,16 +92,16 @@ public SwerveModuleIOReal(SwerveModuleConstants moduleConstants, int moduleNumbe anglePidController.setFeedbackDevice(angleAbsoluteEncoder); angleRelativeEncoder = angleMotor.getEncoder(); - angleRelativeEncoder.setPositionConversionFactor(Conversions.twoPi / angleGearRatio); // rad - angleRelativeEncoder.setVelocityConversionFactor(Conversions.twoPi / angleGearRatio / 60.0); // radps + angleRelativeEncoder.setPositionConversionFactor(Math102.twoPi / angleGearRatio); // rad + angleRelativeEncoder.setVelocityConversionFactor(Math102.twoPi / angleGearRatio / 60.0); // radps // on first cycle, reset relative encoder to absolute - angleRelativeEncoder.setPosition(Conversions.angleModulus2pi(angleAbsoluteEncoder.getPosition() - angleOffset_rad)); + angleRelativeEncoder.setPosition(Math102.angleModulus2pi(angleAbsoluteEncoder.getPosition() - angleOffset_rad)); // wrap betwee 0 and 2pi radians anglePidController.setPositionPIDWrappingEnabled(true); anglePidController.setPositionPIDWrappingMinInput(0); - anglePidController.setPositionPIDWrappingMaxInput(Conversions.twoPi); + anglePidController.setPositionPIDWrappingMaxInput(Math102.twoPi); SparkUtil.setPeriodicFrames(angleMotor, false, true, true, true, false, false, false); @@ -112,18 +112,20 @@ public SwerveModuleIOReal(SwerveModuleConstants moduleConstants, int moduleNumbe public void updateInputs(SwerveModuleIOInputs inputs) { inputs.driveDistance_m = SparkUtil.cleanValue(inputs.driveDistance_m, driveRelativeEncoder.getPosition()); inputs.driveVelocity_mps = SparkUtil.cleanValue(inputs.driveVelocity_mps, driveRelativeEncoder.getVelocity()); - inputs.driveVoltage_V = driveMotor.getBusVoltage() * driveMotor.getAppliedOutput(); + inputs.driveAppliedVoltage_V = 12 * driveMotor.getAppliedOutput(); + inputs.driveBusVoltage_V = driveMotor.getBusVoltage(); inputs.driveCurrent_A = driveMotor.getOutputCurrent(); inputs.driveTemperature_C = driveMotor.getMotorTemperature(); inputs.anglePosition_rad = SparkUtil.cleanValue(inputs.anglePosition_rad, angleRelativeEncoder.getPosition()); inputs.angleAbsolutePosition_rad = SparkUtil.cleanValue(inputs.angleAbsolutePosition_rad, // ensure offset is properly wrapped - Conversions.truncate(Conversions.angleModulus2pi(angleAbsoluteEncoder.getPosition() - angleOffset_rad), 3)); + Math102.truncate(Math102.angleModulus2pi(angleAbsoluteEncoder.getPosition() - angleOffset_rad), 3)); // inputs.angleVelocity_radps = angleAbsoluteEncoder.getVelocity(); // gives garbage data // inputs.angleAbsolutePosition_rad = angleAbsoluteEncoder.getPosition() - angleOffset_rad;s inputs.angleVelocity_radps = SparkUtil.cleanValue(inputs.angleVelocity_radps, angleRelativeEncoder.getVelocity()); - inputs.angleVoltage_V = angleMotor.getBusVoltage() * angleMotor.getAppliedOutput(); + inputs.angleAppliedVoltage_V = 12 * angleMotor.getAppliedOutput(); + inputs.angleBusVoltage_V = angleMotor.getBusVoltage(); inputs.angleCurrent_A = angleMotor.getOutputCurrent(); inputs.angleTemperature_C = angleMotor.getMotorTemperature(); } @@ -145,7 +147,7 @@ public void setAngleVoltage(double voltage) { @Override public void setAnglePosition(Rotation2d angle) { - final double targetAngle_rad = Conversions.angleModulus2pi(angle.getRadians() + angleOffset_rad); + final double targetAngle_rad = Math102.angleModulus2pi(angle.getRadians() + angleOffset_rad); anglePidController.setReference(targetAngle_rad, ControlType.kPosition); // 0, angleFeedforward.calculate(targetAngle_rad)); } @@ -162,6 +164,6 @@ public void setOffset(double offset_rad) { @Override public double getDriveCharacterizationVelocity_radps() { - return Conversions.twoPi * (driveRelativeEncoder.getVelocity() / wheelCircumference_m); // undo unit conversion + return Math102.twoPi * (driveRelativeEncoder.getVelocity() / wheelCircumference_m); // undo unit conversion } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIOSim.java index dd9aa67..9b63f7d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModuleIOSim.java @@ -3,7 +3,7 @@ import static frc.robot.constants.Constants.loopPeriod_s; import static frc.robot.constants.SwerveConstants.*; -import frc.robot.util.Conversions; +import frc.robot.util.Math102; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -39,7 +39,7 @@ public class SwerveModuleIOSim implements SwerveModuleIO { private double angleApplied_V = 0.0; public SwerveModuleIOSim() { - angleController.enableContinuousInput(0, Conversions.twoPi); + angleController.enableContinuousInput(0, Math102.twoPi); } @Override @@ -66,13 +66,13 @@ public void updateInputs(SwerveModuleIOInputs inputs) { // inputs.drivePosition_rad = driveSim.getAngularPositionRad(); inputs.driveDistance_m = driveSim.getAngularPositionRad() * wheelRadius_m; inputs.driveVelocity_mps = driveSim.getAngularVelocityRadPerSec() * wheelRadius_m; - inputs.driveVoltage_V = driveApplied_V; + inputs.driveAppliedVoltage_V = driveApplied_V; inputs.driveCurrent_A = Math.abs(driveSim.getCurrentDrawAmps()); inputs.anglePosition_rad = angleSim.getAngularPositionRad(); - inputs.angleAbsolutePosition_rad = Conversions.angleModulus2pi(angleInitPosition_rad + inputs.anglePosition_rad); + inputs.angleAbsolutePosition_rad = Math102.angleModulus2pi(angleInitPosition_rad + inputs.anglePosition_rad); inputs.angleVelocity_radps = angleSim.getAngularVelocityRadPerSec(); - inputs.angleVoltage_V = angleApplied_V; + inputs.angleAppliedVoltage_V = angleApplied_V; inputs.angleCurrent_A = Math.abs(angleSim.getCurrentDrawAmps()); RoboRioSim.setVInVoltage( diff --git a/src/main/java/frc/robot/util/ControllerUtil.java b/src/main/java/frc/robot/util/ControllerUtil.java new file mode 100644 index 0000000..f1e8964 --- /dev/null +++ b/src/main/java/frc/robot/util/ControllerUtil.java @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import frc.robot.constants.Constants.OperatorConstants; + +import edu.wpi.first.math.MathUtil; + +/** Add your docs here. */ +public class ControllerUtil { + /** + * Applies a deadband defined by {@link OperatorConstants#xboxStickDeadband xboxStickDeadband}, + * and scales the resulting value on the function x^2 * sgn(x) (squares it but preserves the curve) + * @param value Unfiltered controller axis value, should be on the interval [-1.0, 1.0] + * @return Returns the squared and deadbanded controller axis + */ + public static double scaleAxis(double value) { + value = MathUtil.applyDeadband(value, OperatorConstants.xboxStickDeadband); + return Math.copySign(value * value, value); + } +} diff --git a/src/main/java/frc/robot/util/Conversions.java b/src/main/java/frc/robot/util/Math102.java similarity index 96% rename from src/main/java/frc/robot/util/Conversions.java rename to src/main/java/frc/robot/util/Math102.java index 4ee923d..a693894 100644 --- a/src/main/java/frc/robot/util/Conversions.java +++ b/src/main/java/frc/robot/util/Math102.java @@ -1,6 +1,6 @@ package frc.robot.util; -public class Conversions { +public class Math102 { public static final double falconCountsPerRotation = 2048.0; public static final double cancoderCountsPerRotation = 4096.0; public static final double twoPi = 2 * Math.PI;