diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3c4e11b..d995eab 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -55,6 +55,41 @@ public static final class FlywheelConstants { // CAN ID's public static final int kTopFlywheelSparkMaxCanId = 13; public static final int kBottomFlywheelSparkMaxCanId = 14; + + // TODO implement the below + public static final int kCurrentLimit = 30; + + public static final int startingAngle = 30; // TODO TUNE + + public static final double armConversion = 1; // TODO TUNE + + // Arm PID constants + public static final double kP = 0.1; // TODO TUNE + public static final double kI = 0.0; // TODO TUNE + public static final double kD = 0.0; // TODO TUNE + + // Arm Feedforward characterization constants + public static final double ks = 0.10; // TODO TUNE + public static final double kv = 0.05; // TODO TUNE + } + + public static final class IntakeConstants { + public static final int kIntakeSparkMaxCanId = 12341; // ! Change before testing + + public static final int kCurrentLimit = 30; + + public static final int startingAngle = 30; // TODO TUNE + + public static final double armConversion = 1; // TODO TUNE + + // Arm PID constants + public static final double kP = 0.1; // TODO TUNE + public static final double kI = 0.0; // TODO TUNE + public static final double kD = 0.0; // TODO TUNE + + // Arm Feedforward characterization constants + public static final double ks = 0.10; // TODO TUNE + public static final double kv = 0.05; // TODO TUNE } public static final class ArmConstants { @@ -111,7 +146,7 @@ public static final class Presets { } public static final class WristConstants { - public static final int kWristMotorId = 103; // ! Change before testing + public static final int kWristMotorId = 104; // ! Change before testing public static final int kCurrentLimit = 30; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 476361c..5218901 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,12 +22,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; +import frc.robot.commands.DriveParts.DriveCommands; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmIO; import frc.robot.subsystems.arm.ArmIOSim; import frc.robot.subsystems.arm.ArmIOSparkMax; +import frc.robot.subsystems.armElevator.ArmElevator; +import frc.robot.subsystems.armElevator.ArmElevatorIO; +import frc.robot.subsystems.armElevator.ArmElevatorIOSim; +import frc.robot.subsystems.armElevator.ArmElevatorIOSparkMax; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; @@ -38,7 +42,12 @@ import frc.robot.subsystems.flywheel.Flywheel; import frc.robot.subsystems.flywheel.FlywheelIO; import frc.robot.subsystems.flywheel.FlywheelIOSim; -import frc.robot.subsystems.flywheel.TopFlywheelIOSparkMax; +import frc.robot.subsystems.flywheel.FlywheelIOSparkMax; +import frc.robot.subsystems.wrist.Wrist; +import frc.robot.subsystems.wrist.WristIO; +import frc.robot.subsystems.wrist.WristIOSim; +import frc.robot.subsystems.wrist.WristIOSparkMax; + import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -54,6 +63,8 @@ public class RobotContainer { private final Flywheel sTopFlywheel; private final Flywheel sBottomFlywheel; private final Arm sArm; + private final ArmElevator sArmElevator; + private final Wrist sWrist; // Controller private final CommandXboxController driverController = new CommandXboxController(0); @@ -79,9 +90,11 @@ public RobotContainer() { new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); - sTopFlywheel = new Flywheel(new TopFlywheelIOSparkMax()); + sTopFlywheel = new Flywheel(new FlywheelIOSparkMax()); sBottomFlywheel = new Flywheel(new BottomFlywheelIOSparkMax()); sArm = new Arm(new ArmIOSparkMax() {}); + sArmElevator = new ArmElevator(new ArmElevatorIOSparkMax() {}); + sWrist = new Wrist(new WristIOSparkMax() {}); // drive = new Drive( // new GyroIOPigeon2(true), // new ModuleIOTalonFX(0), @@ -103,6 +116,8 @@ public RobotContainer() { sTopFlywheel = new Flywheel(new FlywheelIOSim()); sBottomFlywheel = new Flywheel(new FlywheelIOSim()); sArm = new Arm(new ArmIOSim() {}); + sArmElevator = new ArmElevator(new ArmElevatorIOSim() {}); + sWrist = new Wrist(new WristIOSim() {}); break; @@ -118,6 +133,9 @@ public RobotContainer() { sTopFlywheel = new Flywheel(new FlywheelIO() {}); sBottomFlywheel = new Flywheel(new FlywheelIO() {}); sArm = new Arm(new ArmIO() {}); + sArmElevator = new ArmElevator(new ArmElevatorIO() {}); + sWrist = new Wrist(new WristIO() {}); + break; } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveParts/DriveCommands.java similarity index 98% rename from src/main/java/frc/robot/commands/DriveCommands.java rename to src/main/java/frc/robot/commands/DriveParts/DriveCommands.java index e8d4faa..6ebae2b 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveParts/DriveCommands.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.commands; +package frc.robot.commands.DriveParts; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java new file mode 100644 index 0000000..e61739b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java @@ -0,0 +1,91 @@ +// Copyright 2021-2024 FRC 6328, FRC 5829 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.flywheel; + +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants.FlywheelConstants; + +/** + * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with + * "CANSparkFlex". + */ +public class FlywheelIOSparkMax implements FlywheelIO { + private static final double GEAR_RATIO = 52.0 / 34.0; // May be reciprocal + + private final CANSparkMax topShooterMotor = + new CANSparkMax(FlywheelConstants.kTopFlywheelSparkMaxCanId, MotorType.kBrushless); + private final RelativeEncoder topShooterEncoder = topShooterMotor.getEncoder(); + private final SparkPIDController topShooterPID = topShooterMotor.getPIDController(); + + private final CANSparkMax bottomShooterMotor = + new CANSparkMax(FlywheelConstants.kBottomFlywheelSparkMaxCanId, MotorType.kBrushless); + private final RelativeEncoder bottomShooterEncoder = bottomShooterMotor.getEncoder(); + private final SparkPIDController bottomShooterPID = bottomShooterMotor.getPIDController(); + + public FlywheelIOSparkMax() { + topShooterMotor.restoreFactoryDefaults(); + + topShooterMotor.setCANTimeout(250); + + topShooterMotor.setInverted(false); + + topShooterMotor.enableVoltageCompensation(12.0); + topShooterMotor.setSmartCurrentLimit(30); + + topShooterMotor.burnFlash(); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + inputs.positionRad = Units.rotationsToRadians(topShooterEncoder.getPosition() / GEAR_RATIO); + inputs.velocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(topShooterEncoder.getVelocity() / GEAR_RATIO); + inputs.appliedVolts = topShooterMotor.getAppliedOutput() * topShooterMotor.getBusVoltage(); + inputs.currentAmps = new double[] {topShooterMotor.getOutputCurrent()}; + } + + @Override + public void setVoltage(double volts) { + topShooterMotor.setVoltage(volts); + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + topShooterPID.setReference( + Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, + ControlType.kVelocity, + 0, + ffVolts, + ArbFFUnits.kVoltage); + } + + @Override + public void stop() { + topShooterMotor.stopMotor(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + topShooterPID.setP(kP, 0); + topShooterPID.setI(kI, 0); + topShooterPID.setD(kD, 0); + topShooterPID.setFF(0, 0); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..7fdb666 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,85 @@ +// Copyright 2021-2024 FRC 6328, FRC 5829 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.EnvironmentalConstants; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Intake extends SubsystemBase { + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + private final SimpleMotorFeedforward ffModel; + + /** Creates a new Flywheel. */ + public Intake(IntakeIO io) { + this.io = io; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (EnvironmentalConstants.currentMode) { + case REAL: + case REPLAY: + ffModel = new SimpleMotorFeedforward(0.1, 0.05); + io.configurePID(1.0, 0.0, 0.0); + break; + case SIM: + ffModel = new SimpleMotorFeedforward(0.0, 0.03); + io.configurePID(0.5, 0.0, 0.0); + break; + default: + ffModel = new SimpleMotorFeedforward(0.0, 0.0); + break; + } + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Flywheel", inputs); + } + + /** Run open loop at the specified voltage. */ + public void runVolts(double volts) { + io.setVoltage(volts); + } + + /** Run closed loop at the specified velocity. */ + public void runVelocity(double velocityRPM) { + var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); + io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); + + // Log flywheel setpoint + Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); + } + + /** Stops the flywheel. */ + public void stop() { + io.stop(); + } + + /** Returns the current velocity in RPM. */ + @AutoLogOutput + public double getVelocityRPM() { + return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); + } + + /** Returns the current velocity in radians per second. */ + public double getCharacterizationVelocity() { + return inputs.velocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..5c52077 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,41 @@ +// Copyright 2021-2024 FRC 6328, FRC 5829 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double[] currentAmps = new double[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(IntakeIOInputs inputs) {} + + /** Run open loop at the specified voltage. */ + public default void setVoltage(double volts) {} + + /** Run closed loop at the specified velocity. */ + public default void setVelocity(double velocityRadPerSec, double ffVolts) {} + + /** Stop in open loop. */ + public default void stop() {} + + /** Set velocity PID constants. */ + public default void configurePID(double kP, double kI, double kD) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..80cbd5b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,68 @@ +// Copyright 2021-2024 FRC 6328, FRC 5829 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +public class IntakeIOSim implements IntakeIO { + private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); + private PIDController pid = new PIDController(0.0, 0.0, 0.0); + + private boolean closedLoop = false; + private double ffVolts = 0.0; + private double appliedVolts = 0.0; + + @Override + public void updateInputs(IntakeIOInputs inputs) { + if (closedLoop) { + appliedVolts = + MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); + sim.setInputVoltage(appliedVolts); + } + + sim.update(0.02); + + inputs.positionRad = 0.0; + inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; + } + + @Override + public void setVoltage(double volts) { + closedLoop = false; + appliedVolts = 0.0; + sim.setInputVoltage(volts); + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + closedLoop = true; + pid.setSetpoint(velocityRadPerSec); + this.ffVolts = ffVolts; + } + + @Override + public void stop() { + setVoltage(0.0); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/TopFlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java similarity index 54% rename from src/main/java/frc/robot/subsystems/flywheel/TopFlywheelIOSparkMax.java rename to src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index f83f2af..e29d9d1 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/TopFlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -11,7 +11,7 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. -package frc.robot.subsystems.flywheel; +package frc.robot.subsystems.intake; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -20,50 +20,51 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.math.util.Units; -import frc.robot.Constants.FlywheelConstants; +import frc.robot.Constants.IntakeConstants; /** * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with * "CANSparkFlex". */ -public class TopFlywheelIOSparkMax implements FlywheelIO { - private static final double GEAR_RATIO = 52.0 / 34.0; // May be recipricol +public class IntakeIOSparkMax implements IntakeIO { + private static final double GEAR_RATIO = 52.0 / 34.0; // May be reciprocal - private final CANSparkMax leader = - new CANSparkMax(FlywheelConstants.kTopFlywheelSparkMaxCanId, MotorType.kBrushless); - private final RelativeEncoder encoder = leader.getEncoder(); - private final SparkPIDController pid = leader.getPIDController(); + private final CANSparkMax intakeMotor = + new CANSparkMax(IntakeConstants.kIntakeSparkMaxCanId, MotorType.kBrushless); + private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); + private final SparkPIDController intakePID = intakeMotor.getPIDController(); - public TopFlywheelIOSparkMax() { - leader.restoreFactoryDefaults(); - leader.setCANTimeout(250); + public IntakeIOSparkMax() { + intakeMotor.restoreFactoryDefaults(); - leader.setInverted(false); + intakeMotor.setCANTimeout(250); - leader.enableVoltageCompensation(12.0); - leader.setSmartCurrentLimit(30); + intakeMotor.setInverted(false); - leader.burnFlash(); + intakeMotor.enableVoltageCompensation(12.0); + intakeMotor.setSmartCurrentLimit(30); + + intakeMotor.burnFlash(); } @Override - public void updateInputs(FlywheelIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); + public void updateInputs(IntakeIOInputs inputs) { + inputs.positionRad = Units.rotationsToRadians(intakeEncoder.getPosition() / GEAR_RATIO); inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); - inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.currentAmps = new double[] {leader.getOutputCurrent()}; + Units.rotationsPerMinuteToRadiansPerSecond(intakeEncoder.getVelocity() / GEAR_RATIO); + inputs.appliedVolts = intakeMotor.getAppliedOutput() * intakeMotor.getBusVoltage(); + inputs.currentAmps = new double[] {intakeMotor.getOutputCurrent()}; } @Override public void setVoltage(double volts) { - leader.setVoltage(volts); + intakeMotor.setVoltage(volts); } @Override public void setVelocity(double velocityRadPerSec, double ffVolts) { - pid.setReference( + intakePID.setReference( Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, ControlType.kVelocity, 0, @@ -73,14 +74,14 @@ public void setVelocity(double velocityRadPerSec, double ffVolts) { @Override public void stop() { - leader.stopMotor(); + intakeMotor.stopMotor(); } @Override public void configurePID(double kP, double kI, double kD) { - pid.setP(kP, 0); - pid.setI(kI, 0); - pid.setD(kD, 0); - pid.setFF(0, 0); + intakePID.setP(kP, 0); + intakePID.setI(kI, 0); + intakePID.setD(kD, 0); + intakePID.setFF(0, 0); } } diff --git a/src/main/java/frc/robot/util/math/TalonConversions.java b/src/main/java/frc/robot/util/math/TalonConversions.java index 42d5143..37f4771 100644 --- a/src/main/java/frc/robot/util/math/TalonConversions.java +++ b/src/main/java/frc/robot/util/math/TalonConversions.java @@ -1,3 +1,15 @@ +// Copyright 2016-2024 FRC 5829 +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + package frc.robot.util.math; public class TalonConversions {