From ed703f78a06b4cfb007e32c1319d6f7130a740ee Mon Sep 17 00:00:00 2001 From: Yash Gupta <67847125+yashgupta-17@users.noreply.github.com> Date: Sat, 10 Feb 2024 15:26:59 -0500 Subject: [PATCH 01/20] Created subsystem & superstructure files --- src/main/java/org/ghrobotics/frc2024/Superstructure.java | 0 src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java | 0 src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java | 0 src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java | 0 src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java | 0 5 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/main/java/org/ghrobotics/frc2024/Superstructure.java create mode 100644 src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java create mode 100644 src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java create mode 100644 src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java create mode 100644 src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java 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..e69de29 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..e69de29 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..e69de29 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..e69de29 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..e69de29 From f2627b8d2ee7d96648155ddc01b811aa9924f3b7 Mon Sep 17 00:00:00 2001 From: Yash Gupta <67847125+yashgupta-17@users.noreply.github.com> Date: Sun, 11 Feb 2024 17:07:55 -0500 Subject: [PATCH 02/20] Created subsystem classes --- .../java/org/ghrobotics/frc2024/Robot.java | 2 + .../ghrobotics/frc2024/Superstructure.java | 5 ++ .../ghrobotics/frc2024/subsystems/Arm.java | 79 +++++++++++++++++++ .../frc2024/subsystems/Climber.java | 7 ++ .../ghrobotics/frc2024/subsystems/Intake.java | 32 ++++++++ .../frc2024/subsystems/Shooter.java | 37 +++++++++ 6 files changed, 162 insertions(+) diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java index 3688845..5d2691e 100644 --- a/src/main/java/org/ghrobotics/frc2024/Robot.java +++ b/src/main/java/org/ghrobotics/frc2024/Robot.java @@ -4,8 +4,10 @@ package org.ghrobotics.frc2024; + import edu.wpi.first.wpilibj.TimedRobot; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java index e69de29..16775da 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -0,0 +1,5 @@ +package org.ghrobotics.frc2024; + +public class Superstructure { + +} diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index e69de29..af38cb4 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -0,0 +1,79 @@ +package org.ghrobotics.frc2024.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.StateSpaceUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.ArmFeedforward; +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.wpilibj2.command.SubsystemBase; +import java.io.PipedOutputStream; + + +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; + + + // Constructor + public Arm() { + // Initialize motor controllers + leader_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_.restoreFactoryDefaults(); + leader_.setInverted(true); + leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + + follower_ = new CANSparkMax(Constants.kFollowerId, MotorType.kBrushless); + follower_.restoreFactoryDefaults(); + follower_.setInverted(false); + follower_.setIdleMode(CANSparkMax.IdleMode.kCoast); + + // 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); + } + + + // IO + public static class PeriodicIO { + + } + + // Constants (TO UPDATE) + public static class Constants { + // Motor Controllers + public static final int kLeaderId = 0; + public static final int kFollowerId = 0; + + // Physical Constants + public static final double kGearRatio = 0; + public static final double kMinAngle = Math.toRadians(0); + public static final double kMaxAngle = Math.toRadians(0); + public static final double kArmLength = 0; + + } +} + diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java index e69de29..8236f56 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java @@ -0,0 +1,7 @@ +package org.ghrobotics.frc2024.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + +} diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java index e69de29..e213d20 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -0,0 +1,32 @@ +package org.ghrobotics.frc2024.subsystems; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + // Motor Controllers + private final CANSparkMax leader_; // Left + + // Constructor + public Intake() { + // Initialize motor controllers + leader_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_.restoreFactoryDefaults(); + leader_.setInverted(true); + leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + } + + // IO + public static class PeriodicIO { + + } + + // Constants + public static class Constants { + // Motor Controllers + public static final int kLeaderId = 0; + public static final int kFollowerId = 0; + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java index e69de29..66c7e87 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java @@ -0,0 +1,37 @@ +package org.ghrobotics.frc2024.subsystems; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase { + // Motor Controllers + private final CANSparkMax left_leader_; + private final CANSparkMax right_leader_; + + // Constructor + public Shooter() { + // Initialize motor controllers + left_leader_ = new CANSparkMax(Constants.kLeftLeaderId, MotorType.kBrushless); + left_leader_.restoreFactoryDefaults(); + left_leader_.setInverted(true); + left_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + + right_leader_ = new CANSparkMax(Constants.kRightLeaderId, MotorType.kBrushless); + right_leader_.restoreFactoryDefaults(); + right_leader_.setInverted(false); + right_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + } + // IO + public static class PeriodicIO { + + } + + // Constants + public static class Constants { + // Motor Controllers + public static final int kLeftLeaderId = 0; + public static final int kRightLeaderId = 0; + } +} From 69fe42a8316ae4cc8443ac74dbeed2ff611a722e Mon Sep 17 00:00:00 2001 From: Yash Gupta Date: Tue, 13 Feb 2024 19:42:16 -0500 Subject: [PATCH 03/20] added stuff to arm.java not much --- .../ghrobotics/frc2024/subsystems/Arm.java | 37 +++++++++++++++++-- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index af38cb4..77db811 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -28,9 +28,9 @@ public class Arm extends SubsystemBase { private final RelativeEncoder follower_encoder_; // Control - // private final ArmFeedforward ff_; - // private final ProfiledPIDController fb_; - // private boolean reset_pid_ = false; + private final ArmFeedforward ff_; + private final ProfiledPIDController fb_; + private boolean reset_pid_ = false; // Constructor @@ -54,6 +54,22 @@ public Arm() { 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); + + // Reset encoder + + System.out.println("hji"); + + } @@ -74,6 +90,21 @@ public static class Constants { public static final double kMaxAngle = Math.toRadians(0); public static final double kArmLength = 0; + // Feedforward + public static final double kA = 0; // volts * seconds^2 / radians + public static final double kG = 0; // volts + public static final double kS = 0; // volts + public static final double kV = 0; // volts * seconds/radians + + // Current Limit + public static final double kCurrentLimit = 0; + + // Control + public static double kMaxVelocity = 0; + public static double kMaxAcceleration = 0; + public static double kP = 0; + + } } From 39eec2df758f5f1abd6e84979a964e6f920915bd Mon Sep 17 00:00:00 2001 From: Yash Gupta Date: Tue, 13 Feb 2024 23:39:29 -0500 Subject: [PATCH 04/20] finished? arm class & commands --- .../frc2024/commands/ArmToPosition.java | 0 .../ghrobotics/frc2024/subsystems/Arm.java | 145 +++++++++++++++++- 2 files changed, 138 insertions(+), 7 deletions(-) create mode 100644 src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java 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..e69de29 diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index 77db811..0e1abfc 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -31,8 +31,16 @@ public class Arm extends SubsystemBase { 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; + // Constructor public Arm() { // Initialize motor controllers @@ -65,19 +73,142 @@ public Arm() { 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); + // 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_.angle = follower_encoder_.getPosition(); + io_.angular_velocity = follower_encoder_.getVelocity(); + io_.current = follower_.getOutputCurrent(); + + if (io_.wants_zero) { + io_.wants_zero = false; + leader_encoder_.setPosition(Constants.kMaxAngle); + follower_encoder_.setPosition(Constants.kMaxAngle); + } + + // 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); + + // Set simulated inputs + if (RobotBase.isSimulation()) + leader_sim_.getDouble("Applied Output").set(io_.demand * 12); + + 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; + } + } - System.out.println("hji"); - + @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()); } - - - // IO - public static class PeriodicIO { - + + public void setPercent(double percent) { + output_type_ = OutputType.PERCENT; + io_.demand = percent; + reset_pid_ = true; + } + + public void setAngle(double angle) { + output_type_ = OutputType.ANGLE; + fb_.setGoal(angle); + reset_pid_ = true; } + public void enableSoftLimits(boolean value) { + leader_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, value); + leader_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, value); + } + + public void zero() { + io_.wants_zero = true; + } + + // Position Getter + public double getAngle() { + return io_.angle; + } + + public double getAngularVelocity() { + return io_.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 current; + + //Outputs + boolean wants_zero; + double demand; + } + + // Constants (TO UPDATE) public static class Constants { // Motor Controllers From 1ef43ff8b86bf318d873f6e5b030f4d43ce5ab01 Mon Sep 17 00:00:00 2001 From: suhaank077 <145230825+suhaank077@users.noreply.github.com> Date: Wed, 14 Feb 2024 19:41:15 -0500 Subject: [PATCH 05/20] Updatedv1Intake.java We worked on this. This is our first version. We think that we included all the necessary functions (looked at 2022 for reference). - Suhaan and Jake --- .../ghrobotics/frc2024/subsystems/Intake.java | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java index e213d20..b0a59ff 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -8,7 +8,7 @@ public class Intake extends SubsystemBase { // Motor Controllers private final CANSparkMax leader_; // Left - + private final PeriodicIO io_ = new PeriodicIO(); // Constructor public Intake() { // Initialize motor controllers @@ -17,10 +17,27 @@ public Intake() { leader_.setInverted(true); leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); } - + + public void periodic() { + // Read inputs. + io_.leader_supply_current = leader_.getOutputCurrent(); + leader_.set(io_.leader_bridge_demand); + } + + //get, set % output on intake. parameter is percent output [-1, 1] + public void setIntakePercent(double value) { + io_.leader_bridge_demand = value; + } + //Returns the % output of the intake and bridge. + public double getIntakePercent() { + return io_.leader_bridge_demand; + } // IO public static class PeriodicIO { - + //input + double leader_supply_current; + //output + double leader_bridge_demand; } // Constants From 5fab694d22c0e66fe546dd4032567eebb1f85236 Mon Sep 17 00:00:00 2001 From: Yash Gupta Date: Wed, 14 Feb 2024 21:23:10 -0500 Subject: [PATCH 06/20] updated Arm.java --- .../frc2024/commands/ArmToPosition.java | 37 +++++++++++++++++++ .../ghrobotics/frc2024/subsystems/Arm.java | 11 ++++-- 2 files changed, 44 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java b/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java index e69de29..157a424 100644 --- a/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java +++ 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; // NEED TO UPDATE + } +} \ No newline at end of file diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index 0e1abfc..fcbfcef 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -3,7 +3,6 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.StateSpaceUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -85,6 +84,10 @@ public Arm() { 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(); @@ -118,6 +121,7 @@ public void periodic() { switch (output_type_) { case PERCENT: leader_.set(io_.demand); + follower_.set(io_.demand); // Set simulated inputs if (RobotBase.isSimulation()) @@ -168,7 +172,8 @@ public void setAngle(double angle) { 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; @@ -234,8 +239,6 @@ public static class Constants { public static double kMaxVelocity = 0; public static double kMaxAcceleration = 0; public static double kP = 0; - - } } From 7563566114187b167fbbd3803640e31af571fb88 Mon Sep 17 00:00:00 2001 From: suhaank077 Date: Thu, 15 Feb 2024 19:40:41 -0500 Subject: [PATCH 07/20] added shooter code --- .../frc2024/subsystems/Shooter.java | 27 ++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java index 66c7e87..567c5fa 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java @@ -1,15 +1,18 @@ package org.ghrobotics.frc2024.subsystems; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +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(); // Constructor public Shooter() { // Initialize motor controllers @@ -22,11 +25,29 @@ public Shooter() { right_leader_.restoreFactoryDefaults(); right_leader_.setInverted(false); right_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + right_leader_.follow(left_leader_); + } + @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); + SmartDashboard.putBoolean("shooterActive", io_.demand>0.2); + } + // IO public static class PeriodicIO { - + //inputs + double left_leader_supply_current; + double right_leader_supply_current; + //ouputs + double demand; } + + // Constants public static class Constants { From 4bce3110e983efddd5ec59bee4e6a83fbeaef784 Mon Sep 17 00:00:00 2001 From: Yash Gupta Date: Fri, 16 Feb 2024 00:18:55 -0500 Subject: [PATCH 08/20] worked on Climber.java --- networktables.json | 1 + simgui-ds.json | 92 +++++ simgui.json | 10 + .../ghrobotics/frc2024/subsystems/Arm.java | 3 +- .../frc2024/subsystems/Climber.java | 316 +++++++++++++++++- 5 files changed, 420 insertions(+), 2 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..5f9d275 --- /dev/null +++ b/simgui.json @@ -0,0 +1,10 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index fcbfcef..ead1400 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -173,7 +173,8 @@ 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); } + follower_.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, value); + } public void zero() { io_.wants_zero = true; diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java index 8236f56..a3b9e3a 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java @@ -1,7 +1,321 @@ 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.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.List; 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. + left_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 From a3ca65636273d9bba6434bdefefc1bbd675ad442 Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Fri, 16 Feb 2024 08:23:00 -0500 Subject: [PATCH 09/20] added build file fixed --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index e48a6ca..d00581b 100644 --- a/build.gradle +++ b/build.gradle @@ -8,7 +8,7 @@ java { targetCompatibility = JavaVersion.VERSION_17 } -def ROBOT_MAIN_CLASS = "org.ghrobotics.frc2024" +def ROBOT_MAIN_CLASS = "org.ghrobotics.frc2024.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. From 8936188afb38f55e7a36dffee46c0d42ab3eba4b Mon Sep 17 00:00:00 2001 From: Yash Gupta Date: Fri, 16 Feb 2024 19:22:12 -0500 Subject: [PATCH 10/20] Added Climber commands, make fixes to Arm, created Superstructure class --- .../java/org/ghrobotics/frc2024/Robot.java | 36 +++++- .../ghrobotics/frc2024/Superstructure.java | 71 ++++++++++ .../frc2024/commands/ArmToPosition.java | 2 +- .../frc2024/commands/ClimbReset.java | 121 ++++++++++++++++++ .../frc2024/commands/ClimbTeleop.java | 77 +++++++++++ .../frc2024/commands/ClimbToPosition.java | 43 +++++++ .../ghrobotics/frc2024/subsystems/Arm.java | 4 +- 7 files changed, 351 insertions(+), 3 deletions(-) create mode 100644 src/main/java/org/ghrobotics/frc2024/commands/ClimbReset.java create mode 100644 src/main/java/org/ghrobotics/frc2024/commands/ClimbTeleop.java create mode 100644 src/main/java/org/ghrobotics/frc2024/commands/ClimbToPosition.java diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java index 5d2691e..5f638f5 100644 --- a/src/main/java/org/ghrobotics/frc2024/Robot.java +++ b/src/main/java/org/ghrobotics/frc2024/Robot.java @@ -4,8 +4,27 @@ package org.ghrobotics.frc2024; - import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.DriverStation; +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.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +// import org.ghrobotics.frc2024.auto.AutoSelector; +// import org.ghrobotics.frc2024.commands.DriveBrakeMode; +// import org.ghrobotics.frc2024.commands.DriveTeleop; +// import org.ghrobotics.frc2024.commands.DriveTowardPosition; +// import org.ghrobotics.frc2024.commands.HomeSuperstructure; +// import org.ghrobotics.frc2024.subsystems.Drivetrain; +import org.ghrobotics.frc2024.subsystems.Arm; +import org.ghrobotics.frc2024.subsystems.Climber; +import org.ghrobotics.frc2024.subsystems.Intake; +import org.ghrobotics.frc2024.subsystems.Shooter; +// import org.ghrobotics.frc2024.subsystems.LED; +// import org.ghrobotics.frc2024.subsystems.LED.StandardLEDOutput; +// import org.ghrobotics.frc2024.subsystems.Limelight; +// import org.ghrobotics.frc2024.subsystems.PoseEstimator; /** @@ -19,6 +38,21 @@ public class Robot extends TimedRobot { * This function is run when the robot is first started up and should be used for any * initialization code. */ + + // Subsystems + private final Arm arm_ = new Arm(); + private final Climber climber_ = new Climber(); + private final Intake intake_ = new Intake(); + private final Shooter shooter_ = new Shooter(); + + // Superstructure + private final Superstructure superstructure_ = new Superstructure(arm_, climber_, intake_, shooter_); + + // Xbox Controllers + private final CommandXboxController driver_controller_ = new CommandXboxController(0); + private final CommandXboxController operator_controller_ = new CommandXboxController(1); + + @Override public void robotInit() {} diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java index 16775da..ce0a7ee 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -1,5 +1,76 @@ package org.ghrobotics.frc2024; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +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 java.util.function.DoubleSupplier; + +import org.ghrobotics.frc2024.commands.ArmToPosition; +import org.ghrobotics.frc2024.commands.ClimbReset; +import org.ghrobotics.frc2024.commands.ClimbToPosition; +import org.ghrobotics.frc2024.commands.ClimbTeleop; + +import org.ghrobotics.frc2024.subsystems.Arm; +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_; + + //Store Position + public String state = "STOW"; + + // Constructor + public Superstructure(Arm arm, Climber climber, Intake intake, Shooter shooter) { + arm_ = arm; + climber_ = climber; + intake_ = intake; + shooter_ = shooter; + } + + // Position Setter + public Command setPosition(Position pos) { + return new SequentialCommandGroup( + new InstantCommand(() -> this.state = pos.posname), + new ParallelCommandGroup( + new ArmToPosition(arm_, pos.angle) + ).withTimeout(6) + ); + } + + //GetPosition of Superstructure + public String getState() { + //System.out.println(state); + return state; + } + + public enum Position { + STOW(0, "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/commands/ArmToPosition.java b/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java index 157a424..8800fe4 100644 --- a/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java +++ b/src/main/java/org/ghrobotics/frc2024/commands/ArmToPosition.java @@ -32,6 +32,6 @@ public boolean isFinished() { // Constants private static final class Constants { - public static final double kTolerance = 0; // NEED TO UPDATE + 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..82ab19d --- /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..6d7b1b8 --- /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..428cf58 --- /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/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index ead1400..d798d65 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -53,6 +53,8 @@ public Arm() { 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); @@ -106,7 +108,7 @@ public void periodic() { io_.current = follower_.getOutputCurrent(); if (io_.wants_zero) { - io_.wants_zero = false; + io_.wants_zero = false; leader_encoder_.setPosition(Constants.kMaxAngle); follower_encoder_.setPosition(Constants.kMaxAngle); } From 09be8ce43e4e6652f474a839398ea7bd5a0a3da3 Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Fri, 16 Feb 2024 21:08:04 -0500 Subject: [PATCH 11/20] Added climber to superstructure --- .../java/org/ghrobotics/frc2024/Robot.java | 22 +--- .../ghrobotics/frc2024/Superstructure.java | 108 ++++++++++-------- .../frc2024/subsystems/Climber.java | 2 +- 3 files changed, 65 insertions(+), 67 deletions(-) diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java index 5f638f5..06fe0db 100644 --- a/src/main/java/org/ghrobotics/frc2024/Robot.java +++ b/src/main/java/org/ghrobotics/frc2024/Robot.java @@ -5,26 +5,14 @@ package org.ghrobotics.frc2024; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.DriverStation; -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.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; -// import org.ghrobotics.frc2024.auto.AutoSelector; -// import org.ghrobotics.frc2024.commands.DriveBrakeMode; -// import org.ghrobotics.frc2024.commands.DriveTeleop; -// import org.ghrobotics.frc2024.commands.DriveTowardPosition; -// import org.ghrobotics.frc2024.commands.HomeSuperstructure; -// import org.ghrobotics.frc2024.subsystems.Drivetrain; + import org.ghrobotics.frc2024.subsystems.Arm; import org.ghrobotics.frc2024.subsystems.Climber; import org.ghrobotics.frc2024.subsystems.Intake; import org.ghrobotics.frc2024.subsystems.Shooter; -// import org.ghrobotics.frc2024.subsystems.LED; -// import org.ghrobotics.frc2024.subsystems.LED.StandardLEDOutput; -// import org.ghrobotics.frc2024.subsystems.Limelight; -// import org.ghrobotics.frc2024.subsystems.PoseEstimator; + /** @@ -57,7 +45,9 @@ public class Robot extends TimedRobot { public void robotInit() {} @Override - public void robotPeriodic() {} + public void robotPeriodic() { + SmartDashboard.putString("Current State", superstructure_.getState()); + } @Override public void autonomousInit() {} diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java index ce0a7ee..20a0fcf 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -1,20 +1,12 @@ package org.ghrobotics.frc2024; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; 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 java.util.function.DoubleSupplier; import org.ghrobotics.frc2024.commands.ArmToPosition; -import org.ghrobotics.frc2024.commands.ClimbReset; -import org.ghrobotics.frc2024.commands.ClimbToPosition; -import org.ghrobotics.frc2024.commands.ClimbTeleop; - import org.ghrobotics.frc2024.subsystems.Arm; import org.ghrobotics.frc2024.subsystems.Climber; import org.ghrobotics.frc2024.subsystems.Intake; @@ -23,54 +15,70 @@ public class Superstructure { - // Subsystems - private final Arm arm_; - private final Climber climber_; - private final Intake intake_; - private final Shooter shooter_; + // Subsystems + private final Arm arm_; + private final Climber climber_; + private final Intake intake_; + private final Shooter shooter_; - //Store Position - public String state = "STOW"; + //Store Position + public String state = "STOW"; - // Constructor - public Superstructure(Arm arm, Climber climber, Intake intake, Shooter shooter) { - arm_ = arm; - climber_ = climber; - intake_ = intake; - shooter_ = shooter; - } + // Constructor + public Superstructure(Arm arm, Climber climber, Intake intake, Shooter shooter) { + arm_ = arm; + climber_ = climber; + intake_ = intake; + shooter_ = shooter; + } - // Position Setter - public Command setPosition(Position pos) { - return new SequentialCommandGroup( - new InstantCommand(() -> this.state = pos.posname), - new ParallelCommandGroup( - new ArmToPosition(arm_, pos.angle) - ).withTimeout(6) - ); - } + // 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) + ); + } - //GetPosition of Superstructure - public String getState() { - //System.out.println(state); - return state; - } + // Jog Left Climber + public Command jogLeftClimber(double percent) { + return new StartEndCommand( + () -> climber_.setLeftPercent(percent), + () -> climber_.setLeftPercent(0.1/12), + climber_ + ); + } - public enum Position { - STOW(0, "STOW"), - SUBWOOFER(0, "SUBWOOFER"), - AMP(0, "AMP"), - GROUND_INTAKE(0, "GROUND_INTAKE"), - SOURCE_INTAKE(0, "SOURCE_INTAKE"); - - final double angle; - final String posname; + // Jog Right Climber + public Command jogRightClimber(double percent) { + return new StartEndCommand( + () -> climber_.setRightPercent(percent), + () -> climber_.setRightPercent(0.1/12), + climber_ + ); + } - Position(double angle_deg, String name) { - this.angle = Math.toRadians(angle_deg); - this.posname = name; - } + // GetPosition of Superstructure + public String getState() { + //System.out.println(state); + return state; + } - } + public enum Position { + STOW(0, "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/subsystems/Climber.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java index a3b9e3a..f717b0a 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java @@ -119,7 +119,7 @@ public void periodic() { switch (right_output_type_) { case PERCENT: // Send the percent output values directly to the motor controller. - left_leader_.set(io_.r_demand); + right_leader_.set(io_.r_demand); case POSITION: // Calculate feedforward value and add to built-in motor controller PID. From 188f3af364170ce079ee880bf8e2b6a8a26b5b79 Mon Sep 17 00:00:00 2001 From: suhaank077 Date: Sat, 17 Feb 2024 11:36:01 -0500 Subject: [PATCH 12/20] fixing shooter and intake stuff --- .../java/org/ghrobotics/frc2024/subsystems/Intake.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java index b0a59ff..a4fbf74 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -25,13 +25,14 @@ public void periodic() { } //get, set % output on intake. parameter is percent output [-1, 1] - public void setIntakePercent(double value) { + public double getPercent() { + return io_.leader_bridge_demand; + } + public void setPercent(double value) { io_.leader_bridge_demand = value; } //Returns the % output of the intake and bridge. - public double getIntakePercent() { - return io_.leader_bridge_demand; - } + // IO public static class PeriodicIO { //input From c8aa26b854d46b09e87706dc57c8f167aecf95e7 Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Fri, 23 Feb 2024 17:21:32 -0500 Subject: [PATCH 13/20] Added to superstructure, constants, fixed formatting --- Hardware.md | 3 + .../ghrobotics/frc2024/Superstructure.java | 22 +- .../ghrobotics/frc2024/subsystems/Arm.java | 427 +++++++++--------- .../frc2024/subsystems/Climber.java | 3 - .../ghrobotics/frc2024/subsystems/Intake.java | 88 ++-- 5 files changed, 284 insertions(+), 259 deletions(-) diff --git a/Hardware.md b/Hardware.md index cb65e13..2054a94 100644 --- a/Hardware.md +++ b/Hardware.md @@ -15,6 +15,9 @@ This document details all of the hardware (motors, motor controllers, sensors, e | 6 | Drivetrain BL Steer | NEO | SPARK MAX | ? | | 7 | Drivetrain BR Drive | NEO | SPARK MAX | ? | | 8 | Drivetrain BR Steer | NEO | SPARK MAX | ? | +| 9 | Arm L | NEO | SPARK MAX | ? | +| 10 | Arm R | NEO | SPARK MAX | ? | +| 11 | Intake | NEO | SPARK MAX | ? | diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java index 20a0fcf..a55d7d3 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -12,8 +12,6 @@ import org.ghrobotics.frc2024.subsystems.Intake; import org.ghrobotics.frc2024.subsystems.Shooter; - - public class Superstructure { // Subsystems private final Arm arm_; @@ -42,6 +40,25 @@ public Command setPosition(Position pos) { ); } + // Intake Setter + // Might change to functional later + public Command setIntake(double percent) { + return new StartEndCommand( + () -> intake_.setPercent(percent), + () -> intake_.setPercent(0), + intake_ + ); + } + + // Jog Arm + public Command jogArm(double percent) { + return new StartEndCommand( + () -> arm_.setPercent(percent), + () -> arm_.setAngle(arm_.getAngle()), + arm_ + ); + } + // Jog Left Climber public Command jogLeftClimber(double percent) { return new StartEndCommand( @@ -62,7 +79,6 @@ public Command jogRightClimber(double percent) { // GetPosition of Superstructure public String getState() { - //System.out.println(state); return state; } diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index d798d65..3218c4b 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -13,235 +13,236 @@ import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.io.PipedOutputStream; 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; + // 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; + + + // Constructor + public Arm() { + // Initialize motor controllers + leader_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_.restoreFactoryDefaults(); + leader_.setInverted(true); + leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); - // Simulation - private final SingleJointedArmSim physics_sim_; - private final SimDeviceSim leader_sim_; - - // IO - private final PeriodicIO io_ = new PeriodicIO(); - private OutputType output_type_ = OutputType.PERCENT; + 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); - - // Constructor - public Arm() { - // Initialize motor controllers - leader_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); - leader_.restoreFactoryDefaults(); - leader_.setInverted(true); - 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)); + 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_.angle = follower_encoder_.getPosition(); + io_.angular_velocity = follower_encoder_.getVelocity(); + io_.current = follower_.getOutputCurrent(); + + if (io_.wants_zero) { + io_.wants_zero = false; + leader_encoder_.setPosition(Constants.kMinAngle); + follower_encoder_.setPosition(Constants.kMinAngle); } - @Override - public void periodic() { - // Read inputs - io_.angle = leader_encoder_.getPosition(); - io_.angular_velocity = leader_encoder_.getVelocity(); - io_.current = leader_.getOutputCurrent(); - - io_.angle = follower_encoder_.getPosition(); - io_.angular_velocity = follower_encoder_.getVelocity(); - io_.current = follower_.getOutputCurrent(); - - if (io_.wants_zero) { - io_.wants_zero = false; - leader_encoder_.setPosition(Constants.kMaxAngle); - follower_encoder_.setPosition(Constants.kMaxAngle); - } - - // 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); - - 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; - } + // Reset controller if we have to + if (reset_pid_) { + reset_pid_ = false; + fb_.reset(io_.angle, io_.angular_velocity); } - @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); + // Write outputs + switch (output_type_) { + case PERCENT: + leader_.set(io_.demand); + follower_.set(io_.demand); - // Update physics sim forward in time - physics_sim_.update(0.02); + // Set simulated inputs + if (RobotBase.isSimulation()) + leader_sim_.getDouble("Applied Output").set(io_.demand * 12); - // Update encoder values - leader_sim_.getDouble("Position").set(physics_sim_.getAngleRads()); - leader_sim_.getDouble("Velocity").set(physics_sim_.getVelocityRadPerSec()); - } + break; + case ANGLE: + double feedback = fb_.calculate(io_.angle); - public void setPercent(double percent) { - output_type_ = OutputType.PERCENT; - io_.demand = percent; - reset_pid_ = true; - } - - public void setAngle(double angle) { - output_type_ = OutputType.ANGLE; - fb_.setGoal(angle); - reset_pid_ = true; - } + 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); - 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; - } - - // Position Getter - public double getAngle() { - return io_.angle; - } - - public double getAngularVelocity() { - return io_.angular_velocity; - } - - public double getAngularVelocitySetpoint() { - return fb_.getSetpoint().velocity; + leader_.setVoltage(feedback + feedforward); + follower_.setVoltage(feedback + feedforward); + break; } + } + + @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; + } + + public void setAngle(double angle) { + output_type_ = OutputType.ANGLE; + fb_.setGoal(angle); + reset_pid_ = true; + } + + 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; + } + + // Position Getter + public double getAngle() { + return io_.angle; + } + + public double getAngularVelocity() { + return io_.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 current; + + //Outputs + boolean wants_zero; + double demand; + } + + + // Constants (TO UPDATE) + public static class Constants { + // Motor Controllers + public static final int kLeaderId = 0; + public static final int kFollowerId = 0; - public double getCurrent() { - return io_.current; - } + // Physical Constants + public static final double kGearRatio = 0; + public static final double kMinAngle = Math.toRadians(0); + public static final double kMaxAngle = Math.toRadians(0); + public static final double kArmLength = 0; - // Output Type - private enum OutputType { - PERCENT, ANGLE - } + // Feedforward + public static final double kA = 0; // volts * seconds^2 / radians + public static final double kG = 0; // volts + public static final double kS = 0; // volts + public static final double kV = 0; // volts * seconds/radians - //IO - private static class PeriodicIO { - //Inputs - double angle; - double angular_velocity; - double current; + // Current Limit + public static final double kCurrentLimit = 50; - //Outputs - boolean wants_zero; - double demand; - } - - - // Constants (TO UPDATE) - public static class Constants { - // Motor Controllers - public static final int kLeaderId = 0; - public static final int kFollowerId = 0; - - // Physical Constants - public static final double kGearRatio = 0; - public static final double kMinAngle = Math.toRadians(0); - public static final double kMaxAngle = Math.toRadians(0); - public static final double kArmLength = 0; - - // Feedforward - public static final double kA = 0; // volts * seconds^2 / radians - public static final double kG = 0; // volts - public static final double kS = 0; // volts - public static final double kV = 0; // volts * seconds/radians - - // Current Limit - public static final double kCurrentLimit = 0; - - // Control - public static double kMaxVelocity = 0; - public static double kMaxAcceleration = 0; - public static double kP = 0; - } + // 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 index f717b0a..9b98900 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java @@ -8,10 +8,7 @@ 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.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.List; public class Climber extends SubsystemBase { // Motor Controllers diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java index a4fbf74..9086281 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -3,48 +3,56 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.MathUtil; +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_; + private final PeriodicIO io_ = new PeriodicIO(); + + private final PIDController controller_ = new PIDController(Constants.kP, 0, 0); + + // Constructor + public Intake() { + // Initialize motor controllers + leader_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_.restoreFactoryDefaults(); + leader_.setInverted(true); + leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + } + + public void periodic() { + // Read inputs. + io_.current = leader_.getOutputCurrent(); + leader_.set(io_.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_.demand = correction; + } + + // IO + public static class PeriodicIO { + // Input + double current; + + // Output + double demand; + } + + // Constants + public static class Constants { // Motor Controllers - private final CANSparkMax leader_; // Left - private final PeriodicIO io_ = new PeriodicIO(); - // Constructor - public Intake() { - // Initialize motor controllers - leader_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); - leader_.restoreFactoryDefaults(); - leader_.setInverted(true); - leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); - } - - public void periodic() { - // Read inputs. - io_.leader_supply_current = leader_.getOutputCurrent(); - leader_.set(io_.leader_bridge_demand); - } - - //get, set % output on intake. parameter is percent output [-1, 1] - public double getPercent() { - return io_.leader_bridge_demand; - } - public void setPercent(double value) { - io_.leader_bridge_demand = value; - } - //Returns the % output of the intake and bridge. - - // IO - public static class PeriodicIO { - //input - double leader_supply_current; - //output - double leader_bridge_demand; - } - - // Constants - public static class Constants { - // Motor Controllers - public static final int kLeaderId = 0; - public static final int kFollowerId = 0; - } + public static final int kLeaderId = 0; + + // PID Constants + public static final double kP = 0.8; + } } From c96b758be58c5866e7ed55a4a56065ad60392173 Mon Sep 17 00:00:00 2001 From: Yash Gupta Date: Sat, 24 Feb 2024 00:22:06 -0500 Subject: [PATCH 14/20] added shooter commands --- .../org/ghrobotics/frc2024/Superstructure.java | 9 +++++++++ .../ghrobotics/frc2024/subsystems/Shooter.java | 18 ++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java index a55d7d3..ce221a3 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -50,6 +50,15 @@ public Command setIntake(double percent) { ); } + // Shooter Setter + public Command setShooter(double percent) { + return new StartEndCommand( + () -> shooter_.setPercent(percent), + () -> shooter_.setPercent(0), + shooter_ + ); + } + // Jog Arm public Command jogArm(double percent) { return new StartEndCommand( diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java index 567c5fa..e0136e4 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java @@ -2,8 +2,11 @@ import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.MathUtil; 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; @@ -13,6 +16,9 @@ public class Shooter extends SubsystemBase { 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 @@ -26,8 +32,17 @@ public Shooter() { right_leader_.setInverted(false); right_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); right_leader_.follow(left_leader_); + } + + 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 = correction; } + @Override public void periodic(){ //read inputs @@ -54,5 +69,8 @@ public static class Constants { // Motor Controllers public static final int kLeftLeaderId = 0; public static final int kRightLeaderId = 0; + + // PID + public static final double kP = 0.8; } } From 2aed196ad5eda1b80a98a89881cd10c3d4c12a30 Mon Sep 17 00:00:00 2001 From: Jake-GHHS Date: Sat, 24 Feb 2024 13:46:33 -0500 Subject: [PATCH 15/20] UpdatedIntakeFeb24 Changed code to work with 2 motors(i hope) --- .../ghrobotics/frc2024/subsystems/Intake.java | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java index 9086281..398136c 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -9,7 +9,8 @@ public class Intake extends SubsystemBase { // Motor Controllers - private final CANSparkMax leader_; + 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); @@ -17,16 +18,26 @@ public class Intake extends SubsystemBase { // Constructor public Intake() { // Initialize motor controllers - leader_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); - leader_.restoreFactoryDefaults(); - leader_.setInverted(true); - leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + leader_left_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_left_.restoreFactoryDefaults(); + leader_left_.setInverted(true); + leader_left_.setIdleMode(CANSparkMax.IdleMode.kCoast); + + + leader_right_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_right_.restoreFactoryDefaults(); + leader_right_.setInverted(false); + leader_right_.setIdleMode(CANSparkMax.IdleMode.kCoast); + leader_right_.follow(leader_left_); + + } public void periodic() { // Read inputs. - io_.current = leader_.getOutputCurrent(); - leader_.set(io_.demand); + io_.current_left_ = leader_left_.getOutputCurrent(); + io_.current_right_ = leader_right_.getOutputCurrent(); + leader_left_.set(io_.demand); } public double getPercent() { @@ -41,7 +52,8 @@ public void setPercent(double value) { // IO public static class PeriodicIO { // Input - double current; + double current_left_; + double current_right_; // Output double demand; From 59bb28e079de77e6b67e1d0be5888d6e436a4439 Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Sat, 24 Feb 2024 16:51:12 -0500 Subject: [PATCH 16/20] Intake and shooter almost finished --- Hardware.md | 6 +- .../java/org/ghrobotics/frc2024/Robot.java | 26 +++- .../ghrobotics/frc2024/Superstructure.java | 6 + .../frc2024/subsystems/Shooter.java | 126 ++++++++++-------- 4 files changed, 104 insertions(+), 60 deletions(-) diff --git a/Hardware.md b/Hardware.md index 2054a94..5974541 100644 --- a/Hardware.md +++ b/Hardware.md @@ -17,7 +17,11 @@ This document details all of the hardware (motors, motor controllers, sensors, e | 8 | Drivetrain BR Steer | NEO | SPARK MAX | ? | | 9 | Arm L | NEO | SPARK MAX | ? | | 10 | Arm R | NEO | SPARK MAX | ? | -| 11 | Intake | NEO | SPARK MAX | ? | +| 11 | Intake L | NEO | SPARK MAX | ? | +| 12 | Intake R | NEO | SPARK MAX | ? | +| 13 | Shooter L | NEO | SPARK MAX | ? | +| 14 | Shooter R | NEO | SPARK MAX | ? | + diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java index ec8e58d..5fbbe8f 100644 --- a/src/main/java/org/ghrobotics/frc2024/Robot.java +++ b/src/main/java/org/ghrobotics/frc2024/Robot.java @@ -8,7 +8,11 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; 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.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 +23,34 @@ 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(); // Robot State private final RobotState robot_state_ = new RobotState(drive_); // Xbox Controller private final CommandXboxController driver_controller_ = new CommandXboxController(0); + + // Superstructure + private final Superstructure superstructure_ = new Superstructure(arm_, climber_, intake_, shooter_); @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(); } - + @Override public void autonomousInit() {} @@ -71,4 +85,12 @@ public void simulationInit() {} @Override public void simulationPeriodic() {} + + private void setupTeleopControls() { + // Driver Control + driver_controller_.rightBumper().whileTrue(superstructure_.setShooter(1.0)); + + driver_controller_.pov(0).whileTrue(superstructure_.setIntake(1.0)); + } + } diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java index ce221a3..97ac867 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -1,5 +1,6 @@ package org.ghrobotics.frc2024; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -30,6 +31,11 @@ public Superstructure(Arm arm, Climber climber, Intake intake, Shooter shooter) shooter_ = shooter; } + public void periodic() { + SmartDashboard.putNumber("Shooter Percent", shooter_.getPercent()); + SmartDashboard.putNumber("Intake Percent", intake_.getPercent()); + } + // Position Setter public Command setPosition(Position pos) { return new SequentialCommandGroup( diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java index e0136e4..387072e 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java @@ -11,66 +11,78 @@ 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(true); - left_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); - - right_leader_ = new CANSparkMax(Constants.kRightLeaderId, MotorType.kBrushless); - right_leader_.restoreFactoryDefaults(); - right_leader_.setInverted(false); - right_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); - right_leader_.follow(left_leader_); - } + // 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(true); + left_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); + left_leader_.enableVoltageCompensation(12.0); - 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 = correction; - } - - @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); - SmartDashboard.putBoolean("shooterActive", io_.demand>0.2); - } + 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 = correction; + } + + @Override + public void periodic(){ + //read inputs - // IO - public static class PeriodicIO { - //inputs - double left_leader_supply_current; - double right_leader_supply_current; - //ouputs - double demand; - } - + 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; - // Constants - public static class Constants { - // Motor Controllers - public static final int kLeftLeaderId = 0; - public static final int kRightLeaderId = 0; + // PID + public static final double kP = 0.8; - // PID - public static final double kP = 0.8; - } + // Current Limit + public static final double kCurrentLimit = 40; + } } From caf83efbcba5dcbef178d951ec4105d91e77848b Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Sun, 25 Feb 2024 20:19:59 -0500 Subject: [PATCH 17/20] Simulation added, climber removed --- .../java/org/ghrobotics/frc2024/Robot.java | 56 +- .../ghrobotics/frc2024/Superstructure.java | 51 +- .../org/ghrobotics/frc2024/Telemetry.java | 43 ++ .../frc2024/commands/ClimbReset.java | 242 +++---- .../frc2024/commands/ClimbTeleop.java | 132 ++-- .../frc2024/commands/ClimbToPosition.java | 72 +- .../ghrobotics/frc2024/subsystems/Arm.java | 65 +- .../frc2024/subsystems/Climber.java | 628 +++++++++--------- .../ghrobotics/frc2024/subsystems/Intake.java | 35 +- .../frc2024/subsystems/Shooter.java | 12 +- 10 files changed, 736 insertions(+), 600 deletions(-) create mode 100644 src/main/java/org/ghrobotics/frc2024/Telemetry.java diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java index 5fbbe8f..7b86682 100644 --- a/src/main/java/org/ghrobotics/frc2024/Robot.java +++ b/src/main/java/org/ghrobotics/frc2024/Robot.java @@ -5,11 +5,18 @@ 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.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +import org.ghrobotics.frc2024.Superstructure.Position; import org.ghrobotics.frc2024.commands.DriveTeleop; import org.ghrobotics.frc2024.subsystems.Arm; -import org.ghrobotics.frc2024.subsystems.Climber; +// import org.ghrobotics.frc2024.subsystems.Climber; import org.ghrobotics.frc2024.subsystems.Drive; import org.ghrobotics.frc2024.subsystems.Intake; import org.ghrobotics.frc2024.subsystems.Shooter; @@ -24,19 +31,31 @@ 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 Climber climber_ = new Climber(); private final Intake intake_ = new Intake(); private final Shooter shooter_ = new Shooter(); // Robot State private final RobotState robot_state_ = new RobotState(drive_); + + // Telemetry + private final Telemetry telemetry_ = new Telemetry(robot_state_, arm_); // Xbox Controller - private final CommandXboxController driver_controller_ = new CommandXboxController(0); + private final CommandXboxController driver_controller_ = new CommandXboxController(1); + // Playstation controller just for testing + private final CommandPS4Controller ps4_controller_ = new CommandPS4Controller(0); // Superstructure - private final Superstructure superstructure_ = new Superstructure(arm_, climber_, intake_, shooter_); + private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_); + + 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_)); @@ -49,8 +68,15 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); 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()); } + @Override public void autonomousInit() {} @@ -63,13 +89,19 @@ public void autonomousPeriodic() {} @Override public void teleopInit() { drive_.setBrakeMode(true); + arm_.setBrakeMode(true); + + // test().schedule(); } @Override public void teleopPeriodic() {} @Override - public void disabledInit() {} + public void disabledInit() { + drive_.setBrakeMode(false); + arm_.setBrakeMode(false); + } @Override public void disabledPeriodic() {} @@ -88,9 +120,19 @@ public void simulationPeriodic() {} private void setupTeleopControls() { // Driver Control - driver_controller_.rightBumper().whileTrue(superstructure_.setShooter(1.0)); + driver_controller_.rightBumper().whileTrue(superstructure_.setShooter(-0.5)); + + driver_controller_.leftBumper().whileTrue(superstructure_.setIntake(-0.20)); + + driver_controller_.pov(0).whileTrue(superstructure_.setIntake(0.3)); + + driver_controller_.pov(180).whileTrue(superstructure_.setShooter(0.3)); + + driver_controller_.b().whileTrue(superstructure_.shoot()); + + ps4_controller_.square().onTrue(test()); - driver_controller_.pov(0).whileTrue(superstructure_.setIntake(1.0)); + } } diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java index 97ac867..c09bc76 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -2,6 +2,8 @@ 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.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -9,14 +11,14 @@ import org.ghrobotics.frc2024.commands.ArmToPosition; import org.ghrobotics.frc2024.subsystems.Arm; -import org.ghrobotics.frc2024.subsystems.Climber; +// 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 Climber climber_; private final Intake intake_; private final Shooter shooter_; @@ -24,16 +26,17 @@ public class Superstructure { public String state = "STOW"; // Constructor - public Superstructure(Arm arm, Climber climber, Intake intake, Shooter shooter) { + public Superstructure(Arm arm, Intake intake, Shooter shooter) { arm_ = arm; - climber_ = climber; + // climber_ = climber; intake_ = intake; shooter_ = shooter; } public void periodic() { SmartDashboard.putNumber("Shooter Percent", shooter_.getPercent()); - SmartDashboard.putNumber("Intake Percent", intake_.getPercent()); + // SmartDashboard.putNumber("Intake Percent", intake_.getPercent()); + SmartDashboard.putNumber("Arm Angle", Math.toDegrees(arm_.getAngle())); } // Position Setter @@ -51,7 +54,7 @@ public Command setPosition(Position pos) { public Command setIntake(double percent) { return new StartEndCommand( () -> intake_.setPercent(percent), - () -> intake_.setPercent(0), + () -> intake_.stopMotor(), intake_ ); } @@ -60,7 +63,7 @@ public Command setIntake(double percent) { public Command setShooter(double percent) { return new StartEndCommand( () -> shooter_.setPercent(percent), - () -> shooter_.setPercent(0), + () -> shooter_.stopMotor(), shooter_ ); } @@ -74,23 +77,27 @@ public Command jogArm(double percent) { ); } - // Jog Left Climber - public Command jogLeftClimber(double percent) { - return new StartEndCommand( - () -> climber_.setLeftPercent(percent), - () -> climber_.setLeftPercent(0.1/12), - climber_ - ); + public Command shoot() { + return Commands.parallel(setIntake(-0.6), setShooter(-0.75)); } - // Jog Right Climber - public Command jogRightClimber(double percent) { - return new StartEndCommand( - () -> climber_.setRightPercent(percent), - () -> climber_.setRightPercent(0.1/12), - climber_ - ); - } + // // 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() { 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..9257355 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/Telemetry.java @@ -0,0 +1,43 @@ +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.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 Mechanism2d superstructure_; + + + public Telemetry(RobotState robot_state, Arm arm) { + ShuffleboardTab tab_ = Shuffleboard.getTab("Robot 2024"); + + 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/ClimbReset.java b/src/main/java/org/ghrobotics/frc2024/commands/ClimbReset.java index 82ab19d..69c459a 100644 --- a/src/main/java/org/ghrobotics/frc2024/commands/ClimbReset.java +++ b/src/main/java/org/ghrobotics/frc2024/commands/ClimbReset.java @@ -1,121 +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 +// 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 index 6d7b1b8..ba89fe3 100644 --- a/src/main/java/org/ghrobotics/frc2024/commands/ClimbTeleop.java +++ b/src/main/java/org/ghrobotics/frc2024/commands/ClimbTeleop.java @@ -1,77 +1,77 @@ -package org.ghrobotics.frc2024.commands; +// 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; +// 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_; +// 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_; +// // 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; +// /** +// * 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_); - } +// // 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; - } +// @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()); +// // 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; - } - } +// // 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()); +// // 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 +// // 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 index 428cf58..521caf1 100644 --- a/src/main/java/org/ghrobotics/frc2024/commands/ClimbToPosition.java +++ b/src/main/java/org/ghrobotics/frc2024/commands/ClimbToPosition.java @@ -1,43 +1,43 @@ -package org.ghrobotics.frc2024.commands; +// package org.ghrobotics.frc2024.commands; -import edu.wpi.first.wpilibj2.command.Command; -import org.ghrobotics.frc2024.subsystems.Climber; +// 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_; +// 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; +// /** +// * 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_); - } +// // Set subsystem requirements. +// addRequirements(climber_); +// } - @Override - public void initialize() { +// @Override +// public void initialize() { - // Set position setpoints on climber arms. - climber_.setLeftPosition(l_setpoint_); - climber_.setRightPosition(r_setpoint_); - } +// // 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 +// @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/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index 3218c4b..7618649 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -1,6 +1,7 @@ package org.ghrobotics.frc2024.subsystems; import com.revrobotics.CANSparkMax; +import com.ctre.phoenix6.controls.VoltageOut; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.VecBuilder; @@ -9,10 +10,14 @@ 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.units.Measure; +import edu.wpi.first.units.Voltage; 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; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class Arm extends SubsystemBase { @@ -36,7 +41,6 @@ public class Arm extends SubsystemBase { // IO private final PeriodicIO io_ = new PeriodicIO(); private OutputType output_type_ = OutputType.PERCENT; - // Constructor public Arm() { @@ -48,7 +52,7 @@ public Arm() { follower_ = new CANSparkMax(Constants.kFollowerId, MotorType.kBrushless); follower_.restoreFactoryDefaults(); - follower_.setInverted(false); + follower_.setInverted(true); follower_.setIdleMode(CANSparkMax.IdleMode.kCoast); follower_.follow(leader_); @@ -100,9 +104,9 @@ public void periodic() { io_.angular_velocity = leader_encoder_.getVelocity(); io_.current = leader_.getOutputCurrent(); - io_.angle = follower_encoder_.getPosition(); - io_.angular_velocity = follower_encoder_.getVelocity(); - io_.current = follower_.getOutputCurrent(); + io_.follower_angular_velocity = follower_encoder_.getVelocity(); + io_.follower_velocity = follower_encoder_.getVelocity(); + if (io_.wants_zero) { io_.wants_zero = false; @@ -124,8 +128,8 @@ public void periodic() { // Set simulated inputs if (RobotBase.isSimulation()) - leader_sim_.getDouble("Applied Output").set(io_.demand * 12); - + leader_sim_.getDouble("Applied Output").set(io_.demand * 12); + SmartDashboard.putNumber("ARM angle", getAngle()); break; case ANGLE: double feedback = fb_.calculate(io_.angle); @@ -135,7 +139,7 @@ public void periodic() { double feedforward = ff_.calculate(io_.angle, velocity_setpoint, acceleration_setpoint); leader_.setVoltage(feedback + feedforward); - follower_.setVoltage(feedback + feedforward); + // follower_.setVoltage(feedback + feedforward); break; } } @@ -162,6 +166,10 @@ public void setPercent(double percent) { reset_pid_ = true; } + /** + * Set Arm Angle + * @param angle in Radians + */ public void setAngle(double angle) { output_type_ = OutputType.ANGLE; fb_.setGoal(angle); @@ -184,15 +192,32 @@ public void zero() { io_.wants_zero = true; } - // Position Getter + /** + * 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; } @@ -211,7 +236,9 @@ private static class PeriodicIO { //Inputs double angle; double angular_velocity; + double follower_angular_velocity; double current; + double follower_velocity; //Outputs boolean wants_zero; @@ -222,23 +249,23 @@ private static class PeriodicIO { // Constants (TO UPDATE) public static class Constants { // Motor Controllers - public static final int kLeaderId = 0; - public static final int kFollowerId = 0; + public static final int kLeaderId = 9; + public static final int kFollowerId = 10; // Physical Constants - public static final double kGearRatio = 0; + public static final double kGearRatio = 140; public static final double kMinAngle = Math.toRadians(0); - public static final double kMaxAngle = Math.toRadians(0); - public static final double kArmLength = 0; + public static final double kMaxAngle = Math.toRadians(120); + public static final double kArmLength = 0.15; // Feedforward - public static final double kA = 0; // volts * seconds^2 / radians - public static final double kG = 0; // volts - public static final double kS = 0; // volts - public static final double kV = 0; // volts * seconds/radians + 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 = 50; + public static final double kCurrentLimit = 40; // Control public static double kMaxVelocity = 2.14; diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java index 9b98900..632a9ff 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Climber.java @@ -1,318 +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); +// 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); +// 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; - } +// 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 +// 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/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java index 398136c..9c61b46 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -18,15 +18,15 @@ public class Intake extends SubsystemBase { // Constructor public Intake() { // Initialize motor controllers - leader_left_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_left_ = new CANSparkMax(Constants.kLeaderLeftId, MotorType.kBrushless); leader_left_.restoreFactoryDefaults(); leader_left_.setInverted(true); leader_left_.setIdleMode(CANSparkMax.IdleMode.kCoast); - leader_right_ = new CANSparkMax(Constants.kLeaderId, MotorType.kBrushless); + leader_right_ = new CANSparkMax(Constants.kLeaderRightId, MotorType.kBrushless); leader_right_.restoreFactoryDefaults(); - leader_right_.setInverted(false); + leader_right_.setInverted(true); leader_right_.setIdleMode(CANSparkMax.IdleMode.kCoast); leader_right_.follow(leader_left_); @@ -37,16 +37,27 @@ public void periodic() { // Read inputs. io_.current_left_ = leader_left_.getOutputCurrent(); io_.current_right_ = leader_right_.getOutputCurrent(); - leader_left_.set(io_.demand); + leader_left_.set(io_.left_demand); + leader_right_.set(io_.right_demand); } - public double getPercent() { - return io_.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) { - double correction = MathUtil.clamp(controller_.calculate(getPercent(), value), -1.0, 1.0); - io_.demand = correction; + io_.left_demand = value; + io_.right_demand = value - 0.1; + } + public void stopMotor() { + io_.left_demand = 0; + io_.right_demand = 0; } // IO @@ -56,13 +67,15 @@ public static class PeriodicIO { double current_right_; // Output - double demand; + double left_demand; + double right_demand; } // Constants public static class Constants { // Motor Controllers - public static final int kLeaderId = 0; + 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 index 387072e..7dba744 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java @@ -24,7 +24,7 @@ public Shooter() { // Initialize motor controllers left_leader_ = new CANSparkMax(Constants.kLeftLeaderId, MotorType.kBrushless); left_leader_.restoreFactoryDefaults(); - left_leader_.setInverted(true); + left_leader_.setInverted(false); left_leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); left_leader_.enableVoltageCompensation(12.0); @@ -45,8 +45,12 @@ public double getPercent() { } public void setPercent(double value) { - double correction = MathUtil.clamp(controller_.calculate(getPercent(), value), -1.0, 1.0); - io_.demand = correction; + // double correction = MathUtil.clamp(controller_.calculate(getPercent(), value), -1.0, 1.0); + io_.demand = value; + } + + public void stopMotor() { + io_.demand = 0; } @Override @@ -80,7 +84,7 @@ public static class Constants { public static final int kRightLeaderId = 14; // PID - public static final double kP = 0.8; + public static final double kP = 2.8; // Current Limit public static final double kCurrentLimit = 40; From c9d21006d135161f266ebcf65ecdeab38a7de596 Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Mon, 26 Feb 2024 15:47:39 -0500 Subject: [PATCH 18/20] Formatting --- simgui.json | 11 +++++++++++ .../org/ghrobotics/frc2024/Superstructure.java | 1 - .../java/org/ghrobotics/frc2024/Telemetry.java | 14 ++++++++++++-- .../org/ghrobotics/frc2024/subsystems/Arm.java | 4 ---- .../org/ghrobotics/frc2024/subsystems/Intake.java | 1 - .../org/ghrobotics/frc2024/subsystems/Shooter.java | 1 - 6 files changed, 23 insertions(+), 9 deletions(-) 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/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java index c09bc76..ccc9f63 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -3,7 +3,6 @@ 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.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/org/ghrobotics/frc2024/Telemetry.java b/src/main/java/org/ghrobotics/frc2024/Telemetry.java index 9257355..832d0b5 100644 --- a/src/main/java/org/ghrobotics/frc2024/Telemetry.java +++ b/src/main/java/org/ghrobotics/frc2024/Telemetry.java @@ -7,6 +7,7 @@ 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; @@ -17,11 +18,20 @@ 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("Robot 2024"); + 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); @@ -39,5 +49,5 @@ public void periodic() { for (Runnable fn : periodic_registry_) fn.run(); // System.out.println(periodic_registry_); -} + } } diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index 7618649..538ac82 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -1,7 +1,6 @@ package org.ghrobotics.frc2024.subsystems; import com.revrobotics.CANSparkMax; -import com.ctre.phoenix6.controls.VoltageOut; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.VecBuilder; @@ -10,14 +9,11 @@ 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.units.Measure; -import edu.wpi.first.units.Voltage; 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; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class Arm extends SubsystemBase { diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java index 9c61b46..8040af2 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -3,7 +3,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java index 7dba744..83b9710 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Shooter.java @@ -2,7 +2,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.math.controller.PIDController; From 5ef62333f689f4d75adf1fa90ebcbc23451d70d8 Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Mon, 26 Feb 2024 17:22:19 -0500 Subject: [PATCH 19/20] added setangle pid --- .../org/ghrobotics/frc2024/subsystems/Arm.java | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java index 538ac82..653ecf9 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -3,8 +3,11 @@ 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; @@ -37,6 +40,8 @@ public class Arm extends SubsystemBase { // IO private final PeriodicIO io_ = new PeriodicIO(); private OutputType output_type_ = OutputType.PERCENT; + + private final PIDController pid_ = new PIDController(0.5, 0, 0); // Constructor public Arm() { @@ -172,6 +177,16 @@ public void setAngle(double angle) { reset_pid_ = true; } + /** + * Set Arm Angle using PID + * @param angle in Degrees + */ + public void setAnglePID(double angle) { + pid_.setSetpoint(angle); + double output = MathUtil.clamp(pid_.calculate(Math.toDegrees(getAngle())), -0.8, 0.8); + 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); From 2a076ac73d4cc3febe61319d7b8f0a2f1ae42c5b Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Wed, 28 Feb 2024 19:33:16 -0500 Subject: [PATCH 20/20] Working superstructure yay --- Hardware.md | 2 +- .../java/org/ghrobotics/frc2024/Robot.java | 82 +++++++++++++++---- .../ghrobotics/frc2024/Superstructure.java | 62 +++++++++++++- .../ghrobotics/frc2024/commands/ArmPID.java | 64 +++++++++++++++ .../frc2024/commands/DriveTeleop.java | 21 ++++- .../ghrobotics/frc2024/subsystems/Arm.java | 23 ++++-- .../ghrobotics/frc2024/subsystems/Feeder.java | 45 ++++++++++ .../ghrobotics/frc2024/subsystems/Intake.java | 4 +- 8 files changed, 274 insertions(+), 29 deletions(-) create mode 100644 src/main/java/org/ghrobotics/frc2024/commands/ArmPID.java create mode 100644 src/main/java/org/ghrobotics/frc2024/subsystems/Feeder.java diff --git a/Hardware.md b/Hardware.md index 5974541..7ec85aa 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/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java index 7b86682..b6e354b 100644 --- a/src/main/java/org/ghrobotics/frc2024/Robot.java +++ b/src/main/java/org/ghrobotics/frc2024/Robot.java @@ -8,16 +8,21 @@ 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; @@ -34,20 +39,29 @@ public class Robot extends TimedRobot { // 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 - private final CommandXboxController driver_controller_ = new CommandXboxController(1); + // 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); + // private final CommandPS4Controller ps4_controller_ = new CommandPS4Controller(0); // Superstructure - private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_); + private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_, feeder_); public Command test() { @@ -70,10 +84,13 @@ public void robotPeriodic() { 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("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()); } @@ -95,7 +112,20 @@ public void teleopInit() { } @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() { @@ -120,19 +150,41 @@ public void simulationPeriodic() {} private void setupTeleopControls() { // Driver Control - driver_controller_.rightBumper().whileTrue(superstructure_.setShooter(-0.5)); + driver_controller_.rightTrigger().whileTrue(superstructure_.setShooter(-0.75)); + + driver_controller_.leftTrigger().whileTrue(superstructure_.setIntake(-0.50)); - driver_controller_.leftBumper().whileTrue(superstructure_.setIntake(-0.20)); + driver_controller_.leftBumper().whileTrue(superstructure_.setIntake(0.15)); - driver_controller_.pov(0).whileTrue(superstructure_.setIntake(0.3)); + driver_controller_.pov(0).whileTrue(superstructure_.setArmPercent(0.056)); - driver_controller_.pov(180).whileTrue(superstructure_.setShooter(0.3)); + // driver_controller_.pov(180).whileTrue(superstructure_.setShooter(0.3)); driver_controller_.b().whileTrue(superstructure_.shoot()); - ps4_controller_.square().onTrue(test()); + // 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 index ccc9f63..5cb1717 100644 --- a/src/main/java/org/ghrobotics/frc2024/Superstructure.java +++ b/src/main/java/org/ghrobotics/frc2024/Superstructure.java @@ -7,9 +7,12 @@ 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; @@ -20,16 +23,18 @@ public class Superstructure { // 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) { + public Superstructure(Arm arm, Intake intake, Shooter shooter, Feeder feeder) { arm_ = arm; // climber_ = climber; intake_ = intake; shooter_ = shooter; + feeder_ = feeder; } public void periodic() { @@ -48,6 +53,12 @@ public Command setPosition(Position pos) { ); } + 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) { @@ -80,6 +91,53 @@ 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( @@ -104,7 +162,7 @@ public String getState() { } public enum Position { - STOW(0, "STOW"), + STOW(-35, "STOW"), SUBWOOFER(0, "SUBWOOFER"), AMP(0, "AMP"), GROUND_INTAKE(0, "GROUND_INTAKE"), 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/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 index 653ecf9..5617dca 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java @@ -41,22 +41,22 @@ public class Arm extends SubsystemBase { private final PeriodicIO io_ = new PeriodicIO(); private OutputType output_type_ = OutputType.PERCENT; - private final PIDController pid_ = new PIDController(0.5, 0, 0); + 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(true); + leader_.setInverted(false); leader_.setIdleMode(CANSparkMax.IdleMode.kCoast); follower_ = new CANSparkMax(Constants.kFollowerId, MotorType.kBrushless); follower_.restoreFactoryDefaults(); - follower_.setInverted(true); + follower_.setInverted(false); follower_.setIdleMode(CANSparkMax.IdleMode.kCoast); - follower_.follow(leader_); + // follower_.follow(leader_); // Initialize encoders leader_encoder_ = leader_.getEncoder(); @@ -125,7 +125,7 @@ public void periodic() { switch (output_type_) { case PERCENT: leader_.set(io_.demand); - follower_.set(io_.demand); + follower_.set(-io_.demand); // Set simulated inputs if (RobotBase.isSimulation()) @@ -143,6 +143,11 @@ public void periodic() { // follower_.setVoltage(feedback + feedforward); break; } + + // Software Stop + if (io_.angle > 90 && io_.demand > 0) { + io_.demand = -0.1; + } } @Override @@ -183,7 +188,13 @@ public void setAngle(double angle) { */ public void setAnglePID(double angle) { pid_.setSetpoint(angle); - double output = MathUtil.clamp(pid_.calculate(Math.toDegrees(getAngle())), -0.8, 0.8); + 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); } 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 index 8040af2..3985e9f 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -20,13 +20,13 @@ public Intake() { leader_left_ = new CANSparkMax(Constants.kLeaderLeftId, MotorType.kBrushless); leader_left_.restoreFactoryDefaults(); leader_left_.setInverted(true); - leader_left_.setIdleMode(CANSparkMax.IdleMode.kCoast); + 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.kCoast); + leader_right_.setIdleMode(CANSparkMax.IdleMode.kBrake); leader_right_.follow(leader_left_);