From 7ffa72609b5c47bdf1fbc6cbd48c5b67465d9949 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Mon, 6 Jan 2025 13:11:28 -0500 Subject: [PATCH 01/18] Add ThriftyLib --- simgui.json | 3 +++ .../autonomous/tasks/DriveTrajectoryTask.java | 4 ++-- vendordeps/ThriftyLib.json | 20 +++++++++++++++++++ 3 files changed, 25 insertions(+), 2 deletions(-) create mode 100644 vendordeps/ThriftyLib.json diff --git a/simgui.json b/simgui.json index a0e69b1..5aa4636 100644 --- a/simgui.json +++ b/simgui.json @@ -28,5 +28,8 @@ "open": true } } + }, + "NetworkTables Info": { + "visible": true } } diff --git a/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java b/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java index 3a8782e..fc7f923 100644 --- a/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java +++ b/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java @@ -43,14 +43,14 @@ public DriveTrajectoryTask(String pathName) { filePath = Filesystem.getDeployDirectory().toPath().resolve("paths/" + pathName); } else { RobotTelemetry.print("Running in simulation!"); - filePath = Filesystem.getLaunchDirectory().toPath().resolve("PathWeaver/output/" + pathName); + filePath = Filesystem.getDeployDirectory().toPath().resolve("pathplanner/paths/" + pathName); } RobotTelemetry.print("Loading path from:\n" + filePath.toString()); m_autoPath = PathPlannerPath.fromPathFile(pathName); // RobotTelemetry.print(m_autoPath.numPoints()); - if (DriverStation.getAlliance().get() == Alliance.Red) { + if (DriverStation.getAlliance().orElse(Alliance.Red) == Alliance.Red) { RobotTelemetry.print("Translating path for Red Alliance!"); m_autoPath = m_autoPath.flipPath(); } diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json new file mode 100644 index 0000000..0098894 --- /dev/null +++ b/vendordeps/ThriftyLib.json @@ -0,0 +1,20 @@ +{ + "fileName": "ThriftyLib.json", + "name": "ThriftyLib", + "version": "2024.0.0", + "frcYear": "2024", + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "mavenUrls": [ + "https://docs.home.thethriftybot.com" + ], + "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", + "javaDependencies": [ + { + "groupId": "com.thethriftybot.frc", + "artifactId": "ThriftyLib-java", + "version": "2024.0.0" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} From e1a769e1121a71e6bcfa34363ea174c259282a9b Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Mon, 6 Jan 2025 14:07:18 -0500 Subject: [PATCH 02/18] Manual elevator working --- AdvantageScope.json | 65 ++++++- src/main/java/frc/robot/Constants.java | 47 ++++- src/main/java/frc/robot/Robot.java | 80 +++++---- .../controllers/DriverController.java | 57 +----- .../controllers/OperatorController.java | 65 ++++--- src/main/java/frc/robot/subsystems/Coral.java | 94 ++++++++++ .../java/frc/robot/subsystems/Elevator.java | 167 ++++++++++++++++++ .../java/frc/robot/subsystems/Shooter.java | 1 - .../java/frc/robot/subsystems/Subsystem.java | 14 +- 9 files changed, 451 insertions(+), 139 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Coral.java create mode 100644 src/main/java/frc/robot/subsystems/Elevator.java diff --git a/AdvantageScope.json b/AdvantageScope.json index 238eb49..c228d59 100644 --- a/AdvantageScope.json +++ b/AdvantageScope.json @@ -21,11 +21,14 @@ "/RealOutputs/Auto/DriveTrajectory/1-2-b", "/RealOutputs/Auto/DriveTrajectory/TargetPose/rotation", "/RealOutputs/Auto/DriveTrajectory/CurrentPose", - "/RealOutputs/Auto/DriveTrajectory/CurrentPose/rotation" + "/RealOutputs/Auto/DriveTrajectory/CurrentPose/rotation", + "/RealOutputs/Elevator", + "/RealOutputs/Elevator/Position", + "/RealOutputs/Elevator/Current" ] }, "tabs": { - "selected": 2, + "selected": 3, "tabs": [ { "type": 0, @@ -78,7 +81,7 @@ "cameraPosition": [ 8.846666903479488, 5.635495269604634, - -2.048242193592743 + -2.0482421935927437 ], "cameraTarget": [ 4.77960094630555, @@ -101,12 +104,12 @@ "fields": [ { "key": "/RealOutputs/Auto/DriveTrajectory/TargetPose/rotation/value", - "color": "#e5b31b", + "color": "#2b66a2", "show": true }, { "key": "/RealOutputs/Auto/DriveTrajectory/CurrentPose/rotation/value", - "color": "#af2437", + "color": "#e5b31b", "show": true } ] @@ -115,7 +118,7 @@ "fields": [ { "key": "/RealOutputs/Auto/DriveTrajectory/shouldReplan", - "color": "#2b66a2", + "color": "#af2437", "show": true } ] @@ -129,7 +132,55 @@ "fields": [] } }, - "title": "Line Graph" + "title": "PathPlanning" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Elevator/Position/Current", + "color": "#2b66a2", + "show": true + }, + { + "key": "/RealOutputs/Elevator/Position/Target", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Elevator/Current/Left", + "color": "#af2437", + "show": true + }, + { + "key": "/RealOutputs/Elevator/Current/Right", + "color": "#80588e", + "show": true + } + ] + } + }, + "title": "Elevator" }, { "type": 3, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 32416e7..534a296 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,6 +10,43 @@ public static class Robot { public static final double k_length = 28; // Inches } + public static class Elevator { + public static final int kElevatorLeftMotorId = 9; + public static final int kElevatorRightMotorId = 10; + + public static final double kP = 0.1; + public static final double kI = 1e-8; + public static final double kD = 1; + public static final double kIZone = 0; + public static final double kFF = 0.0; + + public static final int kMaxCurrent = 30; + public static final double kMaxPowerUp = 0.1; + public static final double kMaxPowerDown = 0.1; + + public static final double kStowHeight = 0.0; + public static final double kL1Height = 0.0; + public static final double kL2Height = 0.0; + public static final double kL3Height = 0.0; + public static final double kL4Height = 52.6; + public static final double kMaxHeight = 56.2; + public static final double kGroundAlgaeHeight = 0.0; + public static final double kScoreAlgaeHeight = 0.0; + public static final double kLowAlgaeHeight = 0.0; + public static final double kHighAlgaeHeight = 0.0; + } + + public static class Coral { + public static final int kLeftMotorId = 11; + public static final int kRightMotorId = 12; + } + + public static class Algae { + public static final int kWristMotorId = 13; + + public static final int kIntakeMotorId = 14; + } + public static class Intake { // Motors public static final int kIntakeMotorId = 9; @@ -69,12 +106,12 @@ public static class Intake { // Drivetrain public static class Drive { - public static final double kP = 0.00085;//3.6954;//0.085; - public static final double kI = 0.0;//0.0; - public static final double kD = 0.0;//0.0; + public static final double kP = 0.00085;// 3.6954;//0.085; + public static final double kI = 0.0;// 0.0; + public static final double kD = 0.0;// 0.0; - public static final double kS = 0.1695;//0.01; - public static final double kV = 2.8559;//2.6; + public static final double kS = 0.1695;// 0.01; + public static final double kV = 2.8559;// 2.6; public static final double kA = 0.4864; public static final int kFLMotorId = 8; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5ac4fab..1054c90 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -25,7 +25,9 @@ import frc.robot.controls.controllers.OperatorController; import frc.robot.simulation.Field; import frc.robot.subsystems.Compressor; +import frc.robot.subsystems.Coral; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Subsystem; import frc.robot.subsystems.leds.LEDs; @@ -52,8 +54,10 @@ public class Robot extends LoggedRobot { // private final Intake m_intake = Intake.getInstance(); private final Compressor m_compressor = Compressor.getInstance(); private final Drivetrain m_drive = Drivetrain.getInstance(); + private final Coral m_coral = Coral.getInstance(); // private final Shooter m_shooter = Shooter.getInstance(); - // private final Climber m_climber = Climber.getInstance(); + private final Elevator m_elevator = Elevator.getInstance(); + public final LEDs m_leds = LEDs.getInstance(); // Auto stuff @@ -71,12 +75,12 @@ public void robotInit() { setupLogging(); // Add all subsystems to the list - // m_allSubsystems.add(m_intake); - m_allSubsystems.add(m_compressor); - m_allSubsystems.add(m_drive); - // m_allSubsystems.add(m_shooter); - // m_allSubsystems.add(m_climber); - m_allSubsystems.add(m_leds); + // m_allSubsystems.add(m_compressor); + // m_allSubsystems.add(m_drive); + // m_allSubsystems.add(m_coral); + m_allSubsystems.add(m_elevator); + + // m_allSubsystems.add(m_leds); // Set up the Field2d object for simulation SmartDashboard.putData("Field", m_field); @@ -92,7 +96,7 @@ public void robotPeriodic() { updateSim(); // Used by sysid - if (this.isTestEnabled()){ + if (this.isTestEnabled()) { CommandScheduler.getInstance().run(); } } @@ -152,50 +156,62 @@ public void teleopPeriodic() { m_drive.drive(xSpeed, rot); + if (m_driverController.getWantsCoralEject()) { + // System.out.println("HERE"); + m_coral.setSpeed(60); + } else { + m_coral.setSpeed(0); + } + + m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); + // // Shooter variable speed - // if (m_driverController.getWantsMoreSpeed() || m_operatorController.getWantsMoreSpeed()) { - // m_leds.setColor(Color.kPink); - // speed = 3000; - // } else if (m_driverController.getWantsLessSpeed() || m_operatorController.getWantsLessSpeed()) { - // m_leds.setColor(Color.kOrange); - // speed = 430; - // } else if (m_driverController.getWantsShooterStop() || m_operatorController.getWantsShooterStop()) { - // m_leds.defaultLEDS(); - // speed = 0; + // if (m_driverController.getWantsMoreSpeed() || + // m_operatorController.getWantsMoreSpeed()) { + // m_leds.setColor(Color.kPink); + // speed = 3000; + // } else if (m_driverController.getWantsLessSpeed() || + // m_operatorController.getWantsLessSpeed()) { + // m_leds.setColor(Color.kOrange); + // speed = 430; + // } else if (m_driverController.getWantsShooterStop() || + // m_operatorController.getWantsShooterStop()) { + // m_leds.defaultLEDS(); + // speed = 0; // } // speed = MathUtil.clamp(speed, -6000, 10000); // m_shooter.setSpeed(speed); // // Intake // if (m_driverController.getWantsFullIntake()) { - // m_intake.goToGround(); + // m_intake.goToGround(); // } else if (m_driverController.getWantsIntake()) { - // if (m_intake.getIntakeHasNote()) { - // m_intake.pulse(); - // } else { - // m_intake.intake(); - // } + // if (m_intake.getIntakeHasNote()) { + // m_intake.pulse(); + // } else { + // m_intake.intake(); + // } // } else if (m_driverController.getWantsEject()) { - // m_intake.eject(); + // m_intake.eject(); // } else if (m_driverController.getWantsSource()) { - // m_intake.goToSource(); + // m_intake.goToSource(); // } else if (m_driverController.getWantsStow()) { - // m_intake.goToStow(); + // m_intake.goToStow(); // } else if (m_intake.getIntakeState() != IntakeState.INTAKE) { - // m_intake.stopIntake(); + // m_intake.stopIntake(); // } // // Climber // if (m_operatorController.getWantsClimberClimb()) { - // m_climber.climb(); + // m_climber.climb(); // } else if (m_operatorController.getWantsClimberRelease()) { - // m_climber.release(); + // m_climber.release(); // } else if (m_operatorController.getWantsClimberTiltLeft()) { - // m_climber.tiltLeft(); + // m_climber.tiltLeft(); // } else if (m_operatorController.getWantsClimberTiltRight()) { - // m_climber.tiltRight(); + // m_climber.tiltRight(); // } else { - // m_climber.stopClimber(); + // m_climber.stopClimber(); // } // if (m_operatorController.getWantsBrakeMode()) { diff --git a/src/main/java/frc/robot/controls/controllers/DriverController.java b/src/main/java/frc/robot/controls/controllers/DriverController.java index cc10f25..24aca79 100644 --- a/src/main/java/frc/robot/controls/controllers/DriverController.java +++ b/src/main/java/frc/robot/controls/controllers/DriverController.java @@ -24,46 +24,6 @@ public double getTurnAxis() { return -this.getFilteredAxis(4); } - public double getShooterAxis() { - return (this.getFilteredAxis(3) - 0.5) * 2; - } - - public boolean getWantsFullIntake() { - return this.getRawButton(1); - } - - public boolean getWantsIntake() { - return this.getRawButton(6); - } - - public boolean getWantsEject() { - return this.getRawButton(2); - } - - public boolean getWantsSource() { - return this.getRawButton(3); - } - - public boolean getWantsStow() { - return this.getRawButton(4); - } - - // public boolean getWantsFire() { - // return this.getHatUpPressed(); - // } - - public boolean getWantsMoreSpeed() { - return this.getHatUp(); - } - - public boolean getWantsLessSpeed() { - return this.getHatDown(); - } - - public boolean getWantsShooterStop() { - return this.getRawButton(5); - } - public boolean getWantsSpeedMode() { return this.getFilteredAxis(2) > k_triggerActivationThreshold; } @@ -72,23 +32,10 @@ public boolean getWantsSlowMode() { return this.getFilteredAxis(3) > k_triggerActivationThreshold; } - // Buttons - public boolean getWantsSomethingToggle() { - return this.getRawButtonPressed(1); - } - - public boolean getWantsSomething2Toggle() { - return this.getRawButtonPressed(3); - } - - public boolean getWantsResetGyro() { - return this.getRawButton(4); + public boolean getWantsCoralEject() { + return this.getRawButton(1); } - // public boolean getWantsBrake() { - // return this.getRawButton(5); - // } - public void outputTelemetry() { SmartDashboard.putNumber(m_smartDashboardKey + "Forward", getForwardAxis()); SmartDashboard.putNumber(m_smartDashboardKey + "Turn", getTurnAxis()); diff --git a/src/main/java/frc/robot/controls/controllers/OperatorController.java b/src/main/java/frc/robot/controls/controllers/OperatorController.java index 8d46404..eca7c95 100644 --- a/src/main/java/frc/robot/controls/controllers/OperatorController.java +++ b/src/main/java/frc/robot/controls/controllers/OperatorController.java @@ -13,50 +13,49 @@ public OperatorController(int port, boolean useDeadband, boolean useSquaredInput // Axis private final double k_triggerActivationThreshold = 0.5; - public boolean getWantsTriggerSomething1() { - return this.getFilteredAxis(3) > k_triggerActivationThreshold; + public double getElevatorAxis() { + return -this.getFilteredAxis(1); } - public boolean getWantsTriggerSomething2() { - return this.getFilteredAxis(2) > k_triggerActivationThreshold; - } + // public boolean getWantsTriggerSomething2() { + // return this.getFilteredAxis(2) > k_triggerActivationThreshold; + // } // Buttons - public boolean getWantsBrakeMode() { - return this.getRawButton(1); - } + // public boolean getWantsBrakeMode() { + // return this.getRawButton(1); + // } - public boolean getWantsCoastMode() { - return this.getRawButton(2); - } + // public boolean getWantsCoastMode() { + // return this.getRawButton(2); + // } - public boolean getWantsMoreSpeed() { - return this.getRawButtonPressed(6); - } + // public boolean getWantsMoreSpeed() { + // return this.getRawButtonPressed(6); + // } - public boolean getWantsLessSpeed() { - return this.getRawButtonPressed(5); - } + // public boolean getWantsLessSpeed() { + // return this.getRawButtonPressed(5); + // } - public boolean getWantsShooterStop() { - return this.getRawButtonPressed(4); - } + // public boolean getWantsShooterStop() { + // return this.getRawButtonPressed(4); + // } // D pad + // public boolean getWantsClimberRelease() { + // return this.getHatDown(); + // } - public boolean getWantsClimberRelease() { - return this.getHatDown(); - } + // public boolean getWantsClimberTiltRight() { + // return this.getHatRight(); + // } - public boolean getWantsClimberTiltRight() { - return this.getHatRight(); - } + // public boolean getWantsClimberClimb() { + // return this.getHatUp(); + // } - public boolean getWantsClimberClimb() { - return this.getHatUp(); - } - - public boolean getWantsClimberTiltLeft() { - return this.getHatLeft(); - } + // public boolean getWantsClimberTiltLeft() { + // return this.getHatLeft(); + // } } diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java new file mode 100644 index 0000000..285cba6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -0,0 +1,94 @@ +package frc.robot.subsystems; + +import com.thethriftybot.ThriftyNova; +import com.thethriftybot.ThriftyNova.CurrentType; + +import frc.robot.Constants; + +public class Coral extends Subsystem { + + /*-------------------------------- Private instance variables ---------------------------------*/ + private static Coral mInstance; + private PeriodicIO mPeriodicIO; + + public static Coral getInstance() { + if (mInstance == null) { + mInstance = new Coral(); + } + return mInstance; + } + + private ThriftyNova mLeftMotor; + private ThriftyNova mRightMotor; + + private Coral() { + super("Coral"); + + mPeriodicIO = new PeriodicIO(); + + mLeftMotor = new ThriftyNova(Constants.Coral.kLeftMotorId); + mRightMotor = new ThriftyNova(Constants.Coral.kRightMotorId); + + mLeftMotor.setBrakeMode(false); + mRightMotor.setBrakeMode(false); + + mLeftMotor.setMaxCurrent(CurrentType.STATOR, 20); + mRightMotor.setMaxCurrent(CurrentType.STATOR, 20); + + // TODO: PID? + + // TODO: Make sure we're inverting the correct motor + mLeftMotor.setInversion(true); + } + + private static class PeriodicIO { + double shooter_rpm = 0.0; + + double left_rpm = 0.0; + double right_rpm = 0.0; + } + + /*-------------------------------- Generic Subsystem Functions --------------------------------*/ + + @Override + public void periodic() { + + } + + @Override + public void writePeriodicOutputs() { + // System.out.println(mPeriodicIO.left_rpm); + mLeftMotor.setVelocity(mPeriodicIO.left_rpm); + mRightMotor.setVelocity(mPeriodicIO.right_rpm); + } + + @Override + public void stop() { + stopCoral(); + } + + @Override + public void outputTelemetry() { + // putNumber("Speed (RPM):", mPeriodicIO.shooter_rpm); + putNumber("Left speed:", mLeftMotor.getVelocity()); + putNumber("Right speed:", mRightMotor.getVelocity()); + } + + @Override + public void reset() { + } + + /*---------------------------------- Custom Public Functions ----------------------------------*/ + + public void setSpeed(double rpm) { + mPeriodicIO.left_rpm = rpm; + mPeriodicIO.right_rpm = rpm; + } + + public void stopCoral() { + mPeriodicIO.left_rpm = 0.0; + mPeriodicIO.right_rpm = 0.0; + } + + /*---------------------------------- Custom Private Functions ---------------------------------*/ +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..8bb23b0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,167 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; + +import frc.robot.Constants; +import frc.robot.simulation.SimulatableCANSparkMax; + +public class Elevator extends Subsystem { + + /*-------------------------------- Private instance variables ---------------------------------*/ + private static Elevator mInstance; + private PeriodicIO mPeriodicIO; + + // private static final double kPivotCLRampRate = 0.5; + // private static final double kCLRampRate = 0.5; + + public static Elevator getInstance() { + if (mInstance == null) { + mInstance = new Elevator(); + } + return mInstance; + } + + private SimulatableCANSparkMax mLeftMotor; + private RelativeEncoder mLeftEncoder; + private SparkPIDController mLeftPIDController; + + private SimulatableCANSparkMax mRightMotor; + private RelativeEncoder mRightEncoder; + private SparkPIDController mRightPIDController; + + // private RelativeEncoder mLeftEncoder; + // private SparkMaxLimitSwitch mLowerLimit; + // private SparkMaxLimitSwitch mUpperLimit; + + // private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(1000); + + private void setUpElevatorMotor(SimulatableCANSparkMax motor, SparkPIDController pidController) { + motor.restoreFactoryDefaults(); + motor.setIdleMode(IdleMode.kBrake); + motor.setSmartCurrentLimit(Constants.Elevator.kMaxCurrent); + // mLeftEncoder = motor.getEncoder(); + + pidController.setP(Constants.Elevator.kP); + pidController.setI(Constants.Elevator.kI); + pidController.setD(Constants.Elevator.kD); + + pidController.setIZone(Constants.Elevator.kIZone); + pidController.setFF(Constants.Elevator.kFF); + mLeftPIDController.setOutputRange(Constants.Elevator.kMaxPowerUp, Constants.Elevator.kMaxPowerDown); + // motor.setClosedLoopRampRate(kExtensionCLRampRate); + + // mLowerLimit = mLeftMotor.getReverseLimitSwitch(Type.kNormallyOpen); + // mUpperLimit = mLeftMotor.getForwardLimitSwitch(Type.kNormallyOpen); + } + + private Elevator() { + super("Elevator"); + + mPeriodicIO = new PeriodicIO(); + + // LEFT ELEVATOR MOTOR + mLeftMotor = new SimulatableCANSparkMax(Constants.Elevator.kElevatorLeftMotorId, MotorType.kBrushless); + mLeftEncoder = mLeftMotor.getEncoder(); + mLeftPIDController = mLeftMotor.getPIDController(); + setUpElevatorMotor(mLeftMotor, mLeftPIDController); + + // RIGHT ELEVATOR MOTOR + mRightMotor = new SimulatableCANSparkMax(Constants.Elevator.kElevatorRightMotorId, MotorType.kBrushless); + mRightEncoder = mRightMotor.getEncoder(); + mRightPIDController = mRightMotor.getPIDController(); + setUpElevatorMotor(mRightMotor, mRightPIDController); + + mRightMotor.setInverted(true); + mLeftMotor.follow(mRightMotor, true); + + mLeftMotor.burnFlash(); + mRightMotor.burnFlash(); + } + + private static class PeriodicIO { + double elevator_target = 0.0; + double elevator_power = 0.0; + + boolean is_elevator_pos_control = false; + } + + /*-------------------------------- Generic Subsystem Functions --------------------------------*/ + + @Override + public void periodic() { + // TODO: Use this pattern to only drive slowly when we're really high up + // if(mPivotEncoder.getPosition() > Constants.kPivotScoreCount) { + // mPeriodicIO.is_pivot_low = true; + // } else { + // mPeriodicIO.is_pivot_low = false; + // } + } + + @Override + public void writePeriodicOutputs() { + if (mPeriodicIO.is_elevator_pos_control) { + mRightPIDController.setReference(mPeriodicIO.elevator_target, CANSparkMax.ControlType.kPosition); + } else { + mRightMotor.set(mPeriodicIO.elevator_power); + } + } + + @Override + public void stop() { + mPeriodicIO.is_elevator_pos_control = false; + mPeriodicIO.elevator_power = 0.0; + + mLeftMotor.set(0.0); + } + + @Override + public void outputTelemetry() { + // putNumber("Speed (RPM):", mPeriodicIO.shooter_rpm); + // putNumber("Left speed:", mLeftShooterEncoder.getVelocity()); + + putNumber("Position/Current", mRightEncoder.getPosition()); + putNumber("Position/Target", mPeriodicIO.elevator_target); + + putNumber("Current/Left", mLeftMotor.getOutputCurrent()); + putNumber("Current/Right", mRightMotor.getOutputCurrent()); + + } + + @Override + public void reset() { + } + + /*---------------------------------- Custom Public Functions ----------------------------------*/ + + public void raise() { + mPeriodicIO.is_elevator_pos_control = false; + mPeriodicIO.elevator_power = Constants.Elevator.kMaxPowerUp; + } + + public void lower() { + mPeriodicIO.is_elevator_pos_control = false; + mPeriodicIO.elevator_power = Constants.Elevator.kMaxPowerDown; + } + + public void setElevatorPower(double power) { + putNumber("setElevatorPower", power); + mPeriodicIO.is_elevator_pos_control = false; + mPeriodicIO.elevator_power = power; + } + + public void goToElevatorStow() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kStowHeight; + } + + public void goToElevatorL4() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kL4Height; + } + + /*---------------------------------- Custom Private Functions ---------------------------------*/ +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 2a61d6f..59452b5 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -65,7 +65,6 @@ private Shooter() { mLeftShooterMotor.setInverted(true); mRightShooterMotor.setInverted(false); - } private static class PeriodicIO { diff --git a/src/main/java/frc/robot/subsystems/Subsystem.java b/src/main/java/frc/robot/subsystems/Subsystem.java index 49c9879..3fb75b2 100644 --- a/src/main/java/frc/robot/subsystems/Subsystem.java +++ b/src/main/java/frc/robot/subsystems/Subsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -67,27 +69,27 @@ public void writeToLog() { public String baseSmartDashboardKey = "UnknownSubsystem/"; public void putNumber(String key, double value) { - SmartDashboard.putNumber(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putBoolean(String key, boolean value) { - SmartDashboard.putBoolean(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putString(String key, String value) { - SmartDashboard.putString(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putNumberArray(String key, double[] value) { - SmartDashboard.putNumberArray(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putBooleanArray(String key, boolean[] value) { - SmartDashboard.putBooleanArray(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putStringArray(String key, String[] value) { - SmartDashboard.putStringArray(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putData(String key, Sendable value) { From 1b87c7f203a8eeb42addf7ed8ed77564593c6153 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Mon, 6 Jan 2025 17:45:29 -0500 Subject: [PATCH 03/18] Add libgrapplefrc2024 --- vendordeps/libgrapplefrc2024.json | 72 +++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) create mode 100644 vendordeps/libgrapplefrc2024.json diff --git a/vendordeps/libgrapplefrc2024.json b/vendordeps/libgrapplefrc2024.json new file mode 100644 index 0000000..5532a13 --- /dev/null +++ b/vendordeps/libgrapplefrc2024.json @@ -0,0 +1,72 @@ +{ + "fileName": "libgrapplefrc2024.json", + "name": "libgrapplefrc", + "version": "2024.3.1", + "frcYear": "2024", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ "https://storage.googleapis.com/grapple-frc-maven" ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2024.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2024.3.1" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2024.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2024.3.1", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2024.3.1", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} From d15afae666c8f702224c98aa8c6afb2dc7873fbf Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Mon, 6 Jan 2025 22:59:41 -0500 Subject: [PATCH 04/18] Ignore TTB stuff --- .gitignore | 2 + src/main/java/frc/robot/Constants.java | 26 ++++- src/main/java/frc/robot/Robot.java | 60 +++++++---- .../controllers/OperatorController.java | 62 ++++++----- src/main/java/frc/robot/subsystems/Coral.java | 102 ++++++++++++++---- .../java/frc/robot/subsystems/Elevator.java | 35 +++--- 6 files changed, 195 insertions(+), 92 deletions(-) diff --git a/.gitignore b/.gitignore index cf497fe..d243bc4 100644 --- a/.gitignore +++ b/.gitignore @@ -179,3 +179,5 @@ ctre_sim/ # gversion build constants file src/main/java/frc/robot/BuildConstants.java + +ttb-maven diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 534a296..3ffbaa5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,18 +14,17 @@ public static class Elevator { public static final int kElevatorLeftMotorId = 9; public static final int kElevatorRightMotorId = 10; - public static final double kP = 0.1; - public static final double kI = 1e-8; - public static final double kD = 1; + public static final double kP = 0.02; + public static final double kI = 0.0; + public static final double kD = 0.0; public static final double kIZone = 0; - public static final double kFF = 0.0; + public static final double kG = 0.5; public static final int kMaxCurrent = 30; public static final double kMaxPowerUp = 0.1; public static final double kMaxPowerDown = 0.1; public static final double kStowHeight = 0.0; - public static final double kL1Height = 0.0; public static final double kL2Height = 0.0; public static final double kL3Height = 0.0; public static final double kL4Height = 52.6; @@ -39,6 +38,23 @@ public static class Elevator { public static class Coral { public static final int kLeftMotorId = 11; public static final int kRightMotorId = 12; + + public static final int kLaserId = 0; + public static final int kColorId = 16; + + public static final double kMaxCurrent = 20; + + public static final double kP = 0.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kIZone = 0; + + public static final double kIntakeSpeed = 0.5; + public static final double kReverseSpeed = -0.3; + public static final double kL1Speed = 0.8; + public static final double kL24Speed = 0.8; + public static final double kIndexSpeed = 0.2; + public static final double kSpeedDifference = 0.2; } public static class Algae { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1054c90..f9e4a44 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -77,7 +77,7 @@ public void robotInit() { // Add all subsystems to the list // m_allSubsystems.add(m_compressor); // m_allSubsystems.add(m_drive); - // m_allSubsystems.add(m_coral); + m_allSubsystems.add(m_coral); m_allSubsystems.add(m_elevator); // m_allSubsystems.add(m_leds); @@ -156,31 +156,45 @@ public void teleopPeriodic() { m_drive.drive(xSpeed, rot); - if (m_driverController.getWantsCoralEject()) { - // System.out.println("HERE"); - m_coral.setSpeed(60); + // if (m_driverController.getWantsCoralEject()) { + // // System.out.println("HERE"); + // m_coral.setSpeed(60); + // } else { + // m_coral.setSpeed(0); + // } + + // ELEVATOR + if (m_operatorController.getWantsElevatorStow()) { + m_elevator.goToElevatorStow(); + } else if (m_operatorController.getWantsElevatorL2()) { + m_elevator.goToElevatorL2(); + } else if (m_operatorController.getWantsElevatorL3()) { + m_elevator.goToElevatorL3(); + } else if (m_operatorController.getWantsElevatorL4()) { + m_elevator.goToElevatorL4(); } else { - m_coral.setSpeed(0); + m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); } - m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); - - // // Shooter variable speed - // if (m_driverController.getWantsMoreSpeed() || - // m_operatorController.getWantsMoreSpeed()) { - // m_leds.setColor(Color.kPink); - // speed = 3000; - // } else if (m_driverController.getWantsLessSpeed() || - // m_operatorController.getWantsLessSpeed()) { - // m_leds.setColor(Color.kOrange); - // speed = 430; - // } else if (m_driverController.getWantsShooterStop() || - // m_operatorController.getWantsShooterStop()) { - // m_leds.defaultLEDS(); - // speed = 0; - // } - // speed = MathUtil.clamp(speed, -6000, 10000); - // m_shooter.setSpeed(speed); + if (m_operatorController.getWantsElevatorReset()) { + RobotTelemetry.print("Resetting elevator"); + m_elevator.reset(); + } + + // CORAL + if (m_operatorController.getWantsCoralIntake()) { + m_coral.intake(); + } else if (m_operatorController.getWantsCoralReverse()) { + m_coral.reverse(); + } else if (m_operatorController.getWantsCoralIndex()) { + m_coral.index(); + } else if (m_operatorController.getWantsCoralL1()) { + m_coral.scoreL1(); + } else if (m_operatorController.getWantsCoralL24()) { + m_coral.scoreL24(); + } else { + m_coral.stopCoral(); + } // // Intake // if (m_driverController.getWantsFullIntake()) { diff --git a/src/main/java/frc/robot/controls/controllers/OperatorController.java b/src/main/java/frc/robot/controls/controllers/OperatorController.java index eca7c95..c800d21 100644 --- a/src/main/java/frc/robot/controls/controllers/OperatorController.java +++ b/src/main/java/frc/robot/controls/controllers/OperatorController.java @@ -21,41 +21,45 @@ public double getElevatorAxis() { // return this.getFilteredAxis(2) > k_triggerActivationThreshold; // } - // Buttons - // public boolean getWantsBrakeMode() { - // return this.getRawButton(1); - // } + // CORAL + public boolean getWantsCoralIntake() { + return this.getRawButton(4); + } - // public boolean getWantsCoastMode() { - // return this.getRawButton(2); - // } + public boolean getWantsCoralReverse() { + return this.getRawButton(1); + } - // public boolean getWantsMoreSpeed() { - // return this.getRawButtonPressed(6); - // } + public boolean getWantsCoralIndex() { + return this.getRawButton(8); + } - // public boolean getWantsLessSpeed() { - // return this.getRawButtonPressed(5); - // } + public boolean getWantsCoralL1() { + return this.getRawButton(3); + } - // public boolean getWantsShooterStop() { - // return this.getRawButtonPressed(4); - // } + public boolean getWantsCoralL24() { + return this.getRawButton(2); + } - // D pad - // public boolean getWantsClimberRelease() { - // return this.getHatDown(); - // } + // ELEVATOR + public boolean getWantsElevatorReset() { + return this.getRawButton(7); + } - // public boolean getWantsClimberTiltRight() { - // return this.getHatRight(); - // } + public boolean getWantsElevatorStow() { + return this.getHatDown(); + } - // public boolean getWantsClimberClimb() { - // return this.getHatUp(); - // } + public boolean getWantsElevatorL2() { + return this.getHatLeft(); + } - // public boolean getWantsClimberTiltLeft() { - // return this.getHatLeft(); - // } + public boolean getWantsElevatorL3() { + return this.getHatRight(); + } + + public boolean getWantsElevatorL4() { + return this.getHatUp(); + } } diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java index 285cba6..5ba1eb0 100644 --- a/src/main/java/frc/robot/subsystems/Coral.java +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -1,8 +1,10 @@ package frc.robot.subsystems; import com.thethriftybot.ThriftyNova; -import com.thethriftybot.ThriftyNova.CurrentType; +import com.thethriftybot.ThriftyNova.PIDConfig; +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; import frc.robot.Constants; public class Coral extends Subsystem { @@ -20,6 +22,10 @@ public static Coral getInstance() { private ThriftyNova mLeftMotor; private ThriftyNova mRightMotor; + private PIDConfig mPIDConfig; + + private LaserCan mLaserCAN; + private int mLaserReading; private Coral() { super("Coral"); @@ -29,65 +35,123 @@ private Coral() { mLeftMotor = new ThriftyNova(Constants.Coral.kLeftMotorId); mRightMotor = new ThriftyNova(Constants.Coral.kRightMotorId); + mLeftMotor.factoryReset(); + mRightMotor.factoryReset(); + mLeftMotor.setBrakeMode(false); mRightMotor.setBrakeMode(false); - mLeftMotor.setMaxCurrent(CurrentType.STATOR, 20); - mRightMotor.setMaxCurrent(CurrentType.STATOR, 20); + // mLeftMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); + // mRightMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); - // TODO: PID? + mPIDConfig = mRightMotor.pid0; + mPIDConfig.setP(Constants.Coral.kP); + mPIDConfig.setI(Constants.Coral.kI); + mPIDConfig.setD(Constants.Coral.kD); + mPIDConfig.setIZone(Constants.Coral.kIZone); // TODO: Make sure we're inverting the correct motor - mLeftMotor.setInversion(true); + mLeftMotor.setInverted(false); + mRightMotor.setInversion(true); + + mLaserCAN = new LaserCan(Constants.Coral.kLaserId); + try { + mLaserCAN.setRangingMode(LaserCan.RangingMode.SHORT); + mLaserCAN.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + mLaserCAN.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + } catch (ConfigurationFailedException e) { + System.out.println("Configuration failed! " + e); + } } private static class PeriodicIO { - double shooter_rpm = 0.0; + double rpm = 0.0; + double speed_diff = 0.0; - double left_rpm = 0.0; - double right_rpm = 0.0; + LaserCan.Measurement measurement; } /*-------------------------------- Generic Subsystem Functions --------------------------------*/ @Override public void periodic() { + mPeriodicIO.measurement = mLaserCAN.getMeasurement(); } @Override public void writePeriodicOutputs() { - // System.out.println(mPeriodicIO.left_rpm); - mLeftMotor.setVelocity(mPeriodicIO.left_rpm); - mRightMotor.setVelocity(mPeriodicIO.right_rpm); + // mLeftMotor.setVelocity(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + // mRightMotor.setVelocity(mPeriodicIO.rpm); + mLeftMotor.setPercent(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + mRightMotor.setPercent(mPeriodicIO.rpm); } @Override public void stop() { - stopCoral(); + mPeriodicIO.rpm = 0.0; + mPeriodicIO.speed_diff = 0.0; } @Override public void outputTelemetry() { - // putNumber("Speed (RPM):", mPeriodicIO.shooter_rpm); - putNumber("Left speed:", mLeftMotor.getVelocity()); - putNumber("Right speed:", mRightMotor.getVelocity()); + putNumber("RPM/target", mPeriodicIO.rpm); + + putNumber("RPM/Left/Position", mLeftMotor.getPosition()); + putNumber("RPM/Right/Position", mRightMotor.getPosition()); + + putNumber("RPM/Left/Velocity", mLeftMotor.getVelocity()); + putNumber("RPM/Right/Velocity", mRightMotor.getVelocity()); + + LaserCan.Measurement measurement = mPeriodicIO.measurement; + if (measurement != null) { + putNumber("Laser/distance", measurement.distance_mm); + putNumber("Laser/ambient", measurement.ambient); + putNumber("Laser/budget_ms", measurement.budget_ms); + putNumber("Laser/status", measurement.status); + } } @Override public void reset() { + stopCoral(); } /*---------------------------------- Custom Public Functions ----------------------------------*/ public void setSpeed(double rpm) { - mPeriodicIO.left_rpm = rpm; - mPeriodicIO.right_rpm = rpm; + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = rpm; + } + + public void intake() { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = Constants.Coral.kIntakeSpeed; + } + + public void reverse() { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = Constants.Coral.kReverseSpeed; + } + + public void index() { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = Constants.Coral.kIndexSpeed; + } + + public void scoreL1() { + mPeriodicIO.speed_diff = Constants.Coral.kSpeedDifference; + mPeriodicIO.rpm = Constants.Coral.kL1Speed; + } + + public void scoreL24() { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = Constants.Coral.kL24Speed; } public void stopCoral() { - mPeriodicIO.left_rpm = 0.0; - mPeriodicIO.right_rpm = 0.0; + mPeriodicIO.rpm = 0.0; + mPeriodicIO.speed_diff = 0.0; } /*---------------------------------- Custom Private Functions ---------------------------------*/ diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 8bb23b0..6dea8e0 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -5,6 +5,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; import frc.robot.Constants; import frc.robot.simulation.SimulatableCANSparkMax; @@ -48,9 +49,8 @@ private void setUpElevatorMotor(SimulatableCANSparkMax motor, SparkPIDController pidController.setP(Constants.Elevator.kP); pidController.setI(Constants.Elevator.kI); pidController.setD(Constants.Elevator.kD); - pidController.setIZone(Constants.Elevator.kIZone); - pidController.setFF(Constants.Elevator.kFF); + mLeftPIDController.setOutputRange(Constants.Elevator.kMaxPowerUp, Constants.Elevator.kMaxPowerDown); // motor.setClosedLoopRampRate(kExtensionCLRampRate); @@ -104,7 +104,12 @@ public void periodic() { @Override public void writePeriodicOutputs() { if (mPeriodicIO.is_elevator_pos_control) { - mRightPIDController.setReference(mPeriodicIO.elevator_target, CANSparkMax.ControlType.kPosition); + mRightPIDController.setReference( + mPeriodicIO.elevator_target, + CANSparkMax.ControlType.kPosition, + 0, + Constants.Elevator.kG, + ArbFFUnits.kVoltage); } else { mRightMotor.set(mPeriodicIO.elevator_power); } @@ -120,9 +125,6 @@ public void stop() { @Override public void outputTelemetry() { - // putNumber("Speed (RPM):", mPeriodicIO.shooter_rpm); - // putNumber("Left speed:", mLeftShooterEncoder.getVelocity()); - putNumber("Position/Current", mRightEncoder.getPosition()); putNumber("Position/Target", mPeriodicIO.elevator_target); @@ -133,20 +135,11 @@ public void outputTelemetry() { @Override public void reset() { + mRightEncoder.setPosition(0.0); } /*---------------------------------- Custom Public Functions ----------------------------------*/ - public void raise() { - mPeriodicIO.is_elevator_pos_control = false; - mPeriodicIO.elevator_power = Constants.Elevator.kMaxPowerUp; - } - - public void lower() { - mPeriodicIO.is_elevator_pos_control = false; - mPeriodicIO.elevator_power = Constants.Elevator.kMaxPowerDown; - } - public void setElevatorPower(double power) { putNumber("setElevatorPower", power); mPeriodicIO.is_elevator_pos_control = false; @@ -158,6 +151,16 @@ public void goToElevatorStow() { mPeriodicIO.elevator_target = Constants.Elevator.kStowHeight; } + public void goToElevatorL2() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kL2Height; + } + + public void goToElevatorL3() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kL3Height; + } + public void goToElevatorL4() { mPeriodicIO.is_elevator_pos_control = true; mPeriodicIO.elevator_target = Constants.Elevator.kL4Height; From 6ae279a612be30c3a1c2b28e37b119aa4d21cd1a Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Mon, 6 Jan 2025 23:33:40 -0500 Subject: [PATCH 05/18] Local ttb-maven --- vendordeps/ThriftyLib.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json index 0098894..9da07c4 100644 --- a/vendordeps/ThriftyLib.json +++ b/vendordeps/ThriftyLib.json @@ -5,7 +5,7 @@ "frcYear": "2024", "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", "mavenUrls": [ - "https://docs.home.thethriftybot.com" + "file:./ttb-maven" ], "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", "javaDependencies": [ From 2b2e46da4a84c5bd0602c99d93df75f36c15df72 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 01:07:10 -0500 Subject: [PATCH 06/18] Motors run --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/subsystems/Coral.java | 25 ++++++++++++++----- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3ffbaa5..3291029 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,7 +49,7 @@ public static class Coral { public static final double kD = 0.0; public static final double kIZone = 0; - public static final double kIntakeSpeed = 0.5; + public static final double kIntakeSpeed = 0.3; public static final double kReverseSpeed = -0.3; public static final double kL1Speed = 0.8; public static final double kL24Speed = 0.8; diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java index 5ba1eb0..2cb2e0c 100644 --- a/src/main/java/frc/robot/subsystems/Coral.java +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -35,8 +35,8 @@ private Coral() { mLeftMotor = new ThriftyNova(Constants.Coral.kLeftMotorId); mRightMotor = new ThriftyNova(Constants.Coral.kRightMotorId); - mLeftMotor.factoryReset(); - mRightMotor.factoryReset(); + // mLeftMotor.factoryReset(); + // mRightMotor.factoryReset(); mLeftMotor.setBrakeMode(false); mRightMotor.setBrakeMode(false); @@ -44,6 +44,18 @@ private Coral() { // mLeftMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); // mRightMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); + // mLeftMotor.setMaxCurrent(CurrentType.STATOR, 200.0); + // mRightMotor.setMaxCurrent(CurrentType.STATOR, 200.0); + + // mLeftMotor.setMaxCurrent(CurrentType.SUPPLY, 200.0); + // mRightMotor.setMaxCurrent(CurrentType.SUPPLY, 200.0); + + mLeftMotor.setRampUp(0.5); + mRightMotor.setRampUp(0.5); + + mLeftMotor.setMaxOutput(1.0); + mRightMotor.setMaxOutput(1.0); + mPIDConfig = mRightMotor.pid0; mPIDConfig.setP(Constants.Coral.kP); mPIDConfig.setI(Constants.Coral.kI); @@ -51,8 +63,9 @@ private Coral() { mPIDConfig.setIZone(Constants.Coral.kIZone); // TODO: Make sure we're inverting the correct motor - mLeftMotor.setInverted(false); - mRightMotor.setInversion(true); + // mLeftMotor.setInverted(true); + // mLeftMotor.setInversion(false); + // mRightMotor.setInversion(false); mLaserCAN = new LaserCan(Constants.Coral.kLaserId); try { @@ -83,8 +96,8 @@ public void periodic() { public void writePeriodicOutputs() { // mLeftMotor.setVelocity(mPeriodicIO.rpm - mPeriodicIO.speed_diff); // mRightMotor.setVelocity(mPeriodicIO.rpm); - mLeftMotor.setPercent(mPeriodicIO.rpm - mPeriodicIO.speed_diff); - mRightMotor.setPercent(mPeriodicIO.rpm); + // mLeftMotor.setPercent(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + mRightMotor.setPercent(-mPeriodicIO.rpm); } @Override From 92b64fa3573ea1fd13226128119d9407d4b91cf1 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 01:57:09 -0500 Subject: [PATCH 07/18] Elevator works --- src/main/java/frc/robot/Constants.java | 20 +++++++++---------- src/main/java/frc/robot/Robot.java | 3 +-- src/main/java/frc/robot/subsystems/Coral.java | 2 +- .../java/frc/robot/subsystems/Elevator.java | 15 +++++++++----- 4 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3291029..c35a445 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,20 +14,20 @@ public static class Elevator { public static final int kElevatorLeftMotorId = 9; public static final int kElevatorRightMotorId = 10; - public static final double kP = 0.02; - public static final double kI = 0.0; + public static final double kP = 0.030; + public static final double kI = 0.00005; public static final double kD = 0.0; - public static final double kIZone = 0; + public static final double kIZone = 5.0; public static final double kG = 0.5; - public static final int kMaxCurrent = 30; + public static final int kMaxCurrent = 40; public static final double kMaxPowerUp = 0.1; public static final double kMaxPowerDown = 0.1; public static final double kStowHeight = 0.0; - public static final double kL2Height = 0.0; - public static final double kL3Height = 0.0; - public static final double kL4Height = 52.6; + public static final double kL2Height = 9.0; + public static final double kL3Height = 25.14; + public static final double kL4Height = 51.0; public static final double kMaxHeight = 56.2; public static final double kGroundAlgaeHeight = 0.0; public static final double kScoreAlgaeHeight = 0.0; @@ -51,10 +51,10 @@ public static class Coral { public static final double kIntakeSpeed = 0.3; public static final double kReverseSpeed = -0.3; - public static final double kL1Speed = 0.8; - public static final double kL24Speed = 0.8; + public static final double kL1Speed = 0.4; + public static final double kL24Speed = 0.4; public static final double kIndexSpeed = 0.2; - public static final double kSpeedDifference = 0.2; + public static final double kSpeedDifference = kL1Speed * 0.5; } public static class Algae { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f9e4a44..bbec255 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -164,6 +164,7 @@ public void teleopPeriodic() { // } // ELEVATOR + // m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); if (m_operatorController.getWantsElevatorStow()) { m_elevator.goToElevatorStow(); } else if (m_operatorController.getWantsElevatorL2()) { @@ -172,8 +173,6 @@ public void teleopPeriodic() { m_elevator.goToElevatorL3(); } else if (m_operatorController.getWantsElevatorL4()) { m_elevator.goToElevatorL4(); - } else { - m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); } if (m_operatorController.getWantsElevatorReset()) { diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java index 2cb2e0c..1408da6 100644 --- a/src/main/java/frc/robot/subsystems/Coral.java +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -96,7 +96,7 @@ public void periodic() { public void writePeriodicOutputs() { // mLeftMotor.setVelocity(mPeriodicIO.rpm - mPeriodicIO.speed_diff); // mRightMotor.setVelocity(mPeriodicIO.rpm); - // mLeftMotor.setPercent(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + mLeftMotor.setPercent(mPeriodicIO.rpm - mPeriodicIO.speed_diff); mRightMotor.setPercent(-mPeriodicIO.rpm); } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 6dea8e0..9dbbd8a 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -50,8 +50,11 @@ private void setUpElevatorMotor(SimulatableCANSparkMax motor, SparkPIDController pidController.setI(Constants.Elevator.kI); pidController.setD(Constants.Elevator.kD); pidController.setIZone(Constants.Elevator.kIZone); + // pidController.setIMaxAccum(0.001, 0) + + // mLeftPIDController.setOutputRange(Constants.Elevator.kMaxPowerUp, + // Constants.Elevator.kMaxPowerDown); - mLeftPIDController.setOutputRange(Constants.Elevator.kMaxPowerUp, Constants.Elevator.kMaxPowerDown); // motor.setClosedLoopRampRate(kExtensionCLRampRate); // mLowerLimit = mLeftMotor.getReverseLimitSwitch(Type.kNormallyOpen); @@ -75,8 +78,8 @@ private Elevator() { mRightPIDController = mRightMotor.getPIDController(); setUpElevatorMotor(mRightMotor, mRightPIDController); - mRightMotor.setInverted(true); - mLeftMotor.follow(mRightMotor, true); + // mRightMotor.setInverted(true); + mRightMotor.follow(mLeftMotor, true); mLeftMotor.burnFlash(); mRightMotor.burnFlash(); @@ -104,14 +107,14 @@ public void periodic() { @Override public void writePeriodicOutputs() { if (mPeriodicIO.is_elevator_pos_control) { - mRightPIDController.setReference( + mLeftPIDController.setReference( mPeriodicIO.elevator_target, CANSparkMax.ControlType.kPosition, 0, Constants.Elevator.kG, ArbFFUnits.kVoltage); } else { - mRightMotor.set(mPeriodicIO.elevator_power); + mLeftMotor.set(mPeriodicIO.elevator_power); } } @@ -131,6 +134,8 @@ public void outputTelemetry() { putNumber("Current/Left", mLeftMotor.getOutputCurrent()); putNumber("Current/Right", mRightMotor.getOutputCurrent()); + putNumber("Output/Left", mLeftMotor.getAppliedOutput()); + putNumber("Output/Right", mRightMotor.getAppliedOutput()); } @Override From e270f7808b76bfee4321b5864cf8adbcc581a1d1 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 02:06:14 -0500 Subject: [PATCH 08/18] Driving --- src/main/java/frc/robot/Constants.java | 6 ++--- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 26 ++++++++++--------- 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c35a445..e78915d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -122,9 +122,9 @@ public static class Intake { // Drivetrain public static class Drive { - public static final double kP = 0.00085;// 3.6954;//0.085; - public static final double kI = 0.0;// 0.0; - public static final double kD = 0.0;// 0.0; + public static final double kP = 0.0; // 0.00085; + public static final double kI = 0.0; + public static final double kD = 0.0; public static final double kS = 0.1695;// 0.01; public static final double kV = 2.8559;// 2.6; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bbec255..1443c1a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -76,7 +76,7 @@ public void robotInit() { // Add all subsystems to the list // m_allSubsystems.add(m_compressor); - // m_allSubsystems.add(m_drive); + m_allSubsystems.add(m_drive); m_allSubsystems.add(m_coral); m_allSubsystems.add(m_elevator); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 6a81d25..5451807 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -170,18 +170,20 @@ public Drivetrain() { // TODO: get rid of this? // Configure AutoBuilder last // AutoBuilder.configureRamsete( - // this::getPose, // Robot pose supplier - // this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - // this::getCurrentSpeeds, // Current ChassisSpeeds supplier - // this::drive, // Method that will drive the robot given ChassisSpeeds - // new ReplanningConfig(), // Default path replanning config. See the API for the options here - // new BooleanSupplier() { - // @Override - // public boolean getAsBoolean() { - // return true; - // } - // }, // determines if paths should be flipped to the other side of the field - // this // Reference to this subsystem to set requirements + // this::getPose, // Robot pose supplier + // this::resetOdometry, // Method to reset odometry (will be called if your auto + // has a starting pose) + // this::getCurrentSpeeds, // Current ChassisSpeeds supplier + // this::drive, // Method that will drive the robot given ChassisSpeeds + // new ReplanningConfig(), // Default path replanning config. See the API for + // the options here + // new BooleanSupplier() { + // @Override + // public boolean getAsBoolean() { + // return true; + // } + // }, // determines if paths should be flipped to the other side of the field + // this // Reference to this subsystem to set requirements // ); mSysIdRoutine = new SysIdRoutine( From f6d1da4d666d01b6af704d5f864e4b3bb085002d Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 04:00:53 -0500 Subject: [PATCH 09/18] Wrist and intake work --- AdvantageScope.json | 84 ++++++++- src/main/java/frc/robot/Constants.java | 31 +++- src/main/java/frc/robot/Robot.java | 19 ++- .../controllers/DriverController.java | 14 +- src/main/java/frc/robot/subsystems/Algae.java | 159 ++++++++++++++++++ .../robot/wrappers/REVThroughBoreEncoder.java | 14 ++ 6 files changed, 311 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Algae.java create mode 100644 src/main/java/frc/robot/wrappers/REVThroughBoreEncoder.java diff --git a/AdvantageScope.json b/AdvantageScope.json index c228d59..b09096d 100644 --- a/AdvantageScope.json +++ b/AdvantageScope.json @@ -24,11 +24,17 @@ "/RealOutputs/Auto/DriveTrajectory/CurrentPose/rotation", "/RealOutputs/Elevator", "/RealOutputs/Elevator/Position", - "/RealOutputs/Elevator/Current" + "/RealOutputs/Elevator/Current", + "/RealOutputs/Coral/RPM", + "/RealOutputs/Coral/Laser", + "/RealOutputs/Coral/RPM/Left", + "/RealOutputs/Coral/RPM/Right", + "/RealOutputs/Coral", + "/RealOutputs/Elevator/Output" ] }, "tabs": { - "selected": 3, + "selected": 4, "tabs": [ { "type": 0, @@ -182,6 +188,80 @@ }, "title": "Elevator" }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "Wrist" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Coral/RPM/target", + "color": "#af2437", + "show": true + }, + { + "key": "/RealOutputs/Coral/RPM/Left/Position", + "color": "#2b66a2", + "show": true + }, + { + "key": "/RealOutputs/Coral/RPM/Right/Position", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Coral/Laser/distance", + "color": "#e48b32", + "show": true + } + ] + } + }, + "title": "Coral" + }, { "type": 3, "field": "/RealOutputs/Console", diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e78915d..8b74bd9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -58,9 +58,38 @@ public static class Coral { } public static class Algae { + // WRIST public static final int kWristMotorId = 13; - public static final int kIntakeMotorId = 14; + + public static final int kWristEncoderId = 9; + + public static final int kMaxWristCurrent = 10; + + public static final double kWristP = 0.01; + public static final double kWristI = 0.0; + public static final double kWristD = 0.0; + + public static final double kWristKS = 0.0; + public static final double kWristKG = 0.0; + public static final double kWristKV = 0.100; + public static final double kWristKA = 0.0; + + public static final double kWristOffset = 141.0; + + public static final double kWristMaxVelocity = 690.0; + public static final double kWristMaxAcceleration = 1380.0; + + public static final double kStowAngle = 233.0; + public static final double kDeAlgaeAngle = 190.0; + public static final double kGroundIntakeAngle = 140.0; + + // INTAKE + public static final int kMaxIntakeCurrent = 80; + + public static final double kIntakeSpeed = 4.0; + public static final double kEjectSpeed = -4.0; + public static final double kGroundIntakeSpeed = -4.0; } public static class Intake { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1443c1a..2ec3421 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,6 +24,7 @@ import frc.robot.controls.controllers.DriverController; import frc.robot.controls.controllers.OperatorController; import frc.robot.simulation.Field; +import frc.robot.subsystems.Algae; import frc.robot.subsystems.Compressor; import frc.robot.subsystems.Coral; import frc.robot.subsystems.Drivetrain; @@ -55,6 +56,7 @@ public class Robot extends LoggedRobot { private final Compressor m_compressor = Compressor.getInstance(); private final Drivetrain m_drive = Drivetrain.getInstance(); private final Coral m_coral = Coral.getInstance(); + private final Algae m_algae = Algae.getInstance(); // private final Shooter m_shooter = Shooter.getInstance(); private final Elevator m_elevator = Elevator.getInstance(); @@ -78,6 +80,7 @@ public void robotInit() { // m_allSubsystems.add(m_compressor); m_allSubsystems.add(m_drive); m_allSubsystems.add(m_coral); + m_allSubsystems.add(m_algae); m_allSubsystems.add(m_elevator); // m_allSubsystems.add(m_leds); @@ -156,12 +159,16 @@ public void teleopPeriodic() { m_drive.drive(xSpeed, rot); - // if (m_driverController.getWantsCoralEject()) { - // // System.out.println("HERE"); - // m_coral.setSpeed(60); - // } else { - // m_coral.setSpeed(0); - // } + // ALGAE + if (m_driverController.getWantsAlgaeStow()) { + m_algae.stow(); + } else if (m_driverController.getWantsAlgaeGrab()) { + m_algae.grabAlgae(); + } else if (m_driverController.getWantsAlgaeScore()) { + m_algae.score(); + } else if (m_driverController.getWantsAlgaeGroundIntake()) { + m_algae.groundIntake(); + } // ELEVATOR // m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); diff --git a/src/main/java/frc/robot/controls/controllers/DriverController.java b/src/main/java/frc/robot/controls/controllers/DriverController.java index 24aca79..13773b2 100644 --- a/src/main/java/frc/robot/controls/controllers/DriverController.java +++ b/src/main/java/frc/robot/controls/controllers/DriverController.java @@ -32,10 +32,22 @@ public boolean getWantsSlowMode() { return this.getFilteredAxis(3) > k_triggerActivationThreshold; } - public boolean getWantsCoralEject() { + public boolean getWantsAlgaeStow() { return this.getRawButton(1); } + public boolean getWantsAlgaeGrab() { + return this.getRawButton(3); + } + + public boolean getWantsAlgaeScore() { + return this.getRawButton(2); + } + + public boolean getWantsAlgaeGroundIntake() { + return this.getRawButton(4); + } + public void outputTelemetry() { SmartDashboard.putNumber(m_smartDashboardKey + "Forward", getForwardAxis()); SmartDashboard.putNumber(m_smartDashboardKey + "Turn", getTurnAxis()); diff --git a/src/main/java/frc/robot/subsystems/Algae.java b/src/main/java/frc/robot/subsystems/Algae.java new file mode 100644 index 0000000..d39e548 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Algae.java @@ -0,0 +1,159 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.simulation.SimulatableCANSparkMax; +import frc.robot.wrappers.REVThroughBoreEncoder; + +public class Algae extends Subsystem { + + /*-------------------------------- Private instance variables ---------------------------------*/ + private static Algae mInstance; + private PeriodicIO mPeriodicIO; + + public static Algae getInstance() { + if (mInstance == null) { + mInstance = new Algae(); + } + return mInstance; + } + + private SimulatableCANSparkMax mWristMotor; + private final ProfiledPIDController mWristPIDController; + private final ArmFeedforward mWristFeedForward; + + private SimulatableCANSparkMax mIntakeMotor; + + private final REVThroughBoreEncoder mWristAbsEncoder = new REVThroughBoreEncoder(Constants.Algae.kWristEncoderId); + + private Algae() { + super("Algae"); + + mPeriodicIO = new PeriodicIO(); + + // WRIST + mWristMotor = new SimulatableCANSparkMax(Constants.Algae.kWristMotorId, MotorType.kBrushless); + mWristMotor.restoreFactoryDefaults(); + mWristMotor.setIdleMode(IdleMode.kCoast); + mWristMotor.setSmartCurrentLimit(Constants.Algae.kMaxWristCurrent); + + // Wrist PID + mWristPIDController = new ProfiledPIDController( + Constants.Algae.kWristP, + Constants.Algae.kWristI, + Constants.Algae.kWristD, + new TrapezoidProfile.Constraints( + Constants.Algae.kWristMaxVelocity, + Constants.Algae.kWristMaxAcceleration)); + + // Wrist Feedforward + mWristFeedForward = new ArmFeedforward( + Constants.Algae.kWristKS, + Constants.Algae.kWristKG, + Constants.Algae.kWristKV, + Constants.Algae.kWristKA); + + // INTAKE + mIntakeMotor = new SimulatableCANSparkMax(Constants.Algae.kIntakeMotorId, MotorType.kBrushless); + mIntakeMotor.restoreFactoryDefaults(); + mIntakeMotor.setIdleMode(IdleMode.kBrake); + mIntakeMotor.setSmartCurrentLimit(Constants.Algae.kMaxIntakeCurrent); + + mIntakeMotor.setInverted(true); + + mWristMotor.setInverted(true); + + mWristMotor.burnFlash(); + mIntakeMotor.burnFlash(); + } + + private static class PeriodicIO { + double wrist_target_angle = 0.0; + double wrist_voltage = 0.0; + + double intake_power = 0.0; + } + + /*-------------------------------- Generic Subsystem Functions --------------------------------*/ + + @Override + public void periodic() { + double pidCalc = mWristPIDController.calculate(getWristAngle(), mPeriodicIO.wrist_target_angle); + double ffCalc = mWristFeedForward.calculate(Math.toRadians(getWristReferenceToHorizontal()), + Math.toRadians(mWristPIDController.getSetpoint().velocity)); + + mPeriodicIO.wrist_voltage = pidCalc + ffCalc; + } + + @Override + public void writePeriodicOutputs() { + mWristMotor.set(mPeriodicIO.wrist_voltage); + + // mIntakeMotor.set(mPeriodicIO.intake_power); + mIntakeMotor.setVoltage(mPeriodicIO.intake_power); + } + + @Override + public void stop() { + mPeriodicIO.wrist_voltage = 0.0; + mPeriodicIO.wrist_target_angle = Constants.Algae.kStowAngle; + + mWristMotor.set(0.0); + mIntakeMotor.set(0.0); + } + + @Override + public void outputTelemetry() { + putNumber("Wrist/Position", getWristAngle()); + putNumber("Wrist/Target", mPeriodicIO.wrist_target_angle); + putNumber("Wrist/Current", mWristMotor.getOutputCurrent()); + putNumber("Wrist/Output", mWristMotor.getAppliedOutput()); + putNumber("Wrist/Voltage", mPeriodicIO.wrist_voltage); + putNumber("Wrist/Frequency", mWristAbsEncoder.getFrequency()); + + putNumber("Intake/Current", mIntakeMotor.getOutputCurrent()); + putNumber("Intake/Output", mIntakeMotor.getAppliedOutput()); + putNumber("Intake/Power", mPeriodicIO.intake_power); + } + + @Override + public void reset() { + } + + /*---------------------------------- Custom Public Functions ----------------------------------*/ + + public void stow() { + mPeriodicIO.wrist_target_angle = Constants.Algae.kStowAngle; + mPeriodicIO.intake_power = 0.0; + } + + public void grabAlgae() { + mPeriodicIO.wrist_target_angle = Constants.Algae.kDeAlgaeAngle; + mPeriodicIO.intake_power = Constants.Algae.kIntakeSpeed; + } + + public void score() { + mPeriodicIO.intake_power = Constants.Algae.kEjectSpeed; + } + + public void groundIntake() { + mPeriodicIO.wrist_target_angle = Constants.Algae.kGroundIntakeAngle; + mPeriodicIO.intake_power = Constants.Algae.kGroundIntakeSpeed; + } + + /*---------------------------------- Custom Private Functions ---------------------------------*/ + + public double getWristAngle() { + return Units.rotationsToDegrees(mWristAbsEncoder.getAbsolutePosition()); + } + + public double getWristReferenceToHorizontal() { + return getWristAngle() - Constants.Algae.kWristOffset; + } +} diff --git a/src/main/java/frc/robot/wrappers/REVThroughBoreEncoder.java b/src/main/java/frc/robot/wrappers/REVThroughBoreEncoder.java new file mode 100644 index 0000000..58db56b --- /dev/null +++ b/src/main/java/frc/robot/wrappers/REVThroughBoreEncoder.java @@ -0,0 +1,14 @@ +package frc.robot.wrappers; + +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +public class REVThroughBoreEncoder extends DutyCycleEncoder { + public REVThroughBoreEncoder(int channel) { + super(channel); + + // Based on logging: + // - Initializing: 764 ± 5 + // - Ready: 973 ± 5 + setConnectedFrequencyThreshold(900); + } +} From d3fa9e2ecfde6813f64af313f9595331ddd928f4 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 04:01:28 -0500 Subject: [PATCH 10/18] Update AS config --- AdvantageScope.json | 78 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 70 insertions(+), 8 deletions(-) diff --git a/AdvantageScope.json b/AdvantageScope.json index b09096d..5955c4a 100644 --- a/AdvantageScope.json +++ b/AdvantageScope.json @@ -22,15 +22,12 @@ "/RealOutputs/Auto/DriveTrajectory/TargetPose/rotation", "/RealOutputs/Auto/DriveTrajectory/CurrentPose", "/RealOutputs/Auto/DriveTrajectory/CurrentPose/rotation", - "/RealOutputs/Elevator", "/RealOutputs/Elevator/Position", - "/RealOutputs/Elevator/Current", - "/RealOutputs/Coral/RPM", - "/RealOutputs/Coral/Laser", "/RealOutputs/Coral/RPM/Left", "/RealOutputs/Coral/RPM/Right", - "/RealOutputs/Coral", - "/RealOutputs/Elevator/Output" + "/RealOutputs/Elevator/Output", + "/RealOutputs/Algae", + "/RealOutputs/Algae/Wrist" ] }, "tabs": { @@ -198,7 +195,23 @@ "type": null, "factor": 1 }, - "fields": [] + "fields": [ + { + "key": "/RealOutputs/Algae/Wrist/Frequency", + "color": "#e5b31b", + "show": false + }, + { + "key": "/RealOutputs/Algae/Wrist/Output", + "color": "#af2437", + "show": true + }, + { + "key": "/RealOutputs/Algae/Wrist/Voltage", + "color": "#e48b32", + "show": true + } + ] }, "discrete": { "fields": [] @@ -209,7 +222,18 @@ "type": null, "factor": 1 }, - "fields": [] + "fields": [ + { + "key": "/RealOutputs/Algae/Wrist/Position", + "color": "#2b66a2", + "show": true + }, + { + "key": "/RealOutputs/Algae/Wrist/Target", + "color": "#80588e", + "show": true + } + ] } }, "title": "Wrist" @@ -262,6 +286,44 @@ }, "title": "Coral" }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Algae/Intake/Output", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Algae/Intake/Current", + "color": "#2b66a2", + "show": true + } + ] + } + }, + "title": "Intake" + }, { "type": 3, "field": "/RealOutputs/Console", From cad4e5b26d180d1b7251be4bbe263a79737026c1 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 10:00:45 -0500 Subject: [PATCH 11/18] New final controls --- src/main/java/frc/robot/Constants.java | 14 +-- src/main/java/frc/robot/Robot.java | 89 ++++++++++++------- .../controllers/DriverController.java | 38 ++++++-- src/main/java/frc/robot/subsystems/Algae.java | 27 +++++- .../java/frc/robot/subsystems/Elevator.java | 34 +++++++ .../java/frc/robot/subsystems/Subsystem.java | 4 + 6 files changed, 156 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8b74bd9..5a6746a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,8 +31,8 @@ public static class Elevator { public static final double kMaxHeight = 56.2; public static final double kGroundAlgaeHeight = 0.0; public static final double kScoreAlgaeHeight = 0.0; - public static final double kLowAlgaeHeight = 0.0; - public static final double kHighAlgaeHeight = 0.0; + public static final double kLowAlgaeHeight = 23.0; + public static final double kHighAlgaeHeight = 40.0; } public static class Coral { @@ -81,15 +81,15 @@ public static class Algae { public static final double kWristMaxAcceleration = 1380.0; public static final double kStowAngle = 233.0; - public static final double kDeAlgaeAngle = 190.0; + public static final double kDeAlgaeAngle = 215.0; public static final double kGroundIntakeAngle = 140.0; // INTAKE - public static final int kMaxIntakeCurrent = 80; + public static final int kMaxIntakeCurrent = 15; - public static final double kIntakeSpeed = 4.0; - public static final double kEjectSpeed = -4.0; - public static final double kGroundIntakeSpeed = -4.0; + public static final double kIntakeSpeed = 0.6; + public static final double kEjectSpeed = -0.3; + public static final double kGroundIntakeSpeed = -0.3; } public static class Intake { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2ec3421..7bc4b46 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -159,47 +159,72 @@ public void teleopPeriodic() { m_drive.drive(xSpeed, rot); - // ALGAE - if (m_driverController.getWantsAlgaeStow()) { - m_algae.stow(); - } else if (m_driverController.getWantsAlgaeGrab()) { - m_algae.grabAlgae(); - } else if (m_driverController.getWantsAlgaeScore()) { - m_algae.score(); - } else if (m_driverController.getWantsAlgaeGroundIntake()) { - m_algae.groundIntake(); - } - - // ELEVATOR - // m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); - if (m_operatorController.getWantsElevatorStow()) { + // FINAL CONTROLS + if (m_driverController.getWantsStow()) { m_elevator.goToElevatorStow(); - } else if (m_operatorController.getWantsElevatorL2()) { + m_algae.stow(); + } else if (m_driverController.getWantsL2()) { m_elevator.goToElevatorL2(); - } else if (m_operatorController.getWantsElevatorL3()) { + m_algae.stow(); + } else if (m_driverController.getWantsL3()) { m_elevator.goToElevatorL3(); - } else if (m_operatorController.getWantsElevatorL4()) { + m_algae.stow(); + } else if (m_driverController.getWantsL4()) { m_elevator.goToElevatorL4(); + m_algae.stow(); } - if (m_operatorController.getWantsElevatorReset()) { - RobotTelemetry.print("Resetting elevator"); - m_elevator.reset(); + if (m_driverController.getWantsScoreCoral()) { + if (m_elevator.getState() == Elevator.ElevatorState.STOW) { + m_coral.scoreL1(); + } else { + m_coral.scoreL24(); + } + } else { + m_coral.stopCoral(); } + // ALGAE + // if (m_driverController.getWantsAlgaeStow()) { + // m_algae.stow(); + // } else if (m_driverController.getWantsAlgaeGrab()) { + // m_algae.grabAlgae(); + // } else if (m_driverController.getWantsAlgaeScore()) { + // m_algae.score(); + // } else if (m_driverController.getWantsAlgaeGroundIntake()) { + // m_algae.groundIntake(); + // } + + // ELEVATOR + // m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); + // if (m_operatorController.getWantsElevatorStow()) { + // m_elevator.goToElevatorStow(); + // } else if (m_operatorController.getWantsElevatorL2()) { + // m_elevator.goToElevatorL2(); + // } else if (m_operatorController.getWantsElevatorL3()) { + // m_elevator.goToElevatorL3(); + // } else if (m_operatorController.getWantsElevatorL4()) { + // m_elevator.goToElevatorL4(); + // } + // CORAL - if (m_operatorController.getWantsCoralIntake()) { - m_coral.intake(); - } else if (m_operatorController.getWantsCoralReverse()) { - m_coral.reverse(); - } else if (m_operatorController.getWantsCoralIndex()) { - m_coral.index(); - } else if (m_operatorController.getWantsCoralL1()) { - m_coral.scoreL1(); - } else if (m_operatorController.getWantsCoralL24()) { - m_coral.scoreL24(); - } else { - m_coral.stopCoral(); + // if (m_operatorController.getWantsCoralIntake()) { + // m_coral.intake(); + // } else if (m_operatorController.getWantsCoralReverse()) { + // m_coral.reverse(); + // } else if (m_operatorController.getWantsCoralIndex()) { + // m_coral.index(); + // } else if (m_operatorController.getWantsCoralL1()) { + // m_coral.scoreL1(); + // } else if (m_operatorController.getWantsCoralL24()) { + // m_coral.scoreL24(); + // } else { + // m_coral.stopCoral(); + // } + + if (m_operatorController.getWantsElevatorReset()) { + RobotTelemetry.print("Resetting elevator"); + m_elevator.reset(); } // // Intake diff --git a/src/main/java/frc/robot/controls/controllers/DriverController.java b/src/main/java/frc/robot/controls/controllers/DriverController.java index 13773b2..3388c5e 100644 --- a/src/main/java/frc/robot/controls/controllers/DriverController.java +++ b/src/main/java/frc/robot/controls/controllers/DriverController.java @@ -28,26 +28,46 @@ public boolean getWantsSpeedMode() { return this.getFilteredAxis(2) > k_triggerActivationThreshold; } - public boolean getWantsSlowMode() { - return this.getFilteredAxis(3) > k_triggerActivationThreshold; - } + // public boolean getWantsSlowMode() { + // return this.getFilteredAxis(3) > k_triggerActivationThreshold; + // } - public boolean getWantsAlgaeStow() { - return this.getRawButton(1); + public boolean getWantsStow() { + return this.getRawButton(3); } - public boolean getWantsAlgaeGrab() { - return this.getRawButton(3); + public boolean getWantsL2() { + return this.getRawButton(1); } - public boolean getWantsAlgaeScore() { + public boolean getWantsL3() { return this.getRawButton(2); } - public boolean getWantsAlgaeGroundIntake() { + public boolean getWantsL4() { return this.getRawButton(4); } + public boolean getWantsScoreCoral() { + return this.getFilteredAxis(3) > k_triggerActivationThreshold; + } + + // public boolean getWantsAlgaeStow() { + // return this.getRawButton(1); + // } + + // public boolean getWantsAlgaeGrab() { + // return this.getRawButton(3); + // } + + // public boolean getWantsAlgaeScore() { + // return this.getRawButton(2); + // } + + // public boolean getWantsAlgaeGroundIntake() { + // return this.getRawButton(4); + // } + public void outputTelemetry() { SmartDashboard.putNumber(m_smartDashboardKey + "Forward", getForwardAxis()); SmartDashboard.putNumber(m_smartDashboardKey + "Turn", getTurnAxis()); diff --git a/src/main/java/frc/robot/subsystems/Algae.java b/src/main/java/frc/robot/subsystems/Algae.java index d39e548..579a4b9 100644 --- a/src/main/java/frc/robot/subsystems/Algae.java +++ b/src/main/java/frc/robot/subsystems/Algae.java @@ -24,6 +24,14 @@ public static Algae getInstance() { return mInstance; } + public enum IntakeState { + NONE, + INTAKE, + INDEX, + READY, + SCORE + } + private SimulatableCANSparkMax mWristMotor; private final ProfiledPIDController mWristPIDController; private final ArmFeedforward mWristFeedForward; @@ -95,8 +103,8 @@ public void periodic() { public void writePeriodicOutputs() { mWristMotor.set(mPeriodicIO.wrist_voltage); - // mIntakeMotor.set(mPeriodicIO.intake_power); - mIntakeMotor.setVoltage(mPeriodicIO.intake_power); + mIntakeMotor.set(mPeriodicIO.intake_power); + // mIntakeMotor.setVoltage(mPeriodicIO.intake_power); } @Override @@ -156,4 +164,19 @@ public double getWristAngle() { public double getWristReferenceToHorizontal() { return getWristAngle() - Constants.Algae.kWristOffset; } + + // public double getSpeedFromState(IntakeState state) { + // switch (state) { + // case NONE: + // return 0.0; + // case INTAKE: + // return RobotConstants.config.Intake.k_intakeSpeed; + // case INDEX: + // return RobotConstants.config.Intake.k_ejectSpeed; + // case READY: + // return RobotConstants.config.Intake.k_feedShooterSpeed; + // default: + // return 0.0; + // } + // } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 9dbbd8a..111777a 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -85,11 +85,23 @@ private Elevator() { mRightMotor.burnFlash(); } + public enum ElevatorState { + NONE, + STOW, + L2, + L3, + L4, + A1, + A2 + } + private static class PeriodicIO { double elevator_target = 0.0; double elevator_power = 0.0; boolean is_elevator_pos_control = false; + + ElevatorState state = ElevatorState.STOW; } /*-------------------------------- Generic Subsystem Functions --------------------------------*/ @@ -136,6 +148,8 @@ public void outputTelemetry() { putNumber("Output/Left", mLeftMotor.getAppliedOutput()); putNumber("Output/Right", mRightMotor.getAppliedOutput()); + + putNumber("State", mPeriodicIO.state); } @Override @@ -145,6 +159,10 @@ public void reset() { /*---------------------------------- Custom Public Functions ----------------------------------*/ + public ElevatorState getState() { + return mPeriodicIO.state; + } + public void setElevatorPower(double power) { putNumber("setElevatorPower", power); mPeriodicIO.is_elevator_pos_control = false; @@ -154,21 +172,37 @@ public void setElevatorPower(double power) { public void goToElevatorStow() { mPeriodicIO.is_elevator_pos_control = true; mPeriodicIO.elevator_target = Constants.Elevator.kStowHeight; + mPeriodicIO.state = ElevatorState.STOW; } public void goToElevatorL2() { mPeriodicIO.is_elevator_pos_control = true; mPeriodicIO.elevator_target = Constants.Elevator.kL2Height; + mPeriodicIO.state = ElevatorState.L2; } public void goToElevatorL3() { mPeriodicIO.is_elevator_pos_control = true; mPeriodicIO.elevator_target = Constants.Elevator.kL3Height; + mPeriodicIO.state = ElevatorState.L3; } public void goToElevatorL4() { mPeriodicIO.is_elevator_pos_control = true; mPeriodicIO.elevator_target = Constants.Elevator.kL4Height; + mPeriodicIO.state = ElevatorState.L4; + } + + public void goToAlgaeLow() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kLowAlgaeHeight; + mPeriodicIO.state = ElevatorState.A1; + } + + public void goToAlgaeHigh() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kHighAlgaeHeight; + mPeriodicIO.state = ElevatorState.A2; } /*---------------------------------- Custom Private Functions ---------------------------------*/ diff --git a/src/main/java/frc/robot/subsystems/Subsystem.java b/src/main/java/frc/robot/subsystems/Subsystem.java index 3fb75b2..18ea09a 100644 --- a/src/main/java/frc/robot/subsystems/Subsystem.java +++ b/src/main/java/frc/robot/subsystems/Subsystem.java @@ -95,4 +95,8 @@ public void putStringArray(String key, String[] value) { public void putData(String key, Sendable value) { SmartDashboard.putData(baseSmartDashboardKey + "/" + key, value); } + + public void putNumber(String key, Elevator.ElevatorState value) { + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); + } } From 75720edbe351a8056dba25cbba6a9d7cae4d4f35 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 10:32:31 -0500 Subject: [PATCH 12/18] Score coral --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 7 ++- .../controllers/DriverController.java | 4 ++ src/main/java/frc/robot/subsystems/Coral.java | 55 ++++++++++++++++++- 4 files changed, 62 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5a6746a..dd59683 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,7 +53,7 @@ public static class Coral { public static final double kReverseSpeed = -0.3; public static final double kL1Speed = 0.4; public static final double kL24Speed = 0.4; - public static final double kIndexSpeed = 0.2; + public static final double kIndexSpeed = 0.1; public static final double kSpeedDifference = kL1Speed * 0.5; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7bc4b46..a92302b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -180,9 +180,12 @@ public void teleopPeriodic() { } else { m_coral.scoreL24(); } - } else { - m_coral.stopCoral(); + } else if (m_driverController.getWantsIntakeCoral()) { + m_coral.intake(); } + // else { + // m_coral.stopCoral(); + // } // ALGAE // if (m_driverController.getWantsAlgaeStow()) { diff --git a/src/main/java/frc/robot/controls/controllers/DriverController.java b/src/main/java/frc/robot/controls/controllers/DriverController.java index 3388c5e..da658a6 100644 --- a/src/main/java/frc/robot/controls/controllers/DriverController.java +++ b/src/main/java/frc/robot/controls/controllers/DriverController.java @@ -52,6 +52,10 @@ public boolean getWantsScoreCoral() { return this.getFilteredAxis(3) > k_triggerActivationThreshold; } + public boolean getWantsIntakeCoral() { + return this.getFilteredAxis(2) > k_triggerActivationThreshold; + } + // public boolean getWantsAlgaeStow() { // return this.getRawButton(1); // } diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java index 1408da6..225709e 100644 --- a/src/main/java/frc/robot/subsystems/Coral.java +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -20,12 +20,20 @@ public static Coral getInstance() { return mInstance; } + public enum IntakeState { + NONE, + INTAKE, + REVERSE, + INDEX, + READY, + SCORE + } + private ThriftyNova mLeftMotor; private ThriftyNova mRightMotor; private PIDConfig mPIDConfig; private LaserCan mLaserCAN; - private int mLaserReading; private Coral() { super("Coral"); @@ -38,8 +46,8 @@ private Coral() { // mLeftMotor.factoryReset(); // mRightMotor.factoryReset(); - mLeftMotor.setBrakeMode(false); - mRightMotor.setBrakeMode(false); + mLeftMotor.setBrakeMode(true); + mRightMotor.setBrakeMode(true); // mLeftMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); // mRightMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); @@ -81,7 +89,11 @@ private static class PeriodicIO { double rpm = 0.0; double speed_diff = 0.0; + int index_debounce = 0; + LaserCan.Measurement measurement; + + IntakeState state = IntakeState.NONE; } /*-------------------------------- Generic Subsystem Functions --------------------------------*/ @@ -90,6 +102,7 @@ private static class PeriodicIO { public void periodic() { mPeriodicIO.measurement = mLaserCAN.getMeasurement(); + checkAutoTasks(); } @Override @@ -104,6 +117,7 @@ public void writePeriodicOutputs() { public void stop() { mPeriodicIO.rpm = 0.0; mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.state = IntakeState.NONE; } @Override @@ -122,6 +136,8 @@ public void outputTelemetry() { putNumber("Laser/ambient", measurement.ambient); putNumber("Laser/budget_ms", measurement.budget_ms); putNumber("Laser/status", measurement.status); + + putBoolean("Laser/hasCoral", isHoldingCoralViaLaserCAN()); } } @@ -132,6 +148,10 @@ public void reset() { /*---------------------------------- Custom Public Functions ----------------------------------*/ + public boolean isHoldingCoralViaLaserCAN() { + return mPeriodicIO.measurement.distance_mm < 50.0; + } + public void setSpeed(double rpm) { mPeriodicIO.speed_diff = 0.0; mPeriodicIO.rpm = rpm; @@ -140,32 +160,61 @@ public void setSpeed(double rpm) { public void intake() { mPeriodicIO.speed_diff = 0.0; mPeriodicIO.rpm = Constants.Coral.kIntakeSpeed; + mPeriodicIO.state = IntakeState.INTAKE; } public void reverse() { mPeriodicIO.speed_diff = 0.0; mPeriodicIO.rpm = Constants.Coral.kReverseSpeed; + mPeriodicIO.state = IntakeState.REVERSE; } public void index() { mPeriodicIO.speed_diff = 0.0; mPeriodicIO.rpm = Constants.Coral.kIndexSpeed; + mPeriodicIO.state = IntakeState.INDEX; } public void scoreL1() { mPeriodicIO.speed_diff = Constants.Coral.kSpeedDifference; mPeriodicIO.rpm = Constants.Coral.kL1Speed; + mPeriodicIO.state = IntakeState.SCORE; } public void scoreL24() { mPeriodicIO.speed_diff = 0.0; mPeriodicIO.rpm = Constants.Coral.kL24Speed; + mPeriodicIO.state = IntakeState.SCORE; } public void stopCoral() { mPeriodicIO.rpm = 0.0; mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.state = IntakeState.NONE; } /*---------------------------------- Custom Private Functions ---------------------------------*/ + + private void checkAutoTasks() { + switch (mPeriodicIO.state) { + case INTAKE: + if (isHoldingCoralViaLaserCAN()) { + mPeriodicIO.index_debounce++; + + if (mPeriodicIO.index_debounce > 10) { + mPeriodicIO.index_debounce = 0; + index(); + } + } + break; + case INDEX: + if (!isHoldingCoralViaLaserCAN()) { + stopCoral(); + mPeriodicIO.state = IntakeState.READY; + } + break; + default: + break; + } + } } From 566c02b81c55843e1ce4609d8aa49bcb357c8160 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 11:12:14 -0500 Subject: [PATCH 13/18] Basic algae controls --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/Robot.java | 13 ++++++++++--- .../controls/controllers/DriverController.java | 16 ++++++++++++++++ src/main/java/frc/robot/subsystems/Algae.java | 11 +---------- 4 files changed, 29 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dd59683..b17e171 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,8 +31,8 @@ public static class Elevator { public static final double kMaxHeight = 56.2; public static final double kGroundAlgaeHeight = 0.0; public static final double kScoreAlgaeHeight = 0.0; - public static final double kLowAlgaeHeight = 23.0; - public static final double kHighAlgaeHeight = 40.0; + public static final double kLowAlgaeHeight = 24.8; + public static final double kHighAlgaeHeight = 42.5; } public static class Coral { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a92302b..d576497 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -172,6 +172,16 @@ public void teleopPeriodic() { } else if (m_driverController.getWantsL4()) { m_elevator.goToElevatorL4(); m_algae.stow(); + } else if (m_driverController.getWantsA1()) { + m_elevator.goToAlgaeLow(); + m_algae.grabAlgae(); + } else if (m_driverController.getWantsA2()) { + m_elevator.goToAlgaeHigh(); + m_algae.grabAlgae(); + } else if (m_driverController.getWantsEjectAlgae()) { + m_algae.score(); + } else if (m_driverController.getWantsGroundAlgae()) { + m_algae.groundIntake(); } if (m_driverController.getWantsScoreCoral()) { @@ -183,9 +193,6 @@ public void teleopPeriodic() { } else if (m_driverController.getWantsIntakeCoral()) { m_coral.intake(); } - // else { - // m_coral.stopCoral(); - // } // ALGAE // if (m_driverController.getWantsAlgaeStow()) { diff --git a/src/main/java/frc/robot/controls/controllers/DriverController.java b/src/main/java/frc/robot/controls/controllers/DriverController.java index da658a6..1b5534e 100644 --- a/src/main/java/frc/robot/controls/controllers/DriverController.java +++ b/src/main/java/frc/robot/controls/controllers/DriverController.java @@ -56,6 +56,22 @@ public boolean getWantsIntakeCoral() { return this.getFilteredAxis(2) > k_triggerActivationThreshold; } + public boolean getWantsA1() { + return this.getHatDown(); + } + + public boolean getWantsA2() { + return this.getHatUp(); + } + + public boolean getWantsEjectAlgae() { + return this.getRawButton(6); + } + + public boolean getWantsGroundAlgae() { + return this.getRawButton(5); + } + // public boolean getWantsAlgaeStow() { // return this.getRawButton(1); // } diff --git a/src/main/java/frc/robot/subsystems/Algae.java b/src/main/java/frc/robot/subsystems/Algae.java index 579a4b9..e233a40 100644 --- a/src/main/java/frc/robot/subsystems/Algae.java +++ b/src/main/java/frc/robot/subsystems/Algae.java @@ -24,14 +24,6 @@ public static Algae getInstance() { return mInstance; } - public enum IntakeState { - NONE, - INTAKE, - INDEX, - READY, - SCORE - } - private SimulatableCANSparkMax mWristMotor; private final ProfiledPIDController mWristPIDController; private final ArmFeedforward mWristFeedForward; @@ -104,7 +96,6 @@ public void writePeriodicOutputs() { mWristMotor.set(mPeriodicIO.wrist_voltage); mIntakeMotor.set(mPeriodicIO.intake_power); - // mIntakeMotor.setVoltage(mPeriodicIO.intake_power); } @Override @@ -138,7 +129,7 @@ public void reset() { public void stow() { mPeriodicIO.wrist_target_angle = Constants.Algae.kStowAngle; - mPeriodicIO.intake_power = 0.0; + // mPeriodicIO.intake_power = 0.0; } public void grabAlgae() { From 785316387b8639e6ab04cc26e63f9d2d7272e112 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 11:57:55 -0500 Subject: [PATCH 14/18] RIP --- src/main/java/frc/robot/Constants.java | 12 ++++---- src/main/java/frc/robot/Robot.java | 9 ++++-- .../controllers/DriverController.java | 8 +++++ src/main/java/frc/robot/subsystems/Algae.java | 30 ++++++++++++++++++- src/main/java/frc/robot/subsystems/Coral.java | 11 ++++++- 5 files changed, 59 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b17e171..a6b8031 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,9 +25,9 @@ public static class Elevator { public static final double kMaxPowerDown = 0.1; public static final double kStowHeight = 0.0; - public static final double kL2Height = 9.0; - public static final double kL3Height = 25.14; - public static final double kL4Height = 51.0; + public static final double kL2Height = 9.5; + public static final double kL3Height = 25.64; + public static final double kL4Height = 52.8; public static final double kMaxHeight = 56.2; public static final double kGroundAlgaeHeight = 0.0; public static final double kScoreAlgaeHeight = 0.0; @@ -82,10 +82,10 @@ public static class Algae { public static final double kStowAngle = 233.0; public static final double kDeAlgaeAngle = 215.0; - public static final double kGroundIntakeAngle = 140.0; + public static final double kGroundIntakeAngle = 162.0; // INTAKE - public static final int kMaxIntakeCurrent = 15; + public static final int kMaxIntakeCurrent = 20; public static final double kIntakeSpeed = 0.6; public static final double kEjectSpeed = -0.3; @@ -175,7 +175,7 @@ public static class Field { } public static class LEDs { - public static final int k_PWMId = 0; + public static final int k_PWMId = 9; public static final int k_totalLength = 300; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d576497..b2e2c73 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -83,7 +83,7 @@ public void robotInit() { m_allSubsystems.add(m_algae); m_allSubsystems.add(m_elevator); - // m_allSubsystems.add(m_leds); + m_allSubsystems.add(m_leds); // Set up the Field2d object for simulation SmartDashboard.putData("Field", m_field); @@ -137,6 +137,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + m_leds.breathe(); } double speed = 0; @@ -162,7 +163,7 @@ public void teleopPeriodic() { // FINAL CONTROLS if (m_driverController.getWantsStow()) { m_elevator.goToElevatorStow(); - m_algae.stow(); + // m_algae.stow(); } else if (m_driverController.getWantsL2()) { m_elevator.goToElevatorL2(); m_algae.stow(); @@ -178,6 +179,8 @@ public void teleopPeriodic() { } else if (m_driverController.getWantsA2()) { m_elevator.goToAlgaeHigh(); m_algae.grabAlgae(); + } else if (m_driverController.getWantsStopAlgae()) { + m_algae.stopAlgae(); } else if (m_driverController.getWantsEjectAlgae()) { m_algae.score(); } else if (m_driverController.getWantsGroundAlgae()) { @@ -232,7 +235,7 @@ public void teleopPeriodic() { // m_coral.stopCoral(); // } - if (m_operatorController.getWantsElevatorReset()) { + if (m_operatorController.getWantsElevatorReset() || m_driverController.getWantsElevatorReset()) { RobotTelemetry.print("Resetting elevator"); m_elevator.reset(); } diff --git a/src/main/java/frc/robot/controls/controllers/DriverController.java b/src/main/java/frc/robot/controls/controllers/DriverController.java index 1b5534e..eed120e 100644 --- a/src/main/java/frc/robot/controls/controllers/DriverController.java +++ b/src/main/java/frc/robot/controls/controllers/DriverController.java @@ -64,6 +64,14 @@ public boolean getWantsA2() { return this.getHatUp(); } + public boolean getWantsStopAlgae() { + return this.getHatRight(); + } + + public boolean getWantsElevatorReset() { + return this.getRawButton(7); + } + public boolean getWantsEjectAlgae() { return this.getRawButton(6); } diff --git a/src/main/java/frc/robot/subsystems/Algae.java b/src/main/java/frc/robot/subsystems/Algae.java index e233a40..64d659d 100644 --- a/src/main/java/frc/robot/subsystems/Algae.java +++ b/src/main/java/frc/robot/subsystems/Algae.java @@ -24,6 +24,13 @@ public static Algae getInstance() { return mInstance; } + public enum IntakeState { + NONE, + STOW, + DEALGAE, + GROUND + } + private SimulatableCANSparkMax mWristMotor; private final ProfiledPIDController mWristPIDController; private final ArmFeedforward mWristFeedForward; @@ -78,6 +85,8 @@ private static class PeriodicIO { double wrist_voltage = 0.0; double intake_power = 0.0; + + IntakeState state = IntakeState.STOW; } /*-------------------------------- Generic Subsystem Functions --------------------------------*/ @@ -129,21 +138,36 @@ public void reset() { public void stow() { mPeriodicIO.wrist_target_angle = Constants.Algae.kStowAngle; + + mPeriodicIO.state = IntakeState.STOW; // mPeriodicIO.intake_power = 0.0; } public void grabAlgae() { mPeriodicIO.wrist_target_angle = Constants.Algae.kDeAlgaeAngle; mPeriodicIO.intake_power = Constants.Algae.kIntakeSpeed; + + mPeriodicIO.state = IntakeState.DEALGAE; } public void score() { - mPeriodicIO.intake_power = Constants.Algae.kEjectSpeed; + if (mPeriodicIO.state == IntakeState.GROUND) { + mPeriodicIO.intake_power = -Constants.Algae.kEjectSpeed; + } else { + mPeriodicIO.intake_power = Constants.Algae.kEjectSpeed; + } } public void groundIntake() { mPeriodicIO.wrist_target_angle = Constants.Algae.kGroundIntakeAngle; mPeriodicIO.intake_power = Constants.Algae.kGroundIntakeSpeed; + + mPeriodicIO.state = IntakeState.GROUND; + } + + public void stopAlgae() { + mPeriodicIO.intake_power = 0.0; + mPeriodicIO.wrist_target_angle = Constants.Algae.kStowAngle; } /*---------------------------------- Custom Private Functions ---------------------------------*/ @@ -156,6 +180,10 @@ public double getWristReferenceToHorizontal() { return getWristAngle() - Constants.Algae.kWristOffset; } + public IntakeState getState() { + return mPeriodicIO.state; + } + // public double getSpeedFromState(IntakeState state) { // switch (state) { // case NONE: diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java index 225709e..f768ca5 100644 --- a/src/main/java/frc/robot/subsystems/Coral.java +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -5,13 +5,16 @@ import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; +import edu.wpi.first.wpilibj.util.Color; import frc.robot.Constants; +import frc.robot.subsystems.leds.LEDs; public class Coral extends Subsystem { /*-------------------------------- Private instance variables ---------------------------------*/ private static Coral mInstance; private PeriodicIO mPeriodicIO; + public final LEDs m_leds = LEDs.getInstance(); public static Coral getInstance() { if (mInstance == null) { @@ -149,7 +152,7 @@ public void reset() { /*---------------------------------- Custom Public Functions ----------------------------------*/ public boolean isHoldingCoralViaLaserCAN() { - return mPeriodicIO.measurement.distance_mm < 50.0; + return mPeriodicIO.measurement.distance_mm < 75.0; } public void setSpeed(double rpm) { @@ -161,6 +164,8 @@ public void intake() { mPeriodicIO.speed_diff = 0.0; mPeriodicIO.rpm = Constants.Coral.kIntakeSpeed; mPeriodicIO.state = IntakeState.INTAKE; + + m_leds.setColor(Color.kYellow); } public void reverse() { @@ -173,6 +178,8 @@ public void index() { mPeriodicIO.speed_diff = 0.0; mPeriodicIO.rpm = Constants.Coral.kIndexSpeed; mPeriodicIO.state = IntakeState.INDEX; + + m_leds.setColor(Color.kBlue); } public void scoreL1() { @@ -210,7 +217,9 @@ private void checkAutoTasks() { case INDEX: if (!isHoldingCoralViaLaserCAN()) { stopCoral(); + mPeriodicIO.state = IntakeState.READY; + m_leds.setColor(Color.kBlue); } break; default: From 0a27b3be96d747803f9536b2291e5de8ada96ed6 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 12:07:30 -0500 Subject: [PATCH 15/18] Switch to SparkMax for Coral --- src/main/java/frc/robot/subsystems/Coral.java | 72 +++++++++++++------ 1 file changed, 50 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java index f768ca5..70163af 100644 --- a/src/main/java/frc/robot/subsystems/Coral.java +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -1,12 +1,15 @@ package frc.robot.subsystems; -import com.thethriftybot.ThriftyNova; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkPIDController; import com.thethriftybot.ThriftyNova.PIDConfig; import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; import edu.wpi.first.wpilibj.util.Color; import frc.robot.Constants; +import frc.robot.simulation.SimulatableCANSparkMax; import frc.robot.subsystems.leds.LEDs; public class Coral extends Subsystem { @@ -32,8 +35,13 @@ public enum IntakeState { SCORE } - private ThriftyNova mLeftMotor; - private ThriftyNova mRightMotor; + // private ThriftyNova mLeftMotor; + // private ThriftyNova mRightMotor; + private SimulatableCANSparkMax mLeftMotor; + private SparkPIDController mLeftPIDController; + private SimulatableCANSparkMax mRightMotor; + private SparkPIDController mRightPIDController; + private PIDConfig mPIDConfig; private LaserCan mLaserCAN; @@ -43,14 +51,30 @@ private Coral() { mPeriodicIO = new PeriodicIO(); - mLeftMotor = new ThriftyNova(Constants.Coral.kLeftMotorId); - mRightMotor = new ThriftyNova(Constants.Coral.kRightMotorId); + // mLeftMotor = new ThriftyNova(Constants.Coral.kLeftMotorId); + // mRightMotor = new ThriftyNova(Constants.Coral.kRightMotorId); + mLeftMotor = new SimulatableCANSparkMax(Constants.Coral.kLeftMotorId, MotorType.kBrushless); + mRightMotor = new SimulatableCANSparkMax(Constants.Coral.kRightMotorId, MotorType.kBrushless); // mLeftMotor.factoryReset(); // mRightMotor.factoryReset(); - mLeftMotor.setBrakeMode(true); - mRightMotor.setBrakeMode(true); + // mLeftMotor.setBrakeMode(true); + // mRightMotor.setBrakeMode(true); + mLeftMotor.setIdleMode(IdleMode.kBrake); + mRightMotor.setIdleMode(IdleMode.kBrake); + + // mLeftPIDController = mLeftMotor.getPIDController(); + // mLeftPIDController.setP(Constants.Coral.kP); + // mLeftPIDController.setI(Constants.Coral.kI); + // mLeftPIDController.setD(Constants.Coral.kD); + // mLeftPIDController.setIZone(Constants.Coral.kIZone); + + // mRightPIDController = mRightMotor.getPIDController(); + // mRightPIDController.setP(Constants.Coral.kP); + // mRightPIDController.setI(Constants.Coral.kI); + // mRightPIDController.setD(Constants.Coral.kD); + // mRightPIDController.setIZone(Constants.Coral.kIZone); // mLeftMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); // mRightMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); @@ -61,17 +85,17 @@ private Coral() { // mLeftMotor.setMaxCurrent(CurrentType.SUPPLY, 200.0); // mRightMotor.setMaxCurrent(CurrentType.SUPPLY, 200.0); - mLeftMotor.setRampUp(0.5); - mRightMotor.setRampUp(0.5); + // mLeftMotor.setRampUp(0.5); + // mRightMotor.setRampUp(0.5); - mLeftMotor.setMaxOutput(1.0); - mRightMotor.setMaxOutput(1.0); + // mLeftMotor.setMaxOutput(1.0); + // mRightMotor.setMaxOutput(1.0); - mPIDConfig = mRightMotor.pid0; - mPIDConfig.setP(Constants.Coral.kP); - mPIDConfig.setI(Constants.Coral.kI); - mPIDConfig.setD(Constants.Coral.kD); - mPIDConfig.setIZone(Constants.Coral.kIZone); + // mPIDConfig = mRightMotor.pid0; + // mPIDConfig.setP(Constants.Coral.kP); + // mPIDConfig.setI(Constants.Coral.kI); + // mPIDConfig.setD(Constants.Coral.kD); + // mPIDConfig.setIZone(Constants.Coral.kIZone); // TODO: Make sure we're inverting the correct motor // mLeftMotor.setInverted(true); @@ -112,8 +136,12 @@ public void periodic() { public void writePeriodicOutputs() { // mLeftMotor.setVelocity(mPeriodicIO.rpm - mPeriodicIO.speed_diff); // mRightMotor.setVelocity(mPeriodicIO.rpm); - mLeftMotor.setPercent(mPeriodicIO.rpm - mPeriodicIO.speed_diff); - mRightMotor.setPercent(-mPeriodicIO.rpm); + + // mLeftMotor.setPercent(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + // mRightMotor.setPercent(-mPeriodicIO.rpm); + + mLeftMotor.set(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + mRightMotor.set(-mPeriodicIO.rpm); } @Override @@ -127,11 +155,11 @@ public void stop() { public void outputTelemetry() { putNumber("RPM/target", mPeriodicIO.rpm); - putNumber("RPM/Left/Position", mLeftMotor.getPosition()); - putNumber("RPM/Right/Position", mRightMotor.getPosition()); + // putNumber("RPM/Left/Position", mLeftMotor.getPosition()); + // putNumber("RPM/Right/Position", mRightMotor.getPosition()); - putNumber("RPM/Left/Velocity", mLeftMotor.getVelocity()); - putNumber("RPM/Right/Velocity", mRightMotor.getVelocity()); + // putNumber("RPM/Left/Velocity", mLeftMotor.getVelocity()); + // putNumber("RPM/Right/Velocity", mRightMotor.getVelocity()); LaserCan.Measurement measurement = mPeriodicIO.measurement; if (measurement != null) { From e390af8aab86fbfd92aa1617c8db33698512a0d7 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 7 Jan 2025 13:40:26 -0500 Subject: [PATCH 16/18] Fix set points --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a6b8031..97dd26f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,9 +25,9 @@ public static class Elevator { public static final double kMaxPowerDown = 0.1; public static final double kStowHeight = 0.0; - public static final double kL2Height = 9.5; - public static final double kL3Height = 25.64; - public static final double kL4Height = 52.8; + public static final double kL2Height = 9.0; + public static final double kL3Height = 25.14; + public static final double kL4Height = 52.0; public static final double kMaxHeight = 56.2; public static final double kGroundAlgaeHeight = 0.0; public static final double kScoreAlgaeHeight = 0.0; From 02ff87763588226c11d75cfb2c359460f9772e86 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Tue, 7 Jan 2025 14:07:53 -0500 Subject: [PATCH 17/18] Elevator motion profiling (#12) * Add motion profile to elevator * tune elevator * Fix edge case * remove import --- src/main/java/frc/robot/Constants.java | 7 +++-- .../java/frc/robot/subsystems/Elevator.java | 28 ++++++++++++++++++- 2 files changed, 32 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 97dd26f..8bcaa80 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,12 +14,15 @@ public static class Elevator { public static final int kElevatorLeftMotorId = 9; public static final int kElevatorRightMotorId = 10; - public static final double kP = 0.030; - public static final double kI = 0.00005; + public static final double kP = 0.15; + public static final double kI = 0; public static final double kD = 0.0; public static final double kIZone = 5.0; public static final double kG = 0.5; + public static final double kMaxVelocity = 65; + public static final double kMaxAcceleration = 200; + public static final int kMaxCurrent = 40; public static final double kMaxPowerUp = 0.1; public static final double kMaxPowerDown = 0.1; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 111777a..8982949 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -7,6 +7,8 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; import frc.robot.simulation.SimulatableCANSparkMax; @@ -34,6 +36,11 @@ public static Elevator getInstance() { private RelativeEncoder mRightEncoder; private SparkPIDController mRightPIDController; + private TrapezoidProfile mProfile; + private TrapezoidProfile.State mCurState = new TrapezoidProfile.State(); + private TrapezoidProfile.State mGoalState = new TrapezoidProfile.State(); + private double prevUpdateTime = Timer.getFPGATimestamp(); + // private RelativeEncoder mLeftEncoder; // private SparkMaxLimitSwitch mLowerLimit; // private SparkMaxLimitSwitch mUpperLimit; @@ -83,6 +90,8 @@ private Elevator() { mLeftMotor.burnFlash(); mRightMotor.burnFlash(); + + mProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(Constants.Elevator.kMaxVelocity, Constants.Elevator.kMaxAcceleration)); } public enum ElevatorState { @@ -118,14 +127,27 @@ public void periodic() { @Override public void writePeriodicOutputs() { + double curTime = Timer.getFPGATimestamp(); + double dt = curTime - prevUpdateTime; + prevUpdateTime = curTime; if (mPeriodicIO.is_elevator_pos_control) { + // Update goal + mGoalState.position = mPeriodicIO.elevator_target; + + // Calculate new state + prevUpdateTime = curTime; + mCurState = mProfile.calculate(dt, mCurState, mGoalState); + + // Set PID controller to new state mLeftPIDController.setReference( - mPeriodicIO.elevator_target, + mCurState.position, CANSparkMax.ControlType.kPosition, 0, Constants.Elevator.kG, ArbFFUnits.kVoltage); } else { + mCurState.position = mRightEncoder.getPosition(); + mCurState.velocity = 0; mLeftMotor.set(mPeriodicIO.elevator_power); } } @@ -142,6 +164,10 @@ public void stop() { public void outputTelemetry() { putNumber("Position/Current", mRightEncoder.getPosition()); putNumber("Position/Target", mPeriodicIO.elevator_target); + putNumber("Velocity/Current", mRightEncoder.getVelocity()); + + putNumber("Position/Setpoint", mCurState.position); + putNumber("Velocity/Setpoint", mCurState.velocity); putNumber("Current/Left", mLeftMotor.getOutputCurrent()); putNumber("Current/Right", mRightMotor.getOutputCurrent()); From c7487649d7ee9452950a4355862269951eccae80 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Wed, 8 Jan 2025 20:32:04 -0500 Subject: [PATCH 18/18] Add tippy mode --- src/main/java/frc/robot/Robot.java | 6 +++++- src/main/java/frc/robot/subsystems/Drivetrain.java | 13 ++++++++++++- src/main/java/frc/robot/subsystems/Elevator.java | 13 +++++++------ 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b2e2c73..c888356 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.autonomous.AutoRunner; @@ -195,6 +196,7 @@ public void teleopPeriodic() { } } else if (m_driverController.getWantsIntakeCoral()) { m_coral.intake(); + m_elevator.goToElevatorStow(); } // ALGAE @@ -281,7 +283,9 @@ public void teleopPeriodic() { @Override public void disabledInit() { - m_leds.rainbow(); + // m_leds.rainbow(); + m_leds.setColor(Color.kRed); + speed = 0; m_allSubsystems.forEach(subsystem -> subsystem.stop()); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 5451807..2faf97f 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -54,6 +54,8 @@ public class Drivetrain extends Subsystem { private static final double kSlowModeRotScale = 0.1; private static final double kSpeedModeScale = 2.0; + private static final double kTippyModeScale = 0.7; + private static final double kTrackWidth = Units.inchesToMeters(20.75); private static final double kWheelRadius = Units.inchesToMeters(3.0); private static final double kGearRatio = 10.71; @@ -77,6 +79,7 @@ public class Drivetrain extends Subsystem { Constants.Drive.kD); private final AHRS mGyro = new AHRS(); + private final Elevator mElevator = Elevator.getInstance(); private final DifferentialDriveKinematics mKinematics = new DifferentialDriveKinematics(kTrackWidth); @@ -270,7 +273,15 @@ public void drive(double xSpeed, double rot) { mPeriodicIO.diffWheelSpeeds = mKinematics .toWheelSpeeds(new ChassisSpeeds(xSpeed * kSpeedModeScale, 0, rot * kSlowModeRotScale)); } else { - mPeriodicIO.diffWheelSpeeds = mKinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)); + Elevator.ElevatorState state = mElevator.getState(); + + boolean highSetPoint = state == Elevator.ElevatorState.L4 || + state == Elevator.ElevatorState.L3 || + state == Elevator.ElevatorState.A2; + + double scale = (highSetPoint ? kTippyModeScale : 1.0); + + mPeriodicIO.diffWheelSpeeds = mKinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed * scale, 0, rot)); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 8982949..06de782 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -91,7 +91,8 @@ private Elevator() { mLeftMotor.burnFlash(); mRightMotor.burnFlash(); - mProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(Constants.Elevator.kMaxVelocity, Constants.Elevator.kMaxAcceleration)); + mProfile = new TrapezoidProfile( + new TrapezoidProfile.Constraints(Constants.Elevator.kMaxVelocity, Constants.Elevator.kMaxAcceleration)); } public enum ElevatorState { @@ -146,7 +147,7 @@ public void writePeriodicOutputs() { Constants.Elevator.kG, ArbFFUnits.kVoltage); } else { - mCurState.position = mRightEncoder.getPosition(); + mCurState.position = mLeftEncoder.getPosition(); mCurState.velocity = 0; mLeftMotor.set(mPeriodicIO.elevator_power); } @@ -162,10 +163,10 @@ public void stop() { @Override public void outputTelemetry() { - putNumber("Position/Current", mRightEncoder.getPosition()); + putNumber("Position/Current", mLeftEncoder.getPosition()); putNumber("Position/Target", mPeriodicIO.elevator_target); - putNumber("Velocity/Current", mRightEncoder.getVelocity()); - + putNumber("Velocity/Current", mLeftEncoder.getVelocity()); + putNumber("Position/Setpoint", mCurState.position); putNumber("Velocity/Setpoint", mCurState.velocity); @@ -180,7 +181,7 @@ public void outputTelemetry() { @Override public void reset() { - mRightEncoder.setPosition(0.0); + mLeftEncoder.setPosition(0.0); } /*---------------------------------- Custom Public Functions ----------------------------------*/