From 7fa8ec8de10208e64d47104d6348ea5528101dec Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Sat, 30 Mar 2024 10:31:17 -0500 Subject: [PATCH 1/3] Reimplemented a variety of improvements from the old development branch --- .vscode/extensions.json | 3 +- .wpilib/wpilib_preferences.json | 2 +- src/main/java/frc/robot/Constants.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 134 +++++++----------- .../ControlCommands/ClimberCommands.java | 53 +++++++ .../ControlCommands/DriveCommands.java | 8 +- .../IntakeShooterControls.java | 43 +----- .../java/frc/robot/commands/IntakeNote.java | 8 -- .../frc/robot/commands/PreRunShooter.java | 21 +-- .../java/frc/robot/commands/ShootNote.java | 2 +- .../java/frc/robot/subsystems/arm/Arm.java | 4 +- .../java/frc/robot/subsystems/arm/ArmIO.java | 4 +- .../robot/subsystems/arm/ArmIOSparkMax.java | 9 +- .../subsystems/climber/ClimberIOSparkMax.java | 109 ++++++++------ .../frc/robot/subsystems/drive/Drive.java | 2 +- .../flywheel/FlywheelIOSparkMax.java | 12 +- .../frc/robot/subsystems/intake/IntakeIO.java | 1 - 17 files changed, 218 insertions(+), 204 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ControlCommands/ClimberCommands.java diff --git a/.vscode/extensions.json b/.vscode/extensions.json index ac610a9..f764530 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -8,6 +8,7 @@ "wpilibsuite.vscode-wpilib", "vscjava.vscode-gradle", "vscjava.vscode-java-debug", - "redhat.java" + "redhat.java", + "yzhang.markdown-all-in-one" ] } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index ce41e5c..6c9a566 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2024", - "teamNumber": 9985 + "teamNumber": 5829 } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e14538..949b49a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -85,6 +85,7 @@ public static final class FlywheelConstants { public static final double ks = 0; public static final double kv = 0.00025; public static final double shootingVelocity = 4500; // revolutions per second + public static final double slowShootingVelocity = 10; // ! guessed value } public static final class IntakeConstants { @@ -160,8 +161,8 @@ public static final class ClimberConstants { public static final int kCurrentLimit = 30; public static final double initialPosition = 0.7; - public static final double minPosition = 0.0; - public static final double maxPosition = 0.55; + public static final double minPosition = -2; // 0 + public static final double maxPosition = 2; // 0.55 public static final double climberConversion = 1; @@ -169,7 +170,7 @@ public static final class ClimberConstants { public static final double gearCircumfrence = 0.134032531982; // Climber PID constants - public static final double kP = 0.1; + public static final double kP = 1; // 0.1 public static final double kI = 0.0; public static final double kD = 0.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 44b0aec..e42d858 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -178,16 +178,14 @@ public RobotContainer() { // NamedCommands.registerCommand("ShootFarPosition", new // ShootFarCommand(sArm).withTimeout(2.0)); - NamedCommands.registerCommand( - "PreRunShooter", new PreRunShooter(sIntake, sFlywheel).withTimeout(4.0)); + NamedCommands.registerCommand("PreRunShooter", new PreRunShooter(sFlywheel).withTimeout(4.0)); // Groups of the above NamedCommands.registerCommand( "StartGroup", new SequentialCommandGroup( new ParallelDeadlineGroup( - ShootClose.run(sArm).withTimeout(2), - new PreRunShooter(sIntake, sFlywheel).withTimeout(4.0)), + ShootClose.run(sArm).withTimeout(2), new PreRunShooter(sFlywheel).withTimeout(4.0)), new ShootNote(sIntake, sFlywheel, sArm).withTimeout(3.0))); NamedCommands.registerCommand( // The name is inaccurate @@ -293,6 +291,9 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + + // # Driver controller configuration + sDrive.setDefaultCommand( DriveCommands.joystickDrive( sDrive, @@ -300,47 +301,6 @@ private void configureButtonBindings() { () -> -driverController.getLeftX(), () -> -driverController.getRightX())); - // driverController - // .a() - // .whileTrue( - // Commands.startEnd( - // () -> - // sFlywheel.runVelocity( - // -flywheelSpeedInput - // .get()), // ! Is the smartdashboard thing permanent? surely not? - // sFlywheel::stop, - // sFlywheel)); - // driverController - // .a() - // .whileFalse(Commands.startEnd(() -> sFlywheel.runVelocity(0), sFlywheel::stop, - // sFlywheel)); - - // driverController // TODO Reverse intake needed, also it stops randomly after a bit, get rid - // of - // .b() - // .whileTrue( - // Commands.startEnd( - // () -> sIntake.runVelocity(Constants.IntakeConstants.velocity), - // sIntake::stop, - // sIntake)); - - // driverController - // .b() - // .whileFalse(Commands.startEnd(() -> sIntake.runVelocity(0), sIntake::stop, sIntake)); - - // driverController // TODO Reverse intake needed, also it stops randomly after a bit, get rid - // of - // .x() - // .whileTrue( - // Commands.startEnd( - // () -> sIntake.runVelocity(-Constants.IntakeConstants.velocity), - // sIntake::stop, - // sIntake)); - - // driverController - // .b() - // .whileFalse(Commands.startEnd(() -> sIntake.runVelocity(0), sIntake::stop, sIntake)); - driverController .start() .whileTrue(Commands.startEnd(() -> sDrive.resetRotation(), sDrive::stop, sDrive)); @@ -353,23 +313,26 @@ private void configureButtonBindings() { .rightTrigger() .whileTrue(Commands.startEnd(() -> sDrive.toggleSlowMode(), sDrive::stop, sDrive)); - // Operator controller configurations + // # Operator controller configuration + + // Arm Controls (Controlled by right stick Y-axis) sArm.setDefaultCommand(ArmCommands.joystickDrive(sArm, () -> -operatorController.getRightY())); + // Arm Positions + operatorController.povDown().whileTrue(ShootFar.run(sArm)); + operatorController.povRight().whileTrue(ShootMedium.run(sArm)); + operatorController.povUp().whileTrue(ShootClose.run(sArm)); + + // operatorController.y().whileTrue(Upwards.run(sArm)); //Note that Y is taken + operatorController.x().whileTrue(AmpShot.run(sArm)); + operatorController.a().whileTrue(FloorPickup.run(sArm)); + operatorController.b().whileTrue(StowPosition.run(sArm)); + + // Intake Controls (Three ways: #1 right trigger, which goes backwards, #2 “Y”, full power, and + // #3 is left trigger, which has sensor logic) sIntake.setDefaultCommand( - IntakeShooterControls.intakeShooterDrive( - sIntake, - sFlywheel, - sArm, - () -> operatorController.getLeftTriggerAxis(), - () -> operatorController.getRightTriggerAxis(), - () -> operatorController.leftBumper().getAsBoolean(), - () -> operatorController.rightBumper().getAsBoolean())); - // repurposed for intake override - // operatorController - // .a() - // .whileTrue( - // Commands.startEnd(() -> sClimber.runTargetPosition(0), sClimber::stop, sClimber)); + IntakeShooterControls.intakeShooterDefaultCommand( + sIntake, () -> operatorController.getRightTriggerAxis())); operatorController .y() @@ -379,6 +342,25 @@ private void configureButtonBindings() { sIntake::stop, sIntake)); + operatorController.leftTrigger().whileTrue(new IntakeNoteAndAlign(sIntake)); + // operatorController.leftTrigger().whileTrue(new IntakeNote(sIntake)); + + // Flywheel commands + operatorController.rightBumper().whileTrue(new ShootNote(sIntake, sFlywheel, sArm)); + operatorController.leftBumper().whileTrue(new PreRunShooter(sFlywheel)); + sFlywheel.setDefaultCommand( + new PreRunShooter(sFlywheel, true)); // Runs the flywheel slowly at all times + + // Climber controls (The first one is 90% probably the one that works.) + // sClimber.setDefaultCommand( + // ClimberCommands.buttonDrive( + // sClimber, operatorController.leftBumper(), operatorController.rightBumper())); + + // operatorController + // .a() + // .whileTrue( + // Commands.startEnd(() -> sClimber.runTargetPosition(0), sClimber::stop, sClimber)); + // operatorController // .b() // .whileTrue( @@ -387,29 +369,17 @@ private void configureButtonBindings() { // sClimber::stop, // sClimber)); - operatorController.povDown().whileTrue(ShootFar.run(sArm)); - operatorController.povRight().whileTrue(ShootMedium.run(sArm)); - operatorController.povUp().whileTrue(ShootClose.run(sArm)); - - operatorController.rightBumper().whileTrue(new ShootNote(sIntake, sFlywheel, sArm)); - operatorController.leftBumper().whileTrue(new PreRunShooter(sIntake, sFlywheel)); - - // run straight up position when y is pressed on operator. Using command Upwards - - operatorController.leftTrigger().whileTrue(new IntakeNoteAndAlign(sIntake)); - - // ! tempory testing keybinds - // operatorController.y().whileTrue(Upwards.run(sArm)); - // operatorController.y().whileTrue(new IntakeNote(sIntake, sArm, sFlywheel)); - // operatorController.y().whileTrue(new ShootNote(sIntake, sArm, sFlywheel)); - - // run straight forwards position when x is pressed - operatorController.x().whileTrue(AmpShot.run(sArm)); - // operatorController.x().whileTrue(new AdjustNote(sIntake, sArm, sFlywheel)); - - operatorController.a().whileTrue(FloorPickup.run(sArm)); - - operatorController.b().whileTrue(StowPosition.run(sArm)); + // driverController + // .povUp() + // .whileTrue( + // Commands.startEnd( + // () -> ClimberCommands.buttonDrive(sClimber, () -> 1), sClimber::stop, sClimber)); + // driverController + // .povDown() + // .whileTrue( + // Commands.startEnd( + // () -> ClimberCommands.buttonDrive(sClimber, () -> -1), sClimber::stop, + // sClimber)); } /** diff --git a/src/main/java/frc/robot/commands/ControlCommands/ClimberCommands.java b/src/main/java/frc/robot/commands/ControlCommands/ClimberCommands.java new file mode 100644 index 0000000..1e33bd6 --- /dev/null +++ b/src/main/java/frc/robot/commands/ControlCommands/ClimberCommands.java @@ -0,0 +1,53 @@ +// Copyright 2021-2024 FRC 6328, FRC 5829 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.commands.ControlCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.climber.Climber; + +public class ClimberCommands { + + private static final double MAX_MS = 0.01; + + private ClimberCommands() {} + + /** Wrist command using one axis of a joystick (controlling wrist velocity). */ + public static Command buttonDrive(Climber climber, Trigger leftTrigger, Trigger rightTrigger) { + return Commands.run( + () -> { + boolean left = leftTrigger.getAsBoolean(); + boolean right = rightTrigger.getAsBoolean(); + double magnitude = 0; + + if (left && right) { // there's probably a better way of doing this + magnitude = 0; + } else if ((left) && (!right)) { + magnitude = 1; + } else if ((!left) && (right)) { + magnitude = -1; + } else if ((!left) && (!right)) { + magnitude = 0; + } + + // Calcaulate new rotational velocity + double position = magnitude * MAX_MS; + + // Send command to wrist subsystem to run wrist + climber.runTargetPosition(position); + }, + climber); + } +} diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java index a6a1256..de52918 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java @@ -69,7 +69,7 @@ public static Command joystickDrive( linearMagnitude = linearMagnitude * linearMagnitude; omega = Math.copySign(omega * omega, omega); - // Calcaulate new linear velocity + // Calculate new linear velocity Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection) .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) @@ -81,7 +81,11 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), 1.25 * omega * drive.getMaxAngularSpeedRadPerSec() / 2.3, - drive.getRotation())); + drive.getRotation() //.plus(new Rotation2d(Math.PI)) + // isFlipped + // ? drive.getRotation().plus(new Rotation2d(Math.PI)) + // : drive.getRotation() + )); }, drive); } diff --git a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java index e817ba8..9067d02 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java +++ b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java @@ -15,59 +15,22 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.flywheel.Flywheel; import frc.robot.subsystems.intake.Intake; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; public class IntakeShooterControls { private static final double DEADBAND = 0.3; - private static final double MAXINCHESPERSECOND = 3; - private IntakeShooterControls() {} + public static Command intakeShooterDefaultCommand( + Intake intake, DoubleSupplier rightTriggerSupplier) { - public static Command intakeShooterDrive( - Intake intake, - Flywheel flywheel, - Arm arm, - DoubleSupplier leftTriggerSupplier, - DoubleSupplier rightTriggerSupplier, - BooleanSupplier leftBumperSupplier, - BooleanSupplier rightBumperSupplier) { - // Left trigger runs outtake, right trigger triggers intake, and right bumper triggers the - // shooter (and the intake once at velocity). - // Intake stops on detection of note in first mode. return Commands.run( () -> { - /* - if (leftBumperSupplier.getAsBoolean()) { - // flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity); - } - if (rightBumperSupplier.getAsBoolean()) { - - } else { - - // double fwdSpeed = leftTriggerSupplier.getAsDouble(); - double revSpeed = rightTriggerSupplier.getAsDouble(); - // fwdSpeed = MathUtil.applyDeadband(fwdSpeed, DEADBAND); - // revSpeed = MathUtil.applyDeadband(revSpeed, DEADBAND); - - double stickMagnitude = -revSpeed; - stickMagnitude = - stickMagnitude * stickMagnitude * stickMagnitude; // more control over lower speeds - - if (Math.abs(revSpeed) > DEADBAND) { - intake.runPercentSpeed(Constants.IntakeConstants.percentPower * stickMagnitude); - } - } - */ if (Math.abs(rightTriggerSupplier.getAsDouble()) > DEADBAND) { intake.runPercentSpeed( -Constants.IntakeConstants.percentPower * rightTriggerSupplier.getAsDouble()); } }, - intake, - flywheel); + intake); } } diff --git a/src/main/java/frc/robot/commands/IntakeNote.java b/src/main/java/frc/robot/commands/IntakeNote.java index c9d37d2..14036bc 100644 --- a/src/main/java/frc/robot/commands/IntakeNote.java +++ b/src/main/java/frc/robot/commands/IntakeNote.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.intake.Intake; // Moves the note so that it is detected by the conveySensor but not shooterSensor @@ -22,19 +21,12 @@ public class IntakeNote extends Command { private Intake intake; - private Drive drive; public IntakeNote(Intake intake) { this.intake = intake; addRequirements(intake); } - public IntakeNote(Intake intake, Drive drive) { // TODO move forward and back - this.intake = intake; - this.drive = drive; - addRequirements(intake, drive); - } - // Called once at the beginning @Override public void initialize() {} diff --git a/src/main/java/frc/robot/commands/PreRunShooter.java b/src/main/java/frc/robot/commands/PreRunShooter.java index 8fa8cfd..11f8b68 100644 --- a/src/main/java/frc/robot/commands/PreRunShooter.java +++ b/src/main/java/frc/robot/commands/PreRunShooter.java @@ -15,18 +15,21 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.flywheel.Flywheel; -import frc.robot.subsystems.intake.Intake; // Moves the note so that it is detected by the conveySensor but not shooterSensor public class PreRunShooter extends Command { - private Intake intake; private Flywheel flywheel; + private boolean slowerDefault; - public PreRunShooter(Intake intake, Flywheel flywheel) { - this.intake = intake; + public PreRunShooter(Flywheel flywheel) { + this(flywheel, false); + } + + public PreRunShooter(Flywheel flywheel, boolean slowRun) { this.flywheel = flywheel; - addRequirements(intake, flywheel); + this.slowerDefault = slowRun; + addRequirements(flywheel); } // Called once at the beginning @@ -36,8 +39,11 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity); + if (!slowerDefault) { + flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity); + } else if (slowerDefault) { + flywheel.runVelocity(-Constants.FlywheelConstants.slowShootingVelocity); + } } // Called once the command ends or is interrupted. @@ -45,7 +51,6 @@ public void execute() { public void end(boolean interrupted) { flywheel.runVelocity(0); flywheel.stop(); - intake.runPercentSpeed(0); } @Override diff --git a/src/main/java/frc/robot/commands/ShootNote.java b/src/main/java/frc/robot/commands/ShootNote.java index 086cb0e..61a353b 100644 --- a/src/main/java/frc/robot/commands/ShootNote.java +++ b/src/main/java/frc/robot/commands/ShootNote.java @@ -43,7 +43,7 @@ public void execute() { double topFlywheelRPM = -flywheel.getVelocityRPMBottom(); double targetRPM = Constants.FlywheelConstants.shootingVelocity; - if (arm.getPosition() > 1) { + if (arm.getAngleRad() > 1) { targetRPM = Constants.FlywheelConstants.shootingVelocity / AmpReductionFactor; } flywheel.runVelocity(-targetRPM); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 6331e31..ebb1cc8 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -106,8 +106,8 @@ public double getVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); } - @AutoLogOutput(key = "Arm/PositionRad") - public double getPosition() { + @AutoLogOutput(key = "Arm/AngleRad") + public double getAngleRad() { return inputs.positionRad; } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index c050fa9..9f39e5f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -17,11 +17,11 @@ public interface ArmIO { @AutoLog public static class ArmIOInputs { - public double positionRad = 0.0; + public double positionRad; public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; public double[] currentAmps = new double[] {}; - public double targetPositionRad = 0.0; + public double targetPositionRad; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index 76344fc..07f7932 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -13,9 +13,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; @@ -34,12 +32,12 @@ public class ArmIOSparkMax implements ArmIO { private final CANSparkMax rightMotor = new CANSparkMax(ArmConstants.kRightArmMotorId, MotorType.kBrushless); - private final RelativeEncoder leftRelativeEncoder = leftMotor.getEncoder(); + // private final RelativeEncoder leftRelativeEncoder = leftMotor.getEncoder(); private final SparkAbsoluteEncoder leftAbsoluteEncoder = leftMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); - private final SparkPIDController pid = leftMotor.getPIDController(); + // private final SparkPIDController pid = leftMotor.getPIDController(); private final PIDController mathPid; @@ -49,7 +47,7 @@ public class ArmIOSparkMax implements ArmIO { private double calculatedPID; - // REMEMBER + // REMEMBER THE ALAMO public ArmIOSparkMax() { // leftMotor.restoreFactoryDefaults(); @@ -136,6 +134,7 @@ public void setVelocity(double velocityRadPerSec, double ffVolts) { } @Override + /** Set the target angle. In radians. */ public void setTargetAngle(double angle) { targetAngle = MathUtil.clamp(angle, ArmConstants.minimumAngle, ArmConstants.maximumAngle); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java index 391217c..bad2f7e 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java @@ -17,8 +17,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import frc.robot.Constants.ClimberConstants; /** @@ -39,10 +39,13 @@ public class ClimberIOSparkMax implements ClimberIO { private final SparkPIDController leftPID = leftMotor.getPIDController(); private final SparkPIDController rightPID = rightMotor.getPIDController(); + private final PIDController leftMathPID; + private final PIDController rightMathPID; + private double targetPosition = ClimberConstants.initialPosition; - private double targetPositionLeft = ClimberConstants.initialPosition; - private double targetPositionRight = ClimberConstants.initialPosition; + // private double targetPositionLeft = ClimberConstants.initialPosition; + // private double targetPositionRight = ClimberConstants.initialPosition; public ClimberIOSparkMax() { leftMotor.restoreFactoryDefaults(); @@ -58,6 +61,9 @@ public ClimberIOSparkMax() { // leftMotor.burnFlash(); // rightMotor.burnFlash(); + + leftMathPID = new PIDController(ClimberConstants.kP, ClimberConstants.kI, ClimberConstants.kD); + rightMathPID = new PIDController(ClimberConstants.kP, ClimberConstants.kI, ClimberConstants.kD); } @Override @@ -87,47 +93,68 @@ public void setVoltage(double volts) { @Override public void setTargetPosition(double position) { + // targetPosition = + // MathUtil.clamp(targetPosition + position, ClimberConstants.minPosition, + // ClimberConstants.maxPosition); + + // leftPID.setReference( + // (position / ClimberConstants.gearCircumfrence) * GEAR_RATIO, + // ControlType.kPosition, + // 0, + // 0, + // ArbFFUnits.kVoltage); + + // rightPID.setReference( + // (position / ClimberConstants.gearCircumfrence) * GEAR_RATIO, + // ControlType.kPosition, + // 0, + // 0, + // ArbFFUnits.kVoltage); targetPosition = - MathUtil.clamp(targetPosition, ClimberConstants.minPosition, ClimberConstants.maxPosition); - - leftPID.setReference( - (position / ClimberConstants.gearCircumfrence) * GEAR_RATIO, - ControlType.kPosition, - 0, - 0, - ArbFFUnits.kVoltage); - - rightPID.setReference( - (position / ClimberConstants.gearCircumfrence) * GEAR_RATIO, - ControlType.kPosition, - 0, - 0, - ArbFFUnits.kVoltage); + MathUtil.clamp( + targetPosition + position, ClimberConstants.minPosition, ClimberConstants.maxPosition); + + leftPID.setReference(targetPosition, ControlType.kPosition, 0, 0); + rightPID.setReference(targetPosition, ControlType.kPosition, 0, 0); + + double leftPosition = leftRelativeEncoder.getPosition(); + double rightPosition = rightRelativeEncoder.getPosition(); + + leftMotor.set( + leftMathPID.calculate( + leftPosition, (targetPosition / ClimberConstants.gearCircumfrence) * GEAR_RATIO)); + // + Constants.FlywheelConstants.kv * targetPosition + // + Constants.FlywheelConstants.ks); + rightMotor.set( + leftMathPID.calculate( + rightPosition, (targetPosition / ClimberConstants.gearCircumfrence) * GEAR_RATIO)); + // + Constants.FlywheelConstants.kv * velocityRevPerSec + // + Constants.FlywheelConstants.ks); } - @Override - public void setTargetPosition(double positionLeft, double positionRight) { - targetPositionLeft = - MathUtil.clamp(positionLeft, ClimberConstants.minPosition, ClimberConstants.maxPosition); - ; - targetPositionRight = - MathUtil.clamp(positionRight, ClimberConstants.minPosition, ClimberConstants.maxPosition); - ; - - leftPID.setReference( - (positionLeft / ClimberConstants.gearCircumfrence) * GEAR_RATIO, - ControlType.kPosition, - 0, - 0, - ArbFFUnits.kVoltage); - - rightPID.setReference( - (positionRight / ClimberConstants.gearCircumfrence) * GEAR_RATIO, - ControlType.kPosition, - 0, - 0, - ArbFFUnits.kVoltage); - } + // @Override + // public void setTargetPosition(double positionLeft, double positionRight) { + // targetPositionLeft = + // MathUtil.clamp(positionLeft, ClimberConstants.minPosition, ClimberConstants.maxPosition); + + // targetPositionRight = + // MathUtil.clamp(positionRight, ClimberConstants.minPosition, + // ClimberConstants.maxPosition); + + // leftPID.setReference( + // (positionLeft / ClimberConstants.gearCircumfrence) * GEAR_RATIO, + // ControlType.kPosition, + // 0, + // 0, + // ArbFFUnits.kVoltage); + + // rightPID.setReference( + // (positionRight / ClimberConstants.gearCircumfrence) * GEAR_RATIO, + // ControlType.kPosition, + // 0, + // 0, + // ArbFFUnits.kVoltage); + // } @Override public void stop() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c9c83fc..3ccad47 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -50,7 +50,7 @@ public class Drive extends SubsystemBase { private static final double FAST_MAX_LINEAR_SPEED = Units.feetToMeters(14.5); private static final double FAST_MAX_ANGULAR_SPEED = FAST_MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; - private static final double SLOW_MAX_LINEAR_SPEED = Units.feetToMeters(14.5); + private static final double SLOW_MAX_LINEAR_SPEED = Units.feetToMeters(4.5); private static final double SLOW_MAX_ANGULAR_SPEED = SLOW_MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; private static double CurrentMaxAngularSpeed = FAST_MAX_ANGULAR_SPEED; diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java index 00517aa..a3c710b 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java @@ -34,12 +34,12 @@ will need to be treated differently (because if they were, they could rip the no new CANSparkMax(FlywheelConstants.kTopFlywheelSparkMaxCanId, MotorType.kBrushless); private final RelativeEncoder topShooterEncoder = topShooterMotor.getEncoder(); private final SparkPIDController topShooterPID = topShooterMotor.getPIDController(); + private final PIDController topMotorPID; private final CANSparkMax bottomShooterMotor = new CANSparkMax(FlywheelConstants.kBottomFlywheelSparkMaxCanId, MotorType.kBrushless); private final RelativeEncoder bottomShooterEncoder = bottomShooterMotor.getEncoder(); private final SparkPIDController bottomShooterPID = bottomShooterMotor.getPIDController(); - private final PIDController topMotorPID; private final PIDController bottomMotorPID; private double targetVelocity; @@ -71,13 +71,13 @@ public FlywheelIOSparkMax() { @Override public void updateInputs(FlywheelIOInputs inputs) { - inputs.positionRadTop = topShooterEncoder.getPosition(); - inputs.velocityRadPerSecTop = topShooterEncoder.getVelocity(); + inputs.positionRadTop = topShooterEncoder.getPosition() / GEAR_RATIO; + inputs.velocityRadPerSecTop = topShooterEncoder.getVelocity() / GEAR_RATIO; inputs.appliedVoltsTop = topShooterMotor.getAppliedOutput() * topShooterMotor.getBusVoltage(); inputs.currentAmpsTop = new double[] {topShooterMotor.getOutputCurrent()}; inputs.positionRadBottom = bottomShooterEncoder.getPosition() / GEAR_RATIO; - inputs.velocityRadPerSecBottom = bottomShooterEncoder.getVelocity(); + inputs.velocityRadPerSecBottom = bottomShooterEncoder.getVelocity() / GEAR_RATIO; inputs.appliedVoltsBottom = bottomShooterMotor.getAppliedOutput() * bottomShooterMotor.getBusVoltage(); inputs.currentAmpsBottom = new double[] {bottomShooterMotor.getOutputCurrent()}; @@ -85,8 +85,8 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setVoltage(double volts) { - // topShooterMotor.setVoltage(volts); - // bottomShooterMotor.setVoltage(volts); + topShooterMotor.setVoltage(volts); + bottomShooterMotor.setVoltage(volts); } @AutoLogOutput(key = "Flywheel/targetVelocity") diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 0d975b0..809d5d7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -43,5 +43,4 @@ public default void runFull() {} public default boolean getIsStalled() { return false; } - ; } From 82faba404f5cc7e2bf42e4988b6e49a9b7d40e8f Mon Sep 17 00:00:00 2001 From: 24cirinom <112493487+24cirinom@users.noreply.github.com> Date: Sat, 30 Mar 2024 11:47:43 -0500 Subject: [PATCH 2/3] swiched motor --- build.gradle | 6 +++++- src/main/java/frc/robot/commands/IntakeNote.java | 1 + .../java/frc/robot/commands/IntakeNoteAndAlign.java | 1 + src/main/java/frc/robot/commands/PreRunShooter.java | 1 + src/main/java/frc/robot/subsystems/arm/Arm.java | 1 + .../frc/robot/subsystems/drive/ModuleIOSparkMax.java | 12 +++++++++++- .../java/frc/robot/subsystems/sticks/Sticks.java | 1 + 7 files changed, 21 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index 5fee2bc..052d1f1 100644 --- a/build.gradle +++ b/build.gradle @@ -113,7 +113,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE diff --git a/src/main/java/frc/robot/commands/IntakeNote.java b/src/main/java/frc/robot/commands/IntakeNote.java index ce86573..a109b23 100644 --- a/src/main/java/frc/robot/commands/IntakeNote.java +++ b/src/main/java/frc/robot/commands/IntakeNote.java @@ -4,6 +4,7 @@ import frc.robot.Constants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.intake.Intake; + // Moves the note so that it is detected by the conveySensor but not shooterSensor public class IntakeNote extends Command { diff --git a/src/main/java/frc/robot/commands/IntakeNoteAndAlign.java b/src/main/java/frc/robot/commands/IntakeNoteAndAlign.java index 9c54618..18f12be 100644 --- a/src/main/java/frc/robot/commands/IntakeNoteAndAlign.java +++ b/src/main/java/frc/robot/commands/IntakeNoteAndAlign.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.intake.Intake; + // Moves the note so that it is detected by the conveySensor but not shooterSensor public class IntakeNoteAndAlign extends Command { diff --git a/src/main/java/frc/robot/commands/PreRunShooter.java b/src/main/java/frc/robot/commands/PreRunShooter.java index ea1e7d2..dae9cfc 100644 --- a/src/main/java/frc/robot/commands/PreRunShooter.java +++ b/src/main/java/frc/robot/commands/PreRunShooter.java @@ -27,6 +27,7 @@ public void execute() { flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity); } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 07f5273..c312c98 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -75,6 +75,7 @@ public void runVelocity(double velocityRPM) { public boolean getIsFinished() { return io.getIsFinished(); } + /** * @param rightPosition */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 9d5164a..ca541ce 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -51,6 +51,7 @@ public class ModuleIOSparkMax implements ModuleIO { private final AnalogInput turnAbsoluteEncoder; private final Queue drivePositionQueue; private final Queue turnPositionQueue; + public boolean isDriveMotorInverted = false; private final SparkAbsoluteEncoder turnAbsoluteEncoderNew; @@ -89,8 +90,10 @@ public ModuleIOSparkMax(int index) { new CANSparkMax(DriveConstants.kRearRightDrivingCanId, MotorType.kBrushless); turnSparkMax = new CANSparkMax(DriveConstants.kRearRightTurningCanId, MotorType.kBrushless); turnAbsoluteEncoder = new AnalogInput(3); + isDriveMotorInverted = true; absoluteEncoderOffset = - new Rotation2d(Math.PI * 3 / 2 + Math.PI + Math.PI + Math.PI); // MUST BE CALIBRATED + new Rotation2d(Math.PI * 3 / 2 + Math.PI + Math.PI); // MUST BE CALIBRATED + break; default: throw new RuntimeException("Invalid module index"); @@ -102,6 +105,13 @@ public ModuleIOSparkMax(int index) { driveSparkMax.setCANTimeout(250); turnSparkMax.setCANTimeout(250); + if (isDriveMotorInverted) { + driveSparkMax.setInverted(true); + }else{ + driveSparkMax.setInverted(false); + + } + driveEncoder = driveSparkMax.getEncoder(); turnRelativeEncoder = turnSparkMax.getEncoder(); turnAbsoluteEncoderNew = turnSparkMax.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); diff --git a/src/main/java/frc/robot/subsystems/sticks/Sticks.java b/src/main/java/frc/robot/subsystems/sticks/Sticks.java index af006c4..b13cee8 100644 --- a/src/main/java/frc/robot/subsystems/sticks/Sticks.java +++ b/src/main/java/frc/robot/subsystems/sticks/Sticks.java @@ -23,6 +23,7 @@ public class Sticks extends SubsystemBase { private final SticksIO io; private final SticksIOInputsAutoLogged inputs = new SticksIOInputsAutoLogged(); + // private final SimpleMotorFeedforward ffModel; /** Creates a new Sticks. */ From e8eda665a6d0d6fecfae1355332e6d7360e42a25 Mon Sep 17 00:00:00 2001 From: 24cirinom <112493487+24cirinom@users.noreply.github.com> Date: Sat, 30 Mar 2024 13:23:48 -0500 Subject: [PATCH 3/3] tunes --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 2 ++ src/main/java/frc/robot/commands/AdjustNote.java | 4 ++-- .../java/frc/robot/commands/Positions/FloorPickup.java | 2 +- src/main/java/frc/robot/commands/ShootNote.java | 2 +- .../java/frc/robot/subsystems/drive/ModuleIOSparkMax.java | 5 ++--- .../frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java | 4 ++-- 7 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e14538..737c27b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -105,7 +105,7 @@ public static final class IntakeConstants { // Intake Feedforward characterization constants public static final double ks = 0.1; public static final double kv = 5; - public static double percentPower = 0.45 * 1.05; + public static double percentPower = 1; public static final int conveyorSensor = 0; public static final int shooterSensor = 1; @@ -122,10 +122,10 @@ public static final class ArmConstants { public static final double kMaxOutput = 0.33; // // Arm PID constants - public static final double kP = 0.7; + public static final double kP = 0.65; public static final double kI = 0.0; - public static final double kD = 0.025; - public static final double kWeightBasedFF = 0.025; + public static final double kD = 0.035; + public static final double kWeightBasedFF = 0.025 * 1.3 * 1.15; // // Arm PID constants // public static final double kP = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 44b0aec..9d1daf7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -300,6 +300,8 @@ private void configureButtonBindings() { () -> -driverController.getLeftX(), () -> -driverController.getRightX())); + + // driverController // .a() // .whileTrue( diff --git a/src/main/java/frc/robot/commands/AdjustNote.java b/src/main/java/frc/robot/commands/AdjustNote.java index 05d95f9..45749d9 100644 --- a/src/main/java/frc/robot/commands/AdjustNote.java +++ b/src/main/java/frc/robot/commands/AdjustNote.java @@ -21,8 +21,8 @@ public class AdjustNote extends Command { private Intake intake; private int phase = 1; // 1 is moving note to shootersensor, 2 is moving note away from shooterSensor, 3 = done - private double forwardsIntakeSpeed = 0.04; - private double backwardsIntakeSpeed = 0.04; + private double forwardsIntakeSpeed = 1; + private double backwardsIntakeSpeed = 1; private long phase2StartTime; diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickup.java b/src/main/java/frc/robot/commands/Positions/FloorPickup.java index 36c21e1..1b96f58 100644 --- a/src/main/java/frc/robot/commands/Positions/FloorPickup.java +++ b/src/main/java/frc/robot/commands/Positions/FloorPickup.java @@ -22,7 +22,7 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 0.135; + double ARMANGLE = 0.09; arm.runTargetAngle(ARMANGLE); }, diff --git a/src/main/java/frc/robot/commands/ShootNote.java b/src/main/java/frc/robot/commands/ShootNote.java index 086cb0e..639ff9f 100644 --- a/src/main/java/frc/robot/commands/ShootNote.java +++ b/src/main/java/frc/robot/commands/ShootNote.java @@ -49,7 +49,7 @@ public void execute() { flywheel.runVelocity(-targetRPM); if (Math.abs(topFlywheelRPM) > Math.abs(targetRPM * 0.9)) { - intake.runPercentSpeed(0.5); + intake.runPercentSpeed(1); } if (!intake.getConveyerProximity() && !intake.getShooterProximity()) { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 5a058b5..dd17eca 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -106,9 +106,8 @@ public ModuleIOSparkMax(int index) { if (isDriveMotorInverted) { driveSparkMax.setInverted(true); - }else{ - driveSparkMax.setInverted(false); - + } else { + driveSparkMax.setInverted(false); } driveEncoder = driveSparkMax.getEncoder(); diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java index 00517aa..1452991 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java @@ -51,8 +51,8 @@ public FlywheelIOSparkMax() { topShooterMotor.setCANTimeout(250); bottomShooterMotor.setCANTimeout(250); - topShooterMotor.setInverted(false); - bottomShooterMotor.setInverted(false); + topShooterMotor.setInverted(true); + bottomShooterMotor.setInverted(true); topShooterMotor.setSmartCurrentLimit(30); bottomShooterMotor.setSmartCurrentLimit(30);