diff --git a/Hardware.md b/Hardware.md index cf8f011..2dcdcda 100644 --- a/Hardware.md +++ b/Hardware.md @@ -21,7 +21,7 @@ This document details all of the hardware (motors, motor controllers, sensors, e | 12 | Intake R | NEO | SPARK MAX | ? | | 13 | Shooter L | NEO | SPARK MAX | ? | | 14 | Shooter R | NEO | SPARK MAX | ? | - +| 15 | Feeder | NEO 550 | SPARK MAX | ? | diff --git a/simgui.json b/simgui.json index d9e0256..0f0e174 100644 --- a/simgui.json +++ b/simgui.json @@ -5,7 +5,18 @@ } }, "NetworkTables": { + "Retained Values": { + "open": false + }, "transitory": { + "Shuffleboard": { + ".metadata": { + "open": true + }, + ".recording": { + "open": true + } + }, "SmartDashboard": { "open": true } diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java index ec8e58d..b6e354b 100644 --- a/src/main/java/org/ghrobotics/frc2024/Robot.java +++ b/src/main/java/org/ghrobotics/frc2024/Robot.java @@ -5,10 +5,26 @@ package org.ghrobotics.frc2024; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; +// import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +import org.ghrobotics.frc2024.Superstructure.Position; +import org.ghrobotics.frc2024.commands.ArmPID; import org.ghrobotics.frc2024.commands.DriveTeleop; +import org.ghrobotics.frc2024.subsystems.Arm; +// import org.ghrobotics.frc2024.subsystems.Climber; import org.ghrobotics.frc2024.subsystems.Drive; +import org.ghrobotics.frc2024.subsystems.Feeder; +import org.ghrobotics.frc2024.subsystems.Intake; +import org.ghrobotics.frc2024.subsystems.Shooter; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -19,24 +35,65 @@ public class Robot extends TimedRobot { // Subsystems private final Drive drive_ = new Drive(); + private final Arm arm_ = new Arm(); + // private final Climber climber_ = new Climber(); + private final Intake intake_ = new Intake(); + private final Shooter shooter_ = new Shooter(); + private final Feeder feeder_ = new Feeder(); + // private final ArmPID arm_command = new ArmPID(); + // Robot State private final RobotState robot_state_ = new RobotState(drive_); + + // Telemetry + private final Telemetry telemetry_ = new Telemetry(robot_state_, arm_); + + private double brake_value_ = 0.05; - // Xbox Controller + // Controller private final CommandXboxController driver_controller_ = new CommandXboxController(0); + private final CommandXboxController operator_controller_ = new CommandXboxController(1); + + + // Trigger hold_position = operator_controller_.rightTrigger(0.2); + + // Playstation controller just for testing + // private final CommandPS4Controller ps4_controller_ = new CommandPS4Controller(0); + // Superstructure + private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_, feeder_); + + public Command test() { + return new SequentialCommandGroup( + superstructure_.setPosition(Position.STOW), + new WaitCommand(3.0) + ); + } @Override public void robotInit() { drive_.setDefaultCommand(new DriveTeleop(drive_, robot_state_, driver_controller_)); + + setupTeleopControls(); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - robot_state_.update(); + + superstructure_.periodic(); + telemetry_.periodic(); + + // SmartDashboard.putNumber("Leader Encoder angle deg", Math.toDegrees(arm_.getLeaderAngle())); + // SmartDashboard.putNumber("Follower Encoder angle deg", Math.toDegrees(arm_.getFollowerAngle())); + // SmartDashboard.putNumber("leader velocity", arm_.getAngularVelocity()); + // SmartDashboard.putNumber("Follower Velocity", arm_.getFollowerAngularVelocity()); + SmartDashboard.putNumber("Robot Angle", drive_.getAngle().getDegrees()); + + SmartDashboard.putNumber("estimated angle", robot_state_.getDegree()); } - + + @Override public void autonomousInit() {} @@ -49,13 +106,32 @@ public void autonomousPeriodic() {} @Override public void teleopInit() { drive_.setBrakeMode(true); + arm_.setBrakeMode(true); + + // test().schedule(); } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + if (Math.toDegrees(arm_.getAngle()) < 15 && Math.toDegrees(arm_.getAngle()) > 0) { + brake_value_ = 0.05; + } + + if (Math.toDegrees(arm_.getAngle()) > 15 && Math.toDegrees(arm_.getAngle()) < 40) { + brake_value_ = 0.05; + } + + if (Math.toDegrees(arm_.getAngle()) > 40) { + brake_value_ = 0.02; + } + + } @Override - public void disabledInit() {} + public void disabledInit() { + drive_.setBrakeMode(false); + arm_.setBrakeMode(false); + } @Override public void disabledPeriodic() {} @@ -71,4 +147,44 @@ public void simulationInit() {} @Override public void simulationPeriodic() {} + + private void setupTeleopControls() { + // Driver Control + driver_controller_.rightTrigger().whileTrue(superstructure_.setShooter(-0.75)); + + driver_controller_.leftTrigger().whileTrue(superstructure_.setIntake(-0.50)); + + driver_controller_.leftBumper().whileTrue(superstructure_.setIntake(0.15)); + + driver_controller_.pov(0).whileTrue(superstructure_.setArmPercent(0.056)); + + // driver_controller_.pov(180).whileTrue(superstructure_.setShooter(0.3)); + + driver_controller_.b().whileTrue(superstructure_.shoot()); + + // This one doesn't work + driver_controller_.y().whileTrue(superstructure_.shootDelay()); + + driver_controller_.a().whileTrue(superstructure_.setFeeder(0.75)); + + driver_controller_.pov(270).onTrue(superstructure_.setArmPercent(0)); + + + // Operator Control + // hold_position.onFalse(new InstantCommand(() -> arm_.setPercent(0.056))); + + operator_controller_.leftTrigger().whileTrue(superstructure_.setArmPercent(brake_value_)); + + operator_controller_.b().onTrue(new ArmPID(arm_, 20)); + + operator_controller_.a().onTrue(new ArmPID(arm_, 2)); + + operator_controller_.y().onTrue(new ArmPID(arm_, 28)); + + operator_controller_.x().onTrue(new ArmPID(arm_, 60)); + + operator_controller_.pov(0).whileTrue(superstructure_.setArmPercent(0.1)); + + operator_controller_.pov(180).whileTrue(superstructure_.setArmPercent(-0.1)); + } } diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java new file mode 100644 index 0000000..5cb1717 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -0,0 +1,179 @@ +package org.ghrobotics.frc2024; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +import org.ghrobotics.frc2024.commands.ArmPID; +import org.ghrobotics.frc2024.commands.ArmToPosition; +import org.ghrobotics.frc2024.subsystems.Arm; +import org.ghrobotics.frc2024.subsystems.Feeder; +// import org.ghrobotics.frc2024.subsystems.Climber; +import org.ghrobotics.frc2024.subsystems.Intake; +import org.ghrobotics.frc2024.subsystems.Shooter; + +public class Superstructure { + // Subsystems + private final Arm arm_; + // private final Climber climber_; + private final Intake intake_; + private final Shooter shooter_; + private final Feeder feeder_; + + //Store Position + public String state = "STOW"; + + // Constructor + public Superstructure(Arm arm, Intake intake, Shooter shooter, Feeder feeder) { + arm_ = arm; + // climber_ = climber; + intake_ = intake; + shooter_ = shooter; + feeder_ = feeder; + } + + public void periodic() { + SmartDashboard.putNumber("Shooter Percent", shooter_.getPercent()); + // SmartDashboard.putNumber("Intake Percent", intake_.getPercent()); + SmartDashboard.putNumber("Arm Angle", Math.toDegrees(arm_.getAngle())); + } + + // Position Setter + public Command setPosition(Position pos) { + return new SequentialCommandGroup( + new InstantCommand(() -> this.state = pos.posname), + new ParallelCommandGroup( + new ArmToPosition(arm_, pos.angle) + ).withTimeout(2) + ); + } + + public Command setAnglePosition(double angle) { + return new SequentialCommandGroup( + new ArmPID(arm_, angle) + ).withTimeout(0.5); + } + + // Intake Setter + // Might change to functional later + public Command setIntake(double percent) { + return new StartEndCommand( + () -> intake_.setPercent(percent), + () -> intake_.stopMotor(), + intake_ + ); + } + + // Shooter Setter + public Command setShooter(double percent) { + return new StartEndCommand( + () -> shooter_.setPercent(percent), + () -> shooter_.stopMotor(), + shooter_ + ); + } + + // Jog Arm + public Command jogArm(double percent) { + return new StartEndCommand( + () -> arm_.setPercent(percent), + () -> arm_.setAngle(arm_.getAngle()), + arm_ + ); + } + + public Command shoot() { + return Commands.parallel(setIntake(-0.6), setShooter(-0.75)); + } + + /** + * Feeder Setter + * @param percent + */ + public Command setFeeder(double percent) { + return new StartEndCommand( + () -> feeder_.setPercent(percent), + () -> feeder_.setPercent(0), + feeder_ + ); + } + + public Command shootDelay() { + return new StartEndCommand( + () -> { + setShooter(0.75); + new WaitCommand(0.5); + Commands.parallel(setIntake(-0.6), setFeeder(-0.75)); + }, + () -> { + new InstantCommand(() -> { + shooter_.stopMotor(); + intake_.stopMotor(); + feeder_.stopMotor(); + }); + }, + feeder_ + ); + } + + public Command setArmPercent(double percent) { + return new StartEndCommand( + () -> arm_.setPercent(percent), + () -> arm_.setPercent(0), + arm_ + ); + } + + + public Command setArmPID(double angle) { + return new StartEndCommand( + () -> arm_.setAnglePID(angle), + () -> arm_.setPercent(0), + arm_ + ); + } + + // // Jog Left Climber + // public Command jogLeftClimber(double percent) { + // return new StartEndCommand( + // () -> climber_.setLeftPercent(percent), + // () -> climber_.setLeftPercent(0.1/12), + // climber_ + // ); + // } + + // // Jog Right Climber + // public Command jogRightClimber(double percent) { + // return new StartEndCommand( + // () -> climber_.setRightPercent(percent), + // () -> climber_.setRightPercent(0.1/12), + // climber_ + // ); + // } + + // GetPosition of Superstructure + public String getState() { + return state; + } + + public enum Position { + STOW(-35, "STOW"), + SUBWOOFER(0, "SUBWOOFER"), + AMP(0, "AMP"), + GROUND_INTAKE(0, "GROUND_INTAKE"), + SOURCE_INTAKE(0, "SOURCE_INTAKE"); + + final double angle; + final String posname; + + Position(double angle_deg, String name) { + this.angle = Math.toRadians(angle_deg); + this.posname = name; + } + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/Telemetry.java b/src/main/java/org/ghrobotics/frc2024/Telemetry.java new file mode 100644 index 0000000..832d0b5 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/Telemetry.java @@ -0,0 +1,53 @@ +package org.ghrobotics.frc2024; + +import java.util.ArrayList; +import java.util.List; + +import org.ghrobotics.frc2024.subsystems.Arm; + +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + + // Periodic + private final List periodic_registry_ = new ArrayList<>(); + + private final Field2d field_; + + private final Mechanism2d superstructure_; + + + public Telemetry(RobotState robot_state, Arm arm) { + ShuffleboardTab tab_ = Shuffleboard.getTab("2024"); + + field_ = new Field2d(); + tab_.add("Field", field_); + if (Robot.isReal()) + periodic_registry_.add(() -> field_.setRobotPose(robot_state.getPosition())); + if(!Robot.isReal()) + field_.setRobotPose(null); + + superstructure_ = new Mechanism2d(1.2, 1.2); + + tab_.add("Superstructure", superstructure_); + + superstructure_.getRoot("Structure Root", 0.33, 0.1).append( + new MechanismLigament2d("Structure", 0.5, 0)); + MechanismLigament2d arm_out = superstructure_.getRoot("Carriage Root", 0.80, 0.15).append( + new MechanismLigament2d("Arm", 0.15, 0, 3, new Color8Bit(Color.kOrangeRed))); + + periodic_registry_.add(() -> arm_out.setAngle(Math.toDegrees(arm.getAngle()))); + } + + public void periodic() { + for (Runnable fn : periodic_registry_) + fn.run(); + // System.out.println(periodic_registry_); + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/commands/ArmPID.java b/src/main/java/org/ghrobotics/frc2024/commands/ArmPID.java new file mode 100644 index 0000000..40c24d0 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/commands/ArmPID.java @@ -0,0 +1,64 @@ +package org.ghrobotics.frc2024.commands; + +import org.ghrobotics.frc2024.subsystems.Arm; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class ArmPID extends Command { + // Subsystems + private final Arm arm_; + + // Position + private final double position_; + + private final Timer timer_ = new Timer(); + + private double something; + // Constructor + public ArmPID(Arm arm, double position) { + // Assign member variables + arm_ = arm; + position_ = position; + + // Add subsystem requirements + addRequirements(arm_); + } + + @Override + public void initialize() { + timer_.reset(); + timer_.start(); + } + + @Override + public void execute() { + arm_.setAnglePID(position_); + something = Math.toDegrees(arm_.getAngle()); + SmartDashboard.putNumber("Somethig", something); + } + + @Override + public boolean isFinished() { + SmartDashboard.putBoolean("Finished", Math.abs(Math.toDegrees(arm_.getAngle()) - position_) < Constants.kTolerance); + SmartDashboard.putNumber("Finished get angle", Math.abs(arm_.getAngle())); + return Math.abs(Math.toDegrees(arm_.getAngle()) - position_) < Constants.kTolerance; + // return timer_.hasElapsed(0.5); + } + + @Override + public void end(boolean interrupted) { + // if(arm_.getAngle() > 40) { + // arm_.setPercent(0.02);; + // } else { + // arm_.setAnglePID(position_ + 5); + // } + timer_.reset(); + } + + // Constants + private static final class Constants { + public static final double kTolerance = 0.05; // NEED TO UPDATE + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java b/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java new file mode 100644 index 0000000..8800fe4 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java @@ -0,0 +1,37 @@ +package org.ghrobotics.frc2024.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import org.ghrobotics.frc2024.subsystems.Arm; + +public class ArmToPosition extends Command { + // Subsystems + private final Arm arm_; + + // Position + private final double position_; + + // Constructor + public ArmToPosition(Arm arm, double position) { + // Assign member variables + arm_ = arm; + position_ = position; + + // Add subsystem requirements + addRequirements(arm_); + } + + @Override + public void initialize() { + arm_.setAngle(position_); + } + + @Override + public boolean isFinished() { + return Math.abs(arm_.getAngle() - position_) < Constants.kTolerance; + } + + // Constants + private static final class Constants { + public static final double kTolerance = 0.5; // NEED TO UPDATE + } +} \ No newline at end of file diff --git a/src/main/java/org/ghrobotics/frc2024/commands/ClimbReset.java b/src/main/java/org/ghrobotics/frc2024/commands/ClimbReset.java new file mode 100644 index 0000000..69c459a --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/commands/ClimbReset.java @@ -0,0 +1,121 @@ +// package org.ghrobotics.frc2024.commands; + +// import edu.wpi.first.math.filter.MedianFilter; +// import edu.wpi.first.wpilibj.Timer; +// import edu.wpi.first.wpilibj2.command.Command; +// import org.ghrobotics.frc2024.subsystems.Climber; + +// public class ClimbReset extends Command { +// // Reference to subsystem. +// private final Climber climber_; + +// // Filters for current. +// private final MedianFilter left_climb_current_filter_; +// private final MedianFilter right_climb_current_filter_; + +// // Current averages. +// private double left_climb_current_ = 0; +// private double right_climb_current_ = 0; + +// // Whether we are done resetting. +// private boolean left_reset_complete_ = false; +// private boolean right_reset_complete_ = false; + +// // Timer to make sure filter fills up. +// private final Timer timer_; + +// /** +// * Resets the state of the climber, bringing both arms down until the limit. +// * +// * @param climber Reference to climber subsystem. +// */ +// public ClimbReset(Climber climber) { +// // Assign member variables. +// climber_ = climber; + +// // Initialize filters. +// left_climb_current_filter_ = new MedianFilter(Constants.kFilterSize); +// right_climb_current_filter_ = new MedianFilter(Constants.kFilterSize); + +// // Initialize timer. +// timer_ = new Timer(); + +// // Set subsystem requirements. +// addRequirements(climber_); +// } + +// @Override +// public void initialize() { +// // Disable soft limits. +// climber_.enableSoftLimits(false); + +// // Reset current-tracking (in case we are not running this for the first time). +// left_reset_complete_ = false; +// right_reset_complete_ = false; +// left_climb_current_filter_.reset(); +// right_climb_current_filter_.reset(); + +// // Start timer. +// timer_.start(); +// } + +// @Override +// public void execute() { +// // Check whether we should start assigning currents at this point. +// boolean assign = timer_.hasElapsed(Constants.kFilterSize * 0.02); + +// // Run each climber arm down until current threshold is reached. +// if (!left_reset_complete_) { +// climber_.setLeftPercent(-0.15); +// double l = left_climb_current_filter_.calculate(climber_.getLeftSupplyCurrent()); +// left_climb_current_ = assign ? l : 0; + +// // Check current value. +// if (left_climb_current_ > Constants.kCurrentThreshold) +// left_reset_complete_ = true; +// } else { +// climber_.setLeftPercent(0); +// } + +// if (!right_reset_complete_) { +// climber_.setRightPercent(-0.15); +// double r = right_climb_current_filter_.calculate(climber_.getRightSupplyCurrent()); +// right_climb_current_ = assign ? r : 0; + +// // Check current value. +// if (right_climb_current_ > Constants.kCurrentThreshold) +// right_reset_complete_ = true; +// } else { +// climber_.setRightPercent(0); +// } +// } + +// @Override +// public void end(boolean interrupted) { +// // Re-enable soft limits and zero outputs. +// climber_.enableSoftLimits(true); +// climber_.setLeftPercent(0); +// climber_.setRightPercent(0); + +// // Zero climb. +// if (!interrupted) { +// climber_.zero(); +// } + +// // Stop timer. +// timer_.stop(); +// } + +// @Override +// public boolean isFinished() { +// return left_reset_complete_ && right_reset_complete_; +// } + +// public static class Constants { +// // Filter Size +// public static final int kFilterSize = 25; + +// // Current Threshold +// public static final int kCurrentThreshold = 4; +// } +// } \ No newline at end of file diff --git a/src/main/java/org/ghrobotics/frc2024/commands/ClimbTeleop.java b/src/main/java/org/ghrobotics/frc2024/commands/ClimbTeleop.java new file mode 100644 index 0000000..ba89fe3 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/commands/ClimbTeleop.java @@ -0,0 +1,77 @@ +// package org.ghrobotics.frc2024.commands; + +// import edu.wpi.first.wpilibj.XboxController; +// import edu.wpi.first.wpilibj2.command.Command; +// import java.util.function.BooleanSupplier; +// import org.ghrobotics.frc2024.subsystems.Climber; + +// public class ClimbTeleop extends Command { +// // Reference to subsystem, controller, and climb mode supplier. +// private final Climber climber_; +// private final XboxController controller_; +// private final BooleanSupplier climb_mode_; + +// // Store arm setpoint for closed loop control. +// private boolean left_arm_setpoint_set_; +// private boolean right_arm_setpoint_set_; + +// /** +// * Controls the climber with manual inputs from the Xbox controller. +// * +// * @param climber Reference to climber subsystem. +// * @param controller Reference to Xbox controller used to control the subsystem. +// * @param climb_mode Whether we are currently in climb mode or not. +// */ +// public ClimbTeleop(Climber climber, XboxController controller, BooleanSupplier climb_mode) { +// // Assign member variables. +// climber_ = climber; +// controller_ = controller; +// climb_mode_ = climb_mode; + +// // Set subsystem requirements. +// addRequirements(climber_); +// } + + +// @Override +// public void execute() { +// // If we are not in climb mode, just set the pivots and exit. +// if (!climb_mode_.getAsBoolean()) { +// climber_.setLeftPercent(0); +// climber_.setRightPercent(0); +// return; +// } + +// // Use left trigger to move climber down and left bumper to move climber up. +// if (Math.abs(controller_.getLeftTriggerAxis()) > 0.1 || controller_.getLeftBumper()) { +// climber_.setLeftPercent( +// controller_.getLeftBumper() ? 0.4 : -controller_.getLeftTriggerAxis()); + +// // In manual control, so we don't have a position setpoint to hold the arm in place. +// if (left_arm_setpoint_set_) +// left_arm_setpoint_set_ = false; +// } else { +// // Hold the left arm position with closed loop control. +// if (!left_arm_setpoint_set_) { +// climber_.setLeftPosition(climber_.getLeftPosition()); +// left_arm_setpoint_set_ = true; +// } +// } + +// // Use right trigger to move climber down and right bumper to move climber up. +// if (Math.abs(controller_.getRightTriggerAxis()) > 0.1 || controller_.getRightBumper()) { +// climber_.setRightPercent( +// controller_.getRightBumper() ? 0.4 : -controller_.getRightTriggerAxis()); + +// // In manual control, so we don't have a position setpoint to hold the arm in place. +// if (right_arm_setpoint_set_) +// right_arm_setpoint_set_ = false; +// } else { +// // Hold the right arm position with closed loop control. +// if (!right_arm_setpoint_set_) { +// climber_.setRightPosition(climber_.getRightPosition()); +// right_arm_setpoint_set_ = true; +// } +// } +// } +// } \ No newline at end of file diff --git a/src/main/java/org/ghrobotics/frc2024/commands/ClimbToPosition.java b/src/main/java/org/ghrobotics/frc2024/commands/ClimbToPosition.java new file mode 100644 index 0000000..521caf1 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/commands/ClimbToPosition.java @@ -0,0 +1,43 @@ +// package org.ghrobotics.frc2024.commands; + +// import edu.wpi.first.wpilibj2.command.Command; +// import org.ghrobotics.frc2024.subsystems.Climber; + +// public class ClimbToPosition extends Command { +// // Reference to subsystem and setpoints. +// private final Climber climber_; +// private final double l_setpoint_; +// private final double r_setpoint_; + +// /** +// * Moves the climber arms to a specified position. +// * +// * @param climber Reference to climber subsystem. +// * @param l_setpoint The left setpoint in meters. +// * @param r_setpoint The right setpoint in meters. +// */ +// public ClimbToPosition(Climber climber, double l_setpoint, double r_setpoint) { +// // Assign member variables. +// climber_ = climber; +// l_setpoint_ = l_setpoint; +// r_setpoint_ = r_setpoint; + +// // Set subsystem requirements. +// addRequirements(climber_); +// } + +// @Override +// public void initialize() { + +// // Set position setpoints on climber arms. +// climber_.setLeftPosition(l_setpoint_); +// climber_.setRightPosition(r_setpoint_); +// } + +// @Override +// public boolean isFinished() { +// // End the command when both arms are within tolerance of the given setpoint. +// return Math.abs(climber_.getLeftPosition() - l_setpoint_) < Climber.Constants.kErrorTolerance && +// Math.abs(climber_.getRightPosition() - r_setpoint_) < Climber.Constants.kErrorTolerance; +// } +// } \ No newline at end of file diff --git a/src/main/java/org/ghrobotics/frc2024/commands/DriveTeleop.java b/src/main/java/org/ghrobotics/frc2024/commands/DriveTeleop.java index e9fc475..aa8ee8a 100644 --- a/src/main/java/org/ghrobotics/frc2024/commands/DriveTeleop.java +++ b/src/main/java/org/ghrobotics/frc2024/commands/DriveTeleop.java @@ -4,10 +4,12 @@ package org.ghrobotics.frc2024.commands; -import org.ghrobotics.frc2024.subsystems.Drive; import org.ghrobotics.frc2024.RobotState; +import org.ghrobotics.frc2024.subsystems.Drive; + import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class DriveTeleop extends Command { @@ -32,17 +34,30 @@ public DriveTeleop(Drive drive, RobotState robot_state, CommandXboxController co @Override public void execute() { // Inputs input + double xSpeed = -controller_.getLeftY() * Constants.kTranslationMultiplier; double ySpeed = -controller_.getLeftX() * Constants.kTranslationMultiplier; double rSpeed = -controller_.getRightX() * Constants.kRotationMultiplier; - boolean robot_oriented = controller_.rightTrigger(0.2).getAsBoolean(); + boolean robot_oriented = controller_.rightTrigger().getAsBoolean(); boolean hold_position_mode = controller_.x().getAsBoolean(); + if(Math.abs((xSpeed)) < 0.02) { + xSpeed = 0; + } + + if (Math.abs((ySpeed)) < 0.02) { + ySpeed = 0; + } + + if (Math.abs((rSpeed)) < 0.02) { + rSpeed = 0; + } + // Create chassis speeds ChassisSpeeds speeds = robot_oriented ? new ChassisSpeeds(xSpeed, ySpeed, rSpeed) : ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rSpeed, - robot_state_.getPosition().getRotation()); + drive_.getAngle()); // Set speeds if (hold_position_mode) { diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java new file mode 100644 index 0000000..5617dca --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -0,0 +1,297 @@ +package org.ghrobotics.frc2024.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class Arm extends SubsystemBase { + // Motor Controllers + private final CANSparkMax leader_; // Left + private final CANSparkMax follower_; // Right + + // Encoders + private final RelativeEncoder leader_encoder_; + private final RelativeEncoder follower_encoder_; + + // Control + private final ArmFeedforward ff_; + private final ProfiledPIDController fb_; + private boolean reset_pid_ = false; + + // Simulation + private final SingleJointedArmSim physics_sim_; + private final SimDeviceSim leader_sim_; + + // IO + private final PeriodicIO io_ = new PeriodicIO(); + private OutputType output_type_ = OutputType.PERCENT; + + private final PIDController pid_ = new PIDController(0.25, 0, 0); + + // Constructor + public Arm() { + // Initialize motor controllers + leader_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_.restoreFactoryDefaults(); + leader_.setInverted(false); + leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + + follower_ = new CANSparkMax(Constants.kFollowerId, MotorType.kBrushless); + follower_.restoreFactoryDefaults(); + follower_.setInverted(false); + follower_.setIdleMode(CANSparkMax.IdleMode.kCoast); + + // follower_.follow(leader_); + + // Initialize encoders + leader_encoder_ = leader_.getEncoder(); + leader_encoder_.setPositionConversionFactor(2 * Math.PI / Constants.kGearRatio); + leader_encoder_.setVelocityConversionFactor(2 * Math.PI / Constants.kGearRatio / 60); + + follower_encoder_ = follower_.getEncoder(); + follower_encoder_.setPositionConversionFactor(2 * Math.PI / Constants.kGearRatio); + follower_encoder_.setVelocityConversionFactor(2 * Math.PI / Constants.kGearRatio / 60); + + // Initialize control + ff_ = new ArmFeedforward(Constants.kS, Constants.kG, Constants.kV, Constants.kA); + fb_ = new ProfiledPIDController(Constants.kP, 0, 0, new TrapezoidProfile.Constraints( + Constants.kMaxVelocity, Constants.kMaxAcceleration)); + + // Safety features + leader_.setSmartCurrentLimit((int) Constants.kCurrentLimit); + leader_.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, (float) Constants.kMinAngle); + leader_.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, (float) Constants.kMaxAngle); + + // Initialize simulation + physics_sim_ = new SingleJointedArmSim( + LinearSystemId.identifyPositionSystem(Constants.kV, Constants.kA), + DCMotor.getNEO(2), Constants.kGearRatio, Constants.kArmLength, Constants.kMinAngle, + Constants.kMaxAngle, false, 0); + leader_sim_ = new SimDeviceSim("SPARK MAX [" + Constants.kLeaderId + "]"); + + // Safety features + leader_.setSmartCurrentLimit((int) Constants.kCurrentLimit); + leader_.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, (float) Constants.kMinAngle); + leader_.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, (float) Constants.kMaxAngle); + + follower_.setSmartCurrentLimit((int) Constants.kCurrentLimit); + follower_.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, (float) Constants.kMinAngle); + follower_.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, (float) Constants.kMaxAngle); + + // Reset encoder + zero(); + physics_sim_.setState(VecBuilder.fill(Constants.kMaxAngle, 0)); + } + + @Override + public void periodic() { + // Read inputs + io_.angle = leader_encoder_.getPosition(); + io_.angular_velocity = leader_encoder_.getVelocity(); + io_.current = leader_.getOutputCurrent(); + + io_.follower_angular_velocity = follower_encoder_.getVelocity(); + io_.follower_velocity = follower_encoder_.getVelocity(); + + + if (io_.wants_zero) { + io_.wants_zero = false; + leader_encoder_.setPosition(Constants.kMinAngle); + follower_encoder_.setPosition(Constants.kMinAngle); + } + + // Reset controller if we have to + if (reset_pid_) { + reset_pid_ = false; + fb_.reset(io_.angle, io_.angular_velocity); + } + + // Write outputs + switch (output_type_) { + case PERCENT: + leader_.set(io_.demand); + follower_.set(-io_.demand); + + // Set simulated inputs + if (RobotBase.isSimulation()) + leader_sim_.getDouble("Applied Output").set(io_.demand * 12); + SmartDashboard.putNumber("ARM angle", getAngle()); + break; + case ANGLE: + double feedback = fb_.calculate(io_.angle); + + double velocity_setpoint = fb_.getSetpoint().velocity; + double acceleration_setpoint = (velocity_setpoint - io_.angular_velocity) / 0.02; + double feedforward = ff_.calculate(io_.angle, velocity_setpoint, acceleration_setpoint); + + leader_.setVoltage(feedback + feedforward); + // follower_.setVoltage(feedback + feedforward); + break; + } + + // Software Stop + if (io_.angle > 90 && io_.demand > 0) { + io_.demand = -0.1; + } + } + + @Override + public void simulationPeriodic() { + // Update physics sim with inputs + double voltage = leader_.getAppliedOutput(); + if (output_type_ == OutputType.ANGLE) + voltage -= Constants.kG * Math.cos(io_.angle); + physics_sim_.setInputVoltage(voltage); + + // Update physics sim forward in time + physics_sim_.update(0.02); + + // Update encoder values + leader_sim_.getDouble("Position").set(physics_sim_.getAngleRads()); + leader_sim_.getDouble("Velocity").set(physics_sim_.getVelocityRadPerSec()); + } + + public void setPercent(double percent) { + output_type_ = OutputType.PERCENT; + io_.demand = percent; + reset_pid_ = true; + } + + /** + * Set Arm Angle + * @param angle in Radians + */ + public void setAngle(double angle) { + output_type_ = OutputType.ANGLE; + fb_.setGoal(angle); + reset_pid_ = true; + } + + /** + * Set Arm Angle using PID + * @param angle in Degrees + */ + public void setAnglePID(double angle) { + pid_.setSetpoint(angle); + SmartDashboard.putNumber("Angle Calculate", pid_.calculate(Math.toDegrees(getAngle()))); + double output = MathUtil.clamp(pid_.calculate(Math.toDegrees(getAngle())), -0.2, 0.35); + SmartDashboard.putNumber("Arm output", output); + + if (Math.abs(output) < 0.01){ + output = 0; + } + setPercent(output); + } + + public void setBrakeMode(boolean value) { + leader_.setIdleMode(value ? CANSparkMax.IdleMode.kBrake : CANSparkMax.IdleMode.kCoast); + follower_.setIdleMode(value ? CANSparkMax.IdleMode.kBrake : CANSparkMax.IdleMode.kCoast); + } + + public void enableSoftLimits(boolean value) { + leader_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, value); + leader_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, value); + follower_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, value); + follower_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, value); + } + + public void zero() { + io_.wants_zero = true; + } + + /** + * Get Arm Angle + * @return Angle in Radians + */ + public double getAngle() { + return io_.angle; + } + + public double getLeaderAngle() { + return leader_encoder_.getPosition(); + } + + public double getFollowerAngle() { + return follower_encoder_.getPosition(); + } + + public double getAngularVelocity() { + return io_.angular_velocity; + } + + public double getFollowerAngularVelocity() { + return io_.follower_angular_velocity; + } + + + + public double getAngularVelocitySetpoint() { + return fb_.getSetpoint().velocity; + } + + public double getCurrent() { + return io_.current; + } + + // Output Type + private enum OutputType { + PERCENT, ANGLE + } + + //IO + private static class PeriodicIO { + //Inputs + double angle; + double angular_velocity; + double follower_angular_velocity; + double current; + double follower_velocity; + + //Outputs + boolean wants_zero; + double demand; + } + + + // Constants (TO UPDATE) + public static class Constants { + // Motor Controllers + public static final int kLeaderId = 9; + public static final int kFollowerId = 10; + + // Physical Constants + public static final double kGearRatio = 140; + public static final double kMinAngle = Math.toRadians(0); + public static final double kMaxAngle = Math.toRadians(120); + public static final double kArmLength = 0.15; + + // Feedforward + public static final double kA = 0.05; // volts * seconds^2 / radians + public static final double kG = 0.89; // volts + public static final double kS = 0.08; // volts + public static final double kV = 2.73; // volts * seconds/radians + + // Current Limit + public static final double kCurrentLimit = 40; + + // Control + public static double kMaxVelocity = 2.14; + public static double kMaxAcceleration = 2.14; + public static double kP = 0.25; + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java new file mode 100644 index 0000000..632a9ff --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java @@ -0,0 +1,318 @@ +// package org.ghrobotics.frc2024.subsystems; + +// import com.revrobotics.CANSparkMax; +// import com.revrobotics.RelativeEncoder; +// import com.revrobotics.CANSparkLowLevel.MotorType; + +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.controller.SimpleMotorFeedforward; +// import edu.wpi.first.math.trajectory.TrapezoidProfile; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// public class Climber extends SubsystemBase { +// // Motor Controllers +// private final CANSparkMax left_leader_; +// private final CANSparkMax right_leader_; + +// // Encoders +// private final RelativeEncoder left_encoder_; +// private final RelativeEncoder right_encoder_; + +// // Control +// private final SimpleMotorFeedforward left_feedforward_; +// private final ProfiledPIDController left_fb_; +// private final SimpleMotorFeedforward right_feedforward_; +// private final ProfiledPIDController right_fb_; + +// // IO +// private final PeriodicIO io_ = new PeriodicIO(); +// private OutputType left_output_type_ = OutputType.PERCENT; +// private OutputType right_output_type_ = OutputType.PERCENT; + +// // Constructor +// public Climber() { +// // Initialize motor controllers. +// left_leader_ = new CANSparkMax(Constants.kLeftLeaderId, MotorType.kBrushless); +// left_leader_.restoreFactoryDefaults(); +// left_leader_.setInverted(false); +// left_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + +// right_leader_ = new CANSparkMax(Constants.kRightLeaderId, MotorType.kBrushless); +// right_leader_.restoreFactoryDefaults(); +// right_leader_.setInverted(true); +// right_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + +// // Initialize encoders - UPDATE CONVERSION FACTORS! +// left_encoder_ = left_leader_.getEncoder(); +// left_encoder_.setPositionConversionFactor(0); +// left_encoder_.setVelocityConversionFactor(0); + +// right_encoder_ = right_leader_.getEncoder(); +// right_encoder_.setPositionConversionFactor(0); +// right_encoder_.setVelocityConversionFactor(0); + + +// // Initialize feedforward and feedback. +// left_feedforward_ = new SimpleMotorFeedforward(Constants.kLeftKs, Constants.kLeftKv, +// Constants.kLeftKa); +// left_fb_ = new ProfiledPIDController(Constants.kLeftKp, 0, 0, new TrapezoidProfile.Constraints( +// Constants.kMaxVelocity, Constants.kMaxAcceleration)); +// right_feedforward_ = new SimpleMotorFeedforward(Constants.kRightKs, Constants.kRightKv, +// Constants.kRightKa); +// right_fb_ = new ProfiledPIDController(Constants.kRightKp, 0, 0, new TrapezoidProfile.Constraints( +// Constants.kMaxVelocity, Constants.kMaxAcceleration)); + +// // Configure soft limits. +// left_leader_.setSmartCurrentLimit((int) Constants.kCurrentLimit); +// left_leader_.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, (float) 0); +// left_leader_.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, (float) Constants.kMaxHeightNativeUnits); +// enableSoftLimits(true); + +// zero(); +// } + +// /** +// * This method runs periodically every 20 ms. +// */ +// @Override +// public void periodic() { +// // Read inputs. +// io_.l_position = left_encoder_.getPosition(); +// io_.r_position = right_encoder_.getPosition(); +// io_.l_velocity = left_encoder_.getVelocity(); +// io_.r_velocity = right_encoder_.getVelocity(); +// io_.l_current = left_leader_.getOutputCurrent(); +// io_.r_current = right_leader_.getOutputCurrent(); + +// // SmartDashboard.putNumber("L Climb Enc", left_leader_.getSelectedSensorPosition()); + +// // Zero +// if (io_.wants_zero) { +// io_.wants_zero = false; +// left_encoder_.setPosition(0); +// right_encoder_.setPosition(0); +// } + +// // Set motor outputs. +// switch (left_output_type_) { +// case PERCENT: +// // Send the percent output values directly to the motor controller. +// left_leader_.set(io_.l_demand); + +// case POSITION: +// // Calculate feedforward value and add to built-in motor controller PID. +// double feedback = left_fb_.calculate(io_.l_position); + +// double velocity_setpoint = left_fb_.getSetpoint().velocity; +// double acceleration_setpoint = (velocity_setpoint - io_.l_velocity) / 0.02; +// double feedforward = left_feedforward_.calculate(io_.l_position, velocity_setpoint, acceleration_setpoint); + +// left_leader_.setVoltage(feedback + feedforward); +// break; +// } + +// // Set motor outputs. +// switch (right_output_type_) { +// case PERCENT: +// // Send the percent output values directly to the motor controller. +// right_leader_.set(io_.r_demand); + +// case POSITION: +// // Calculate feedforward value and add to built-in motor controller PID. +// double feedback = right_fb_.calculate(io_.r_position); + +// double velocity_setpoint = right_fb_.getSetpoint().velocity; +// double acceleration_setpoint = (velocity_setpoint - io_.r_velocity) / 0.02; +// double feedforward = right_feedforward_.calculate(io_.r_position, velocity_setpoint, acceleration_setpoint); + +// right_leader_.setVoltage(feedback + feedforward); +// break; +// } + +// } + +// public void zero() { +// io_.wants_zero = true; +// } + +// /** +// * Enables or disables soft limits for each motor controller. This needs to be done to zero the +// * climber at the start of the match. +// * +// * @param value Whether the soft limits should be enforced. +// */ +// public void enableSoftLimits(boolean value) { +// left_leader_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, value); +// left_leader_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, value); +// right_leader_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, value); +// right_leader_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, value); +// } + +// /** +// * Sets the left arm % output. +// * +// * @param value The left arm % output in [-1, 1]. +// */ +// public void setLeftPercent(double value) { +// left_output_type_ = OutputType.PERCENT; +// io_.l_demand = value; +// } + +// /** +// * Sets the right arm % output. +// * +// * @param value The right arm % output in [-1, 1]. +// */ +// public void setRightPercent(double value) { +// right_output_type_ = OutputType.PERCENT; +// io_.r_demand = value; +// } + +// /** +// * Sets the left arm position. +// * +// * @param value The left arm position in meters. +// */ +// public void setLeftPosition(double value) { +// left_output_type_ = OutputType.POSITION; +// io_.l_demand = toCTREPosition(value); +// } + +// /** +// * Sets the right arm position. +// * +// * @param value The right arm position in meters. +// */ +// public void setRightPosition(double value) { +// right_output_type_ = OutputType.POSITION; +// io_.r_demand = toCTREPosition(value); +// } + +// /** +// * Returns the left position of the climber in meters. +// * +// * @return The left position of the climber in meters. +// */ +// public double getLeftPosition() { +// return io_.l_position; +// } + +// /** +// * Returns the right position of the climber in meters. +// * +// * @return The right position of the climber in meters. +// */ +// public double getRightPosition() { +// return io_.r_position; +// } + +// /** +// * Returns the left supply current. +// * +// * @return The left supply current. +// */ +// public double getLeftSupplyCurrent() { +// return io_.l_current; +// } + +// /** +// * Returns the right supply current. +// * +// * @return The right supply current. +// */ +// public double getRightSupplyCurrent() { +// return io_.r_current; +// } + +// /** +// * Converts position to CTRE native units. +// * +// * @param pos The position in meters. +// * @return The position in native units. +// */ +// private static double toCTREPosition(double pos) { +// return pos * Constants.kMaxHeightNativeUnits / Constants.kMaxHeight; +// } + +// /** +// * Converts position to CTRE native units / 100 ms. +// * +// * @param vel The velocity in meters per second. +// * @return The velocity in native units / 100 ms. +// */ +// private static double toCTREVelocity(double vel) { +// return toCTREPosition(vel) * 10; +// } + +// /** +// * Converts CTRE native units to position. +// * +// * @param pos The position in native units. +// * @return The position in meters. +// */ +// private static double fromCTREPosition(double pos) { +// return pos * Constants.kMaxHeight / Constants.kMaxHeightNativeUnits; +// } + +// /** +// * Converts CTRE native units / 100 ms to velocity. +// * +// * @param vel The velocity in native units / 100 ms. +// * @return The velocity in meters per second. +// */ +// private static double fromCTREVelocity(double vel) { +// return fromCTREPosition(vel) / 10; +// } + +// public enum OutputType { +// PERCENT, POSITION +// } + +// public static class PeriodicIO { +// // Inputs +// double l_position; +// double r_position; + +// double l_velocity; +// double r_velocity; + +// double l_current; +// double r_current; + +// // Outputs +// double l_demand; +// double r_demand; + +// boolean wants_zero = false; + +// } + +// public static class Constants { +// // Motor Controllers +// public static final int kLeftLeaderId = 0; +// public static final int kRightLeaderId = 0; + +// // Hardware +// public static final double kMaxHeightNativeUnits = 151543; +// public static final double kMaxHeight = Units.inchesToMeters(25.75); + +// // Control + +// public static final double kCurrentLimit = 0; + +// public static final double kLeftKs = 0; +// public static final double kLeftKv = 0; +// public static final double kLeftKa = 0; +// public static final double kLeftKp = 0.2; + +// public static final double kRightKs = 0; +// public static final double kRightKv = 0; +// public static final double kRightKa = 0; +// public static final double kRightKp = 0.2; + +// public static final double kMaxVelocity = Units.inchesToMeters(0.1); +// public static final double kMaxAcceleration = Units.inchesToMeters(0.1); +// public static final double kErrorTolerance = Units.inchesToMeters(1.51); +// } +// } \ No newline at end of file diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Feeder.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Feeder.java new file mode 100644 index 0000000..9607b4a --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Feeder.java @@ -0,0 +1,45 @@ +package org.ghrobotics.frc2024.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Feeder extends SubsystemBase{ + private final CANSparkMax leader_; + + private final PeriodicIO io_ = new PeriodicIO(); + + public Feeder() { + leader_ = new CANSparkMax(Constants.kFeederId, MotorType.kBrushless); + leader_.restoreFactoryDefaults(); + leader_.setInverted(false); + leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + } + + @Override + public void periodic() { + leader_.set(io_.demand); + } + + public void setPercent(double value) { + io_.demand = value; + } + + public void stopMotor() { + leader_.set(0); + } + + public double getPercent() { + return leader_.get(); + } + + // IO + public static class PeriodicIO { + public double demand; + } + + public static class Constants { + public static final int kFeederId = 15; + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java new file mode 100644 index 0000000..3985e9f --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -0,0 +1,82 @@ +package org.ghrobotics.frc2024.subsystems; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + // Motor Controllers + private final CANSparkMax leader_left_; + private final CANSparkMax leader_right_; + private final PeriodicIO io_ = new PeriodicIO(); + + private final PIDController controller_ = new PIDController(Constants.kP, 0, 0); + + // Constructor + public Intake() { + // Initialize motor controllers + leader_left_ = new CANSparkMax(Constants.kLeaderLeftId, MotorType.kBrushless); + leader_left_.restoreFactoryDefaults(); + leader_left_.setInverted(true); + leader_left_.setIdleMode(CANSparkMax.IdleMode.kBrake); + + + leader_right_ = new CANSparkMax(Constants.kLeaderRightId, MotorType.kBrushless); + leader_right_.restoreFactoryDefaults(); + leader_right_.setInverted(true); + leader_right_.setIdleMode(CANSparkMax.IdleMode.kBrake); + leader_right_.follow(leader_left_); + + + } + + public void periodic() { + // Read inputs. + io_.current_left_ = leader_left_.getOutputCurrent(); + io_.current_right_ = leader_right_.getOutputCurrent(); + leader_left_.set(io_.left_demand); + leader_right_.set(io_.right_demand); + } + + // public double getPercent() { + // return io_.demand; + // } + + // public void setPercent(double value) { + // double correction = MathUtil.clamp(controller_.calculate(getPercent(), value), -1.0, 1.0); + // io_.left_demand = correction; + // io_.right_demand = correction; + // } + + public void setPercent(double value) { + io_.left_demand = value; + io_.right_demand = value - 0.1; + } + public void stopMotor() { + io_.left_demand = 0; + io_.right_demand = 0; + } + + // IO + public static class PeriodicIO { + // Input + double current_left_; + double current_right_; + + // Output + double left_demand; + double right_demand; + } + + // Constants + public static class Constants { + // Motor Controllers + public static final int kLeaderLeftId = 11; + public static final int kLeaderRightId = 12; + + // PID Constants + public static final double kP = 0.8; + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java new file mode 100644 index 0000000..83b9710 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java @@ -0,0 +1,91 @@ +package org.ghrobotics.frc2024.subsystems; + +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.controller.PIDController; + + +import com.revrobotics.CANSparkMax; + +public class Shooter extends SubsystemBase { + // Motor Controllers + private final CANSparkMax left_leader_; + private final CANSparkMax right_leader_; + //io + private final PeriodicIO io_ = new PeriodicIO(); + + private final PIDController controller_ = new PIDController(Constants.kP, 0, 0); + + // Constructor + public Shooter() { + // Initialize motor controllers + left_leader_ = new CANSparkMax(Constants.kLeftLeaderId, MotorType.kBrushless); + left_leader_.restoreFactoryDefaults(); + left_leader_.setInverted(false); + left_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + left_leader_.enableVoltageCompensation(12.0); + + right_leader_ = new CANSparkMax(Constants.kRightLeaderId, MotorType.kBrushless); + right_leader_.restoreFactoryDefaults(); + right_leader_.setInverted(false); + right_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + right_leader_.enableVoltageCompensation(12.0); + + // Safety + left_leader_.setSmartCurrentLimit((int) Constants.kCurrentLimit); + right_leader_.setSmartCurrentLimit((int) Constants.kCurrentLimit); + + } + + public double getPercent() { + return io_.demand; + } + + public void setPercent(double value) { + // double correction = MathUtil.clamp(controller_.calculate(getPercent(), value), -1.0, 1.0); + io_.demand = value; + } + + public void stopMotor() { + io_.demand = 0; + } + + @Override + public void periodic(){ + //read inputs + + io_.left_leader_supply_current = left_leader_.getOutputCurrent(); + io_.right_leader_supply_current = right_leader_.getOutputCurrent(); + + left_leader_.set(io_.demand); + right_leader_.set(io_.demand); + SmartDashboard.putBoolean("shooterActive", io_.demand>0.2); + } + + // IO + public static class PeriodicIO { + // Inputs + double left_leader_supply_current; + double right_leader_supply_current; + + // Outputs + double demand; + } + + + + // Constants + public static class Constants { + // Motor Controllers + public static final int kLeftLeaderId = 13; + public static final int kRightLeaderId = 14; + + // PID + public static final double kP = 2.8; + + // Current Limit + public static final double kCurrentLimit = 40; + } +}