From 969d187180569ec4b819b72a578f69f601ad8b2e Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Thu, 18 Jan 2024 20:20:15 -0500 Subject: [PATCH 01/15] WORK IN PROGRESS. basic arm setpoint control. TODO: switch to abs encoder, make open loop control. --- simgui.json | 4 +- .../jmhsrobotics/frc2024/RobotContainer.java | 5 ++ .../frc2024/subsystems/arm/ArmCommand.java | 44 ++++++++++++++++ .../frc2024/subsystems/arm/ArmSubsystem.java | 50 +++++++++++++++++++ 4 files changed, 102 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java diff --git a/simgui.json b/simgui.json index ddf9da1a..17976415 100644 --- a/simgui.json +++ b/simgui.json @@ -15,7 +15,9 @@ "/SmartDashboard/Field": "Field2d", "/SmartDashboard/IntakeCommand": "Command", "/SmartDashboard/PIDController[2]": "PIDController", - "/SmartDashboard/Schedular": "Scheduler" + "/SmartDashboard/Schedular": "Scheduler", + "/SmartDashboard/VisionDebug": "Field2d", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" }, "windows": { "/SmartDashboard/Auto Chooser": { diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index f587c41d..1f50cdd4 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -6,6 +6,8 @@ import org.jmhsrobotics.frc2024.controlBoard.CompControl; import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.DriveConstants; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; @@ -32,6 +34,7 @@ public class RobotContainer { private ControlBoard control = new CompControl(); // Subsystems private final DriveSubsystem driveSubsystem = new DriveSubsystem(); + private final ArmSubsystem armSubsystem = new ArmSubsystem(); private final VisionSubsystem vision = new VisionSubsystem(this.driveSubsystem); @@ -43,6 +46,8 @@ public RobotContainer() { SmartDashboard.putData("Schedular", CommandScheduler.getInstance()); configureBindings(); + armSubsystem.setDefaultCommand(new ArmCommand(0, armSubsystem)); + // Named commands must be added before building the chooser. configurePathPlanner(); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java new file mode 100644 index 00000000..28066631 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java @@ -0,0 +1,44 @@ +package org.jmhsrobotics.frc2024.subsystems.arm; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +public class ArmCommand extends Command { + + private ArmSubsystem armSubsystem; + private double angle; + ProfiledPIDController ArmPID; + + public ArmCommand(double angle, ArmSubsystem armSubsystem) { + this.armSubsystem = armSubsystem; + this.angle = angle; + this.ArmPID = new ProfiledPIDController(.1, 0, 0, null); + addRequirements(armSubsystem); + + } + @Override + public void initialize() { + // TODO Auto-generated method stub + super.initialize(); + ArmPID.reset(new State(angle, 0)); + ArmPID.setGoal(angle); + ArmPID.setTolerance(3, 3); + } + + @Override + public void execute() { + + double current = ArmPID.calculate(armSubsystem.getArmPitch()); + + armSubsystem.setArmPivot(current); + + } + + public boolean isFinished() { + return ArmPID.atGoal(); + } + + public void end() { + armSubsystem.setArmPivot(0); + } +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java new file mode 100644 index 00000000..aebe9fb3 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -0,0 +1,50 @@ +package org.jmhsrobotics.frc2024.subsystems.arm; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ArmSubsystem extends SubsystemBase { + + private MechanismLigament2d m_arm; + private CANSparkMax armPivot = new CANSparkMax(9, MotorType.kBrushless); + private RelativeEncoder PitchEncoder = armPivot.getEncoder(); + // private AbsoluteEncoder PitchEncoder=armPivot.getAbsoluteEncoder(kDutyCycle); + // private AbsoluteEncoder PitchEncoder=armPivot.getEncoder(); + + public ArmSubsystem() { + + armPivot.setSmartCurrentLimit(40); + armPivot.setIdleMode(com.revrobotics.CANSparkBase.IdleMode.kBrake); + + } + + public void setArmPivot(double amount) { + armPivot.set(amount); + } + + public double getArmPitch() { + return PitchEncoder.getPosition(); + } + + public void init2d() { + Mechanism2d mech = new Mechanism2d(3, 3); + MechanismRoot2d root = mech.getRoot("base", 1.5, 0); + var m_arm = root.append(new MechanismLigament2d("arm", 2, 0)); + SmartDashboard.putData("Mech2d", mech); + } + + @Override + public void periodic() { + // TODO Auto-generated method stub + super.periodic(); + m_arm.setAngle(getArmPitch()); + } + +} From 2c1ef0f729b01392367fd2b70c147dbc29f912e9 Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Mon, 22 Jan 2024 20:15:14 -0500 Subject: [PATCH 02/15] changed rel encoder to absolute, and finished sim. TODO: tune PID values and add setpoints --- simgui.json | 37 +++++++++++++++++-- .../jmhsrobotics/frc2024/RobotContainer.java | 3 ++ .../frc2024/controlBoard/CompControl.java | 5 +++ .../frc2024/controlBoard/ControlBoard.java | 2 + .../frc2024/subsystems/arm/ArmCommand.java | 16 ++++---- .../frc2024/subsystems/arm/ArmSubsystem.java | 15 +++++--- 6 files changed, 62 insertions(+), 16 deletions(-) diff --git a/simgui.json b/simgui.json index 705623b4..69bdb440 100644 --- a/simgui.json +++ b/simgui.json @@ -11,11 +11,17 @@ "open": true } } + }, + "Timing": { + "window": { + "visible": false + } } }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/SmartDashboard/ArmSubsystem/armSIM": "Mechanism2d", "/SmartDashboard/Auto Chooser": "String Chooser", "/SmartDashboard/Field": "Field2d", "/SmartDashboard/IntakeCommand": "Command", @@ -25,17 +31,17 @@ "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" }, "windows": { - "/SmartDashboard/Auto Chooser": { + "/SmartDashboard/ArmSubsystem/armSIM": { "window": { "visible": true } }, - "/SmartDashboard/Field": { + "/SmartDashboard/Auto Chooser": { "window": { "visible": true } }, - "/SmartDashboard/IntakeCommand": { + "/SmartDashboard/Field": { "window": { "visible": true } @@ -48,6 +54,13 @@ } }, "NetworkTables": { + "retained": { + "SmartDashboard": { + "Auto Chooser": { + "open": true + } + } + }, "transitory": { "SmartDashboard": { "Intakecommand": { @@ -71,5 +84,23 @@ "open": true }, "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332 + } + ], + "window": { + "visible": false + } + } } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index b78539f1..972f9d2f 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -8,6 +8,7 @@ import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand; @@ -38,6 +39,8 @@ public class RobotContainer { private final LEDSubsystem ledSubsystem = new LEDSubsystem(); + private final ArmSubsystem armSubsystem = new ArmSubsystem(); + private final SendableChooser autoChooser; public RobotContainer() { diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java index 7a1110f3..a9e83d25 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java @@ -41,4 +41,9 @@ public Trigger setZeroHeading() { return this.driver.rightBumper(); } + public Trigger presetHigh() { + // TODO Auto-generated method stub + return this.operator.y(); + } + } diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java index 2958f026..af998c58 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java @@ -6,6 +6,8 @@ public interface ControlBoard { // =============Operator Controls============= + public Trigger presetHigh(); + // =============Driver Controls============= public Trigger brake(); diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java index 28066631..41dbf3b9 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java @@ -7,12 +7,14 @@ public class ArmCommand extends Command { private ArmSubsystem armSubsystem; private double angle; - ProfiledPIDController ArmPID; + ProfiledPIDController armPID; public ArmCommand(double angle, ArmSubsystem armSubsystem) { this.armSubsystem = armSubsystem; this.angle = angle; - this.ArmPID = new ProfiledPIDController(.1, 0, 0, null); + // TODO: tune Values; + this.armPID = new ProfiledPIDController(.1, 0, 0, null); + addRequirements(armSubsystem); } @@ -20,22 +22,22 @@ public ArmCommand(double angle, ArmSubsystem armSubsystem) { public void initialize() { // TODO Auto-generated method stub super.initialize(); - ArmPID.reset(new State(angle, 0)); - ArmPID.setGoal(angle); - ArmPID.setTolerance(3, 3); + armPID.reset(new State(angle, 0)); + armPID.setGoal(angle); + armPID.setTolerance(3, 3); } @Override public void execute() { - double current = ArmPID.calculate(armSubsystem.getArmPitch()); + double current = armPID.calculate(armSubsystem.getArmPitch()); armSubsystem.setArmPivot(current); } public boolean isFinished() { - return ArmPID.atGoal(); + return armPID.atGoal(); } public void end() { diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index aebe9fb3..356a6cbf 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -1,7 +1,7 @@ package org.jmhsrobotics.frc2024.subsystems.arm; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -9,19 +9,21 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.SparkAbsoluteEncoder.Type; public class ArmSubsystem extends SubsystemBase { private MechanismLigament2d m_arm; private CANSparkMax armPivot = new CANSparkMax(9, MotorType.kBrushless); - private RelativeEncoder PitchEncoder = armPivot.getEncoder(); + private AbsoluteEncoder pitchEncoder; // private AbsoluteEncoder PitchEncoder=armPivot.getAbsoluteEncoder(kDutyCycle); // private AbsoluteEncoder PitchEncoder=armPivot.getEncoder(); public ArmSubsystem() { - + pitchEncoder = armPivot.getAbsoluteEncoder(Type.kDutyCycle); armPivot.setSmartCurrentLimit(40); armPivot.setIdleMode(com.revrobotics.CANSparkBase.IdleMode.kBrake); + init2d(); } @@ -30,14 +32,15 @@ public void setArmPivot(double amount) { } public double getArmPitch() { - return PitchEncoder.getPosition(); + return pitchEncoder.getPosition(); } public void init2d() { + // TODO: finish sim Mechanism2d mech = new Mechanism2d(3, 3); MechanismRoot2d root = mech.getRoot("base", 1.5, 0); - var m_arm = root.append(new MechanismLigament2d("arm", 2, 0)); - SmartDashboard.putData("Mech2d", mech); + m_arm = root.append(new MechanismLigament2d("arm", 2, 0)); + SmartDashboard.putData("ArmSubsystem/armSIM", mech); } @Override From d0db4cf8e333678fd77eaa040db1e5631ac54597 Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Thu, 25 Jan 2024 21:04:16 -0500 Subject: [PATCH 03/15] added basic setpoints, tried to get sim to work. TODO: fix sim --- simgui-ds.json | 6 ++ simgui.json | 19 ++++ .../jmhsrobotics/frc2024/RobotContainer.java | 8 ++ .../frc2024/controlBoard/CompControl.java | 92 ++++++++++--------- .../frc2024/controlBoard/ControlBoard.java | 19 ++-- .../subsystems/arm/ArmClosedCommand.java | 30 ++++++ .../frc2024/subsystems/arm/ArmCommand.java | 25 +++-- .../frc2024/subsystems/arm/ArmSubsystem.java | 35 ++++--- 8 files changed, 164 insertions(+), 70 deletions(-) create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmClosedCommand.java diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd3..b198faa6 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -93,6 +93,12 @@ { "guid": "78696e70757401000000000000000000", "useGamepad": true + }, + { + "useGamepad": true + }, + { + "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json index 69bdb440..325d4a79 100644 --- a/simgui.json +++ b/simgui.json @@ -21,6 +21,8 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/SmartDashboard/ArmCommand": "Command", + "/SmartDashboard/ArmPID": "ProfiledPIDController", "/SmartDashboard/ArmSubsystem/armSIM": "Mechanism2d", "/SmartDashboard/Auto Chooser": "String Chooser", "/SmartDashboard/Field": "Field2d", @@ -31,6 +33,11 @@ "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" }, "windows": { + "/SmartDashboard/ArmCommand": { + "window": { + "visible": true + } + }, "/SmartDashboard/ArmSubsystem/armSIM": { "window": { "visible": true @@ -63,6 +70,18 @@ }, "transitory": { "SmartDashboard": { + "ArmCommand": { + "data": { + "open": true + }, + "open": true + }, + "ArmSubsystem": { + "data": { + "open": true + }, + "open": true + }, "Intakecommand": { "open": true }, diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 972f9d2f..a50aacca 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -8,6 +8,8 @@ import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmClosedCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmCommand; import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; @@ -47,7 +49,10 @@ public RobotContainer() { this.driveSubsystem.setDefaultCommand(new DriveCommand(this.driveSubsystem, this.control)); this.ledSubsystem.setDefaultCommand(new RainbowLEDCommand(this.ledSubsystem)); + this.armSubsystem.setDefaultCommand(new ArmClosedCommand(armSubsystem, this.control)); + SmartDashboard.putData("Schedular", CommandScheduler.getInstance()); + SmartDashboard.putData("ArmCommand", new ArmCommand(0.6, this.armSubsystem)); configureBindings(); // Named commands must be added before building the chooser. @@ -75,6 +80,9 @@ private boolean getAllianceFlipState() { } private void configureBindings() { + this.control.presetHigh().onTrue(new ArmCommand(100, this.armSubsystem)); + this.control.presetLow().onTrue(new ArmCommand(0, this.armSubsystem)); + } public Command getAutonomousCommand() { diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java index a9e83d25..a3f0e62f 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java @@ -4,46 +4,56 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; public class CompControl implements ControlBoard { - private CommandXboxController driver = new CommandXboxController(0); - private CommandXboxController operator = new CommandXboxController(1); - - // TODO: Implement operator controls in the future - // =============Operator Controls============= - - // =============Driver Controls============= - @Override - public double xInput() { - // TODO Auto-generated method stub - return this.driver.getLeftX(); - } - - @Override - public double yInput() { - // TODO Auto-generated method stub - return this.driver.getLeftY(); - } - - @Override - public double rotationalInput() { - // TODO Auto-generated method stub - return this.driver.getRightX(); - } - - @Override - public Trigger brake() { - // TODO Auto-generated method stub - return this.driver.leftBumper(); - } - - @Override - public Trigger setZeroHeading() { - // TODO Auto-generated method stub - return this.driver.rightBumper(); - } - - public Trigger presetHigh() { - // TODO Auto-generated method stub - return this.operator.y(); - } + private CommandXboxController driver = new CommandXboxController(0); + private CommandXboxController operator = new CommandXboxController(1); + + // TODO: Implement operator controls in the future + // =============Operator Controls============= + + // =============Driver Controls============= + @Override + public double xInput() { + // TODO Auto-generated method stub + return this.driver.getLeftX(); + } + + @Override + public double yInput() { + // TODO Auto-generated method stub + return this.driver.getLeftY(); + } + + @Override + public double rotationalInput() { + // TODO Auto-generated method stub + return this.driver.getRightX(); + } + + public double pitchInput() { + // TODO Auto-generated method stub + return this.driver.getRightY(); + } + + @Override + public Trigger brake() { + // TODO Auto-generated method stub + return this.driver.leftBumper(); + } + + @Override + public Trigger setZeroHeading() { + // TODO Auto-generated method stub + return this.driver.rightBumper(); + } + + public Trigger presetHigh() { + // TODO Auto-generated method stub + return this.driver.y(); + } + + public Trigger presetLow() { + // TODO Auto-generated method stub + return this.driver.a(); + } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java index af998c58..6ee743cc 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java @@ -4,19 +4,22 @@ public interface ControlBoard { - // =============Operator Controls============= + // =============Operator Controls============= - public Trigger presetHigh(); + public Trigger presetHigh(); + public Trigger presetLow(); - // =============Driver Controls============= - public Trigger brake(); + // =============Driver Controls============= + public Trigger brake(); - public Trigger setZeroHeading(); + public Trigger setZeroHeading(); - public double xInput(); + public double xInput(); - public double yInput(); + public double yInput(); - public double rotationalInput(); + public double rotationalInput(); + + public double pitchInput(); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmClosedCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmClosedCommand.java new file mode 100644 index 00000000..6b5503dc --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmClosedCommand.java @@ -0,0 +1,30 @@ +package org.jmhsrobotics.frc2024.subsystems.arm; + +import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ArmClosedCommand extends Command { + + ArmSubsystem armSubsystem; + ControlBoard control; + + public ArmClosedCommand(ArmSubsystem armSubsystem, ControlBoard control) { + this.armSubsystem = armSubsystem; + this.control = control; + addRequirements(armSubsystem); + } + + @Override + public void initialize() { + // TODO Auto-generated method stub + this.armSubsystem.setArmPivot(0); + + } + @Override + public void execute() { + // TODO Auto-generated method stub + armSubsystem.setArmPivot(control.pitchInput()); + } + +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java index 41dbf3b9..93b8dfff 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java @@ -1,39 +1,44 @@ package org.jmhsrobotics.frc2024.subsystems.arm; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ArmCommand extends Command { private ArmSubsystem armSubsystem; private double angle; - ProfiledPIDController armPID; + private ProfiledPIDController armPID; public ArmCommand(double angle, ArmSubsystem armSubsystem) { this.armSubsystem = armSubsystem; this.angle = angle; // TODO: tune Values; - this.armPID = new ProfiledPIDController(.1, 0, 0, null); - + this.armPID = new ProfiledPIDController(1, 0, 0, new Constraints(5, 2.5)); + SmartDashboard.putData("ArmPID", this.armPID); addRequirements(armSubsystem); } @Override public void initialize() { // TODO Auto-generated method stub - super.initialize(); - armPID.reset(new State(angle, 0)); + armPID.reset(new State(0, 0)); armPID.setGoal(angle); - armPID.setTolerance(3, 3); + armPID.setTolerance(1, 3); } @Override public void execute() { - double current = armPID.calculate(armSubsystem.getArmPitch()); - - armSubsystem.setArmPivot(current); - + double PIDOut = armPID.calculate(armSubsystem.getArmPitch()); + PIDOut = MathUtil.clamp(PIDOut, -.9, .9); + armSubsystem.setArmPivot(PIDOut); + SmartDashboard.putNumber("ArmCommand/data/goal", this.angle); + SmartDashboard.putNumber("ArmCommand/data/setPoint", this.armPID.getSetpoint().position); + SmartDashboard.putNumber("ArmCommand/data/PIDOut", PIDOut); + SmartDashboard.putNumber("ArmCommand/data/Current", this.armSubsystem.getArmPitch()); } public boolean isFinished() { diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index 356a6cbf..9f975fea 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -1,46 +1,55 @@ package org.jmhsrobotics.frc2024.subsystems.arm; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAnalogSensor; +import com.revrobotics.SparkMaxAnalogSensor; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.CANSparkBase.IdleMode; public class ArmSubsystem extends SubsystemBase { private MechanismLigament2d m_arm; private CANSparkMax armPivot = new CANSparkMax(9, MotorType.kBrushless); private AbsoluteEncoder pitchEncoder; - // private AbsoluteEncoder PitchEncoder=armPivot.getAbsoluteEncoder(kDutyCycle); - // private AbsoluteEncoder PitchEncoder=armPivot.getEncoder(); + private Mechanism2d mech; + public ArmSubsystem() { pitchEncoder = armPivot.getAbsoluteEncoder(Type.kDutyCycle); armPivot.setSmartCurrentLimit(40); - armPivot.setIdleMode(com.revrobotics.CANSparkBase.IdleMode.kBrake); - init2d(); + armPivot.setIdleMode(IdleMode.kBrake); + init2d(); } public void setArmPivot(double amount) { armPivot.set(amount); + // SmartDashboard.putNumber("ArmSubsystem/data/ArmPivotSpeed", amount); } public double getArmPitch() { - return pitchEncoder.getPosition(); + return this.pitchEncoder.getPosition(); } + private SparkAnalogSensor pitchencsim; + public void init2d() { // TODO: finish sim - Mechanism2d mech = new Mechanism2d(3, 3); - MechanismRoot2d root = mech.getRoot("base", 1.5, 0); + mech = new Mechanism2d(3, 3); + MechanismRoot2d root = mech.getRoot("base", 1.5, 1.5); m_arm = root.append(new MechanismLigament2d("arm", 2, 0)); - SmartDashboard.putData("ArmSubsystem/armSIM", mech); + // pitchencsim = new SparkAnalogSensor(pitchEncoder); + } @Override @@ -48,6 +57,10 @@ public void periodic() { // TODO Auto-generated method stub super.periodic(); m_arm.setAngle(getArmPitch()); - } + SmartDashboard.putData("ArmSubsystem/armSIM", mech); + SmartDashboard.putNumber("ArmSubsystem/velocity", this.pitchEncoder.getVelocity()); + SmartDashboard.putString("ArmSubsystem/encoder", pitchEncoder.toString()); + + } } From d0e20de1729642c4f3ee921df788eedb81a5d3e3 Mon Sep 17 00:00:00 2001 From: m10653 Date: Fri, 26 Jan 2024 22:50:19 +0000 Subject: [PATCH 04/15] Fix git ignore. --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 42106003..6c573d38 100644 --- a/.gitignore +++ b/.gitignore @@ -173,3 +173,5 @@ out/ ctre_sim/ src/main/java/org/jmhsrobotics/frc2024/BuildConstants.java *.wpilog +simgui-ds.json +simgui.json From 121f664670621c0b876c6dd71b5a47ee2d1b0e4f Mon Sep 17 00:00:00 2001 From: m10653 Date: Fri, 26 Jan 2024 22:53:10 +0000 Subject: [PATCH 05/15] Fix compile Error broken import --- .../jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index 9f975fea..63211525 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -1,13 +1,10 @@ package org.jmhsrobotics.frc2024.subsystems.arm; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkMaxAnalogSensor; import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; @@ -22,7 +19,6 @@ public class ArmSubsystem extends SubsystemBase { private CANSparkMax armPivot = new CANSparkMax(9, MotorType.kBrushless); private AbsoluteEncoder pitchEncoder; private Mechanism2d mech; - public ArmSubsystem() { pitchEncoder = armPivot.getAbsoluteEncoder(Type.kDutyCycle); @@ -61,6 +57,6 @@ public void periodic() { SmartDashboard.putNumber("ArmSubsystem/velocity", this.pitchEncoder.getVelocity()); SmartDashboard.putString("ArmSubsystem/encoder", pitchEncoder.toString()); - } + } } From 548c0626895450461a49d096acdd4964c1de939d Mon Sep 17 00:00:00 2001 From: m10653 Date: Sat, 27 Jan 2024 00:54:45 +0000 Subject: [PATCH 06/15] Very hacky simulation support. --- .../frc2024/subsystems/arm/ArmSubsystem.java | 32 +++++-- .../arm/SimableAbsoluteEncoder.java | 86 +++++++++++++++++++ 2 files changed, 110 insertions(+), 8 deletions(-) create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index 63211525..7f0d3d67 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -1,11 +1,13 @@ package org.jmhsrobotics.frc2024.subsystems.arm; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkAbsoluteEncoder.Type; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -17,15 +19,17 @@ public class ArmSubsystem extends SubsystemBase { private MechanismLigament2d m_arm; private CANSparkMax armPivot = new CANSparkMax(9, MotorType.kBrushless); - private AbsoluteEncoder pitchEncoder; + private SimableAbsoluteEncoder pitchEncoder; private Mechanism2d mech; public ArmSubsystem() { - pitchEncoder = armPivot.getAbsoluteEncoder(Type.kDutyCycle); + pitchEncoder = new SimableAbsoluteEncoder(armPivot.getAbsoluteEncoder(Type.kDutyCycle)); armPivot.setSmartCurrentLimit(40); - armPivot.setIdleMode(IdleMode.kBrake); init2d(); + if (RobotBase.isSimulation()) { + initSim(); + } } public void setArmPivot(double amount) { @@ -37,8 +41,6 @@ public double getArmPitch() { return this.pitchEncoder.getPosition(); } - private SparkAnalogSensor pitchencsim; - public void init2d() { // TODO: finish sim mech = new Mechanism2d(3, 3); @@ -55,8 +57,22 @@ public void periodic() { m_arm.setAngle(getArmPitch()); SmartDashboard.putData("ArmSubsystem/armSIM", mech); SmartDashboard.putNumber("ArmSubsystem/velocity", this.pitchEncoder.getVelocity()); - SmartDashboard.putString("ArmSubsystem/encoder", pitchEncoder.toString()); + SmartDashboard.putNumber("ArmSubsystem/encoder", pitchEncoder.getPosition()); } + SingleJointedArmSim armSim; + public void initSim() { + double armGearRatio = 1; + double moi = 1; + armSim = new SingleJointedArmSim(DCMotor.getNEO(2), armGearRatio, moi, Units.inchesToMeters(23), 0, + Units.degreesToRadians(180), false, 0); + // simEncoder = new RevEncoderSimWrapper(null, null); + } + @Override + public void simulationPeriodic() { + double armVolts = armPivot.get() * 12; + armSim.setInputVoltage(armVolts); + pitchEncoder.setPosition(Units.radiansToDegrees(armSim.getAngleRads())); + } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java new file mode 100644 index 00000000..789a65bc --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java @@ -0,0 +1,86 @@ +package org.jmhsrobotics.frc2024.subsystems.arm; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.REVLibError; + +import edu.wpi.first.wpilibj.RobotBase; + +public class SimableAbsoluteEncoder implements AbsoluteEncoder { + + private AbsoluteEncoder enc; + private double position; + + SimableAbsoluteEncoder(AbsoluteEncoder enc) { + this.enc = enc; + } + + @Override + public double getPosition() { + if (RobotBase.isSimulation()) { + return position; + } else { + return enc.getPosition(); + } + + } + + @Override + public double getVelocity() { + return enc.getVelocity(); + } + + @Override + public REVLibError setPositionConversionFactor(double factor) { + return enc.setPositionConversionFactor(factor); + } + + @Override + public double getPositionConversionFactor() { + return enc.getPositionConversionFactor(); + } + + @Override + public REVLibError setVelocityConversionFactor(double factor) { + return enc.setVelocityConversionFactor(factor); + } + + @Override + public double getVelocityConversionFactor() { + return enc.getVelocityConversionFactor(); + } + + @Override + public REVLibError setInverted(boolean inverted) { + return enc.setInverted(inverted); + } + + @Override + public boolean getInverted() { + return enc.getInverted(); + } + + @Override + public REVLibError setAverageDepth(int depth) { + return enc.setAverageDepth(depth); + } + + @Override + public int getAverageDepth() { + return enc.getAverageDepth(); + } + + @Override + public REVLibError setZeroOffset(double offset) { + return enc.setZeroOffset(offset); + } + + @Override + public double getZeroOffset() { + return enc.getZeroOffset(); + } + + public void setPosition(double position) { + this.position = position; + } + +} From 70f9604504938d55b5e9a624c37121a2128f0431 Mon Sep 17 00:00:00 2001 From: m10653 Date: Sat, 27 Jan 2024 15:27:25 +0000 Subject: [PATCH 07/15] Fix arm simulation --- .../frc2024/subsystems/arm/ArmSubsystem.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index 7f0d3d67..cd98849e 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -62,16 +62,17 @@ public void periodic() { } SingleJointedArmSim armSim; public void initSim() { - double armGearRatio = 1; - double moi = 1; - armSim = new SingleJointedArmSim(DCMotor.getNEO(2), armGearRatio, moi, Units.inchesToMeters(23), 0, - Units.degreesToRadians(180), false, 0); + double armGearRatio = 183.33; + double moi = 0.399649199419221; + armSim = new SingleJointedArmSim(DCMotor.getNEO(1), armGearRatio, moi, Units.inchesToMeters(23), 0, + Units.degreesToRadians(180), true, 0); // simEncoder = new RevEncoderSimWrapper(null, null); } @Override public void simulationPeriodic() { double armVolts = armPivot.get() * 12; armSim.setInputVoltage(armVolts); + armSim.update(0.2); pitchEncoder.setPosition(Units.radiansToDegrees(armSim.getAngleRads())); } From 84734f8ced972aa86ef8ad6ffbc240fd24e1c7ce Mon Sep 17 00:00:00 2001 From: m10653 Date: Mon, 29 Jan 2024 13:13:22 +0000 Subject: [PATCH 08/15] Add pose3d for advantagescope --- .../jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index cd98849e..7401ecdb 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -1,9 +1,14 @@ package org.jmhsrobotics.frc2024.subsystems.arm; +import org.jmhsrobotics.warcore.nt.NT4Util; + +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder.Type; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; @@ -13,7 +18,6 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.CANSparkBase.IdleMode; public class ArmSubsystem extends SubsystemBase { @@ -58,6 +62,8 @@ public void periodic() { SmartDashboard.putData("ArmSubsystem/armSIM", mech); SmartDashboard.putNumber("ArmSubsystem/velocity", this.pitchEncoder.getVelocity()); SmartDashboard.putNumber("ArmSubsystem/encoder", pitchEncoder.getPosition()); + NT4Util.putPose3d("ArmSubsystem/armpose3d", + new Pose3d(0.2, 0, 0.28, new Rotation3d(0, Units.degreesToRadians(getArmPitch()), 0))); } SingleJointedArmSim armSim; From b54839fca04e1459c41a51a7a6062447bf445cb1 Mon Sep 17 00:00:00 2001 From: m10653 Date: Tue, 30 Jan 2024 17:12:09 +0000 Subject: [PATCH 09/15] Renamed Closed loop command to open loop. Added bad static example for armcommand --- .../jmhsrobotics/frc2024/RobotContainer.java | 6 ++--- .../frc2024/subsystems/arm/ArmCommand.java | 23 +++++++++++++------ ...nd.java => ArmOpenLoopControlCommand.java} | 4 ++-- 3 files changed, 21 insertions(+), 12 deletions(-) rename src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/{ArmClosedCommand.java => ArmOpenLoopControlCommand.java} (79%) diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 6753a4b8..bcc73e67 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -8,7 +8,7 @@ import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; -import org.jmhsrobotics.frc2024.subsystems.arm.ArmClosedCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmOpenLoopControlCommand; import org.jmhsrobotics.frc2024.subsystems.arm.ArmCommand; import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; @@ -52,9 +52,9 @@ public RobotContainer() { this.driveSubsystem.setDefaultCommand(new DriveCommand(this.driveSubsystem, this.control)); this.ledSubsystem.setDefaultCommand(new RainbowLEDCommand(this.ledSubsystem)); - this.armSubsystem.setDefaultCommand(new ArmClosedCommand(armSubsystem, this.control)); + this.armSubsystem.setDefaultCommand(new ArmOpenLoopControlCommand(armSubsystem, this.control)); - SmartDashboard.putData("ArmCommand", new ArmCommand(0.6, this.armSubsystem)); + SmartDashboard.putData("ArmCommand", new ArmCommand(90, this.armSubsystem)); SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); SmartDashboard.putData("LockAprilTagCommand", new LockAprilTag(4, this.driveSubsystem, this.visionSubsystem)); configureBindings(); diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java index 93b8dfff..62fe9211 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java @@ -1,30 +1,36 @@ package org.jmhsrobotics.frc2024.subsystems.arm; import edu.wpi.first.wpilibj2.command.Command; + +import org.jmhsrobotics.warcore.math.TuneableProfiledPIDController; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class ArmCommand extends Command { private ArmSubsystem armSubsystem; private double angle; - private ProfiledPIDController armPID; - + private static ProfiledPIDController armPID; + private static int num = 0; public ArmCommand(double angle, ArmSubsystem armSubsystem) { this.armSubsystem = armSubsystem; this.angle = angle; // TODO: tune Values; - this.armPID = new ProfiledPIDController(1, 0, 0, new Constraints(5, 2.5)); - SmartDashboard.putData("ArmPID", this.armPID); + armPID = new TuneableProfiledPIDController(0.01, 0, 0, new Constraints(180, 180)); + SmartDashboard.putData("ArmPID2", armPID); addRequirements(armSubsystem); } + @Override public void initialize() { // TODO Auto-generated method stub - armPID.reset(new State(0, 0)); + + // armPID.reset(new State(0, 0)); armPID.setGoal(angle); armPID.setTolerance(1, 3); } @@ -41,11 +47,14 @@ public void execute() { SmartDashboard.putNumber("ArmCommand/data/Current", this.armSubsystem.getArmPitch()); } + @Override public boolean isFinished() { - return armPID.atGoal(); + return false; + // return armPID.atGoal(); } - public void end() { + @Override + public void end(boolean interrupted) { armSubsystem.setArmPivot(0); } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmClosedCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmOpenLoopControlCommand.java similarity index 79% rename from src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmClosedCommand.java rename to src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmOpenLoopControlCommand.java index 6b5503dc..ffb22437 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmClosedCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmOpenLoopControlCommand.java @@ -4,12 +4,12 @@ import edu.wpi.first.wpilibj2.command.Command; -public class ArmClosedCommand extends Command { +public class ArmOpenLoopControlCommand extends Command { ArmSubsystem armSubsystem; ControlBoard control; - public ArmClosedCommand(ArmSubsystem armSubsystem, ControlBoard control) { + public ArmOpenLoopControlCommand(ArmSubsystem armSubsystem, ControlBoard control) { this.armSubsystem = armSubsystem; this.control = control; addRequirements(armSubsystem); From 4dec44c5f87ef62d480aa0c39a86f84a3b7237ab Mon Sep 17 00:00:00 2001 From: m10653 Date: Tue, 30 Jan 2024 17:39:55 +0000 Subject: [PATCH 10/15] Cleanup --- .../org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java index 62fe9211..dafd1f6a 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ArmCommand extends Command { @@ -29,7 +28,7 @@ public ArmCommand(double angle, ArmSubsystem armSubsystem) { @Override public void initialize() { // TODO Auto-generated method stub - + // armPID.reset(new State(0, 0)); armPID.setGoal(angle); armPID.setTolerance(1, 3); From 75e0cf823bfc031fe86ba02f082f2dfc735c4052 Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Tue, 30 Jan 2024 19:30:40 -0500 Subject: [PATCH 11/15] updated commands' location, fixed formatting --- .../jmhsrobotics/frc2024/RobotContainer.java | 7 ++--- .../arm/ArmOpenLoopControlCommand.java | 30 ------------------- .../arm/{ => commands}/ArmCommand.java | 19 ++++++------ 3 files changed, 13 insertions(+), 43 deletions(-) delete mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmOpenLoopControlCommand.java rename src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/{ => commands}/ArmCommand.java (70%) diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index bcc73e67..9ef87241 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -8,9 +8,9 @@ import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; -import org.jmhsrobotics.frc2024.subsystems.arm.ArmOpenLoopControlCommand; -import org.jmhsrobotics.frc2024.subsystems.arm.ArmCommand; import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; +import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmOpenLoopControlCommand; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.auto.DriveTimeCommand; @@ -52,11 +52,10 @@ public RobotContainer() { this.driveSubsystem.setDefaultCommand(new DriveCommand(this.driveSubsystem, this.control)); this.ledSubsystem.setDefaultCommand(new RainbowLEDCommand(this.ledSubsystem)); - this.armSubsystem.setDefaultCommand(new ArmOpenLoopControlCommand(armSubsystem, this.control)); - SmartDashboard.putData("ArmCommand", new ArmCommand(90, this.armSubsystem)); SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); SmartDashboard.putData("LockAprilTagCommand", new LockAprilTag(4, this.driveSubsystem, this.visionSubsystem)); + SmartDashboard.putData("ArmCommand", new ArmCommand(90, this.armSubsystem)); configureBindings(); // Named commands must be added before building the chooser. diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmOpenLoopControlCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmOpenLoopControlCommand.java deleted file mode 100644 index ffb22437..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmOpenLoopControlCommand.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.arm; - -import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ArmOpenLoopControlCommand extends Command { - - ArmSubsystem armSubsystem; - ControlBoard control; - - public ArmOpenLoopControlCommand(ArmSubsystem armSubsystem, ControlBoard control) { - this.armSubsystem = armSubsystem; - this.control = control; - addRequirements(armSubsystem); - } - - @Override - public void initialize() { - // TODO Auto-generated method stub - this.armSubsystem.setArmPivot(0); - - } - @Override - public void execute() { - // TODO Auto-generated method stub - armSubsystem.setArmPivot(control.pitchInput()); - } - -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmCommand.java similarity index 70% rename from src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java rename to src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmCommand.java index dafd1f6a..65f52a3c 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmCommand.java @@ -1,12 +1,13 @@ -package org.jmhsrobotics.frc2024.subsystems.arm; +package org.jmhsrobotics.frc2024.subsystems.arm.commands; import edu.wpi.first.wpilibj2.command.Command; -import org.jmhsrobotics.warcore.math.TuneableProfiledPIDController; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ArmCommand extends Command { @@ -19,9 +20,9 @@ public ArmCommand(double angle, ArmSubsystem armSubsystem) { this.armSubsystem = armSubsystem; this.angle = angle; // TODO: tune Values; - armPID = new TuneableProfiledPIDController(0.01, 0, 0, new Constraints(180, 180)); - SmartDashboard.putData("ArmPID2", armPID); - addRequirements(armSubsystem); + armPID = new ProfiledPIDController(0.01, 0, 0, new Constraints(180, 180)); + SmartDashboard.putData("ArmPID2", this.armPID); + addRequirements(this.armSubsystem); } @@ -29,16 +30,16 @@ public ArmCommand(double angle, ArmSubsystem armSubsystem) { public void initialize() { // TODO Auto-generated method stub - // armPID.reset(new State(0, 0)); - armPID.setGoal(angle); + armPID.reset(new State(0, 0)); + armPID.setGoal(this.angle); armPID.setTolerance(1, 3); } @Override public void execute() { - double PIDOut = armPID.calculate(armSubsystem.getArmPitch()); - PIDOut = MathUtil.clamp(PIDOut, -.9, .9); + double PIDOut = this.armPID.calculate(this.armSubsystem.getArmPitch()); + PIDOut = MathUtil.clamp(PIDOut, -.5, .5); armSubsystem.setArmPivot(PIDOut); SmartDashboard.putNumber("ArmCommand/data/goal", this.angle); SmartDashboard.putNumber("ArmCommand/data/setPoint", this.armPID.getSetpoint().position); From 75f21feda08ce243264992c253ff08547351de50 Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Tue, 30 Jan 2024 19:31:22 -0500 Subject: [PATCH 12/15] removed unused import in robot container --- src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 9ef87241..267f103b 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -10,7 +10,6 @@ import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmCommand; -import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmOpenLoopControlCommand; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.auto.DriveTimeCommand; From fd389784a4fcff1a91a6d02ba032fc1c7e0cc684 Mon Sep 17 00:00:00 2001 From: m10653 Date: Wed, 31 Jan 2024 13:49:12 +0000 Subject: [PATCH 13/15] Clamp Sim voltage --- .../org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index 7401ecdb..e01c8935 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -7,6 +7,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder.Type; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.system.plant.DCMotor; @@ -76,7 +77,7 @@ public void initSim() { } @Override public void simulationPeriodic() { - double armVolts = armPivot.get() * 12; + double armVolts = MathUtil.clamp(armPivot.get() * 12, -12, 12); armSim.setInputVoltage(armVolts); armSim.update(0.2); pitchEncoder.setPosition(Units.radiansToDegrees(armSim.getAngleRads())); From 3acaf70ad41757dc03b6c4791eafac38fa0f0852 Mon Sep 17 00:00:00 2001 From: m10653 Date: Thu, 1 Feb 2024 04:28:13 +0000 Subject: [PATCH 14/15] Simulation Fixes. --- .../jmhsrobotics/frc2024/RobotContainer.java | 3 +- .../frc2024/subsystems/arm/ArmSubsystem.java | 6 ++-- .../commands/ArmOpenLoopControlCommand.java | 29 +++++++++++++++++++ 3 files changed, 34 insertions(+), 4 deletions(-) create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmOpenLoopControlCommand.java diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 267f103b..fc788a40 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -10,6 +10,7 @@ import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmOpenLoopControlCommand; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.auto.DriveTimeCommand; @@ -56,7 +57,7 @@ public RobotContainer() { SmartDashboard.putData("LockAprilTagCommand", new LockAprilTag(4, this.driveSubsystem, this.visionSubsystem)); SmartDashboard.putData("ArmCommand", new ArmCommand(90, this.armSubsystem)); configureBindings(); - + armSubsystem.setDefaultCommand(new ArmOpenLoopControlCommand(armSubsystem, control)); // Named commands must be added before building the chooser. configurePathPlanner(); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index e01c8935..e21548c9 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -64,14 +64,14 @@ public void periodic() { SmartDashboard.putNumber("ArmSubsystem/velocity", this.pitchEncoder.getVelocity()); SmartDashboard.putNumber("ArmSubsystem/encoder", pitchEncoder.getPosition()); NT4Util.putPose3d("ArmSubsystem/armpose3d", - new Pose3d(0.2, 0, 0.28, new Rotation3d(0, Units.degreesToRadians(getArmPitch()), 0))); + new Pose3d(0.2, 0, 0.283, new Rotation3d(0, Units.degreesToRadians(getArmPitch()), 0))); } SingleJointedArmSim armSim; public void initSim() { double armGearRatio = 183.33; double moi = 0.399649199419221; - armSim = new SingleJointedArmSim(DCMotor.getNEO(1), armGearRatio, moi, Units.inchesToMeters(23), 0, + armSim = new SingleJointedArmSim(DCMotor.getNEO(2), armGearRatio, moi, Units.inchesToMeters(23), 0, Units.degreesToRadians(180), true, 0); // simEncoder = new RevEncoderSimWrapper(null, null); } @@ -79,7 +79,7 @@ public void initSim() { public void simulationPeriodic() { double armVolts = MathUtil.clamp(armPivot.get() * 12, -12, 12); armSim.setInputVoltage(armVolts); - armSim.update(0.2); + armSim.update(0.02); pitchEncoder.setPosition(Units.radiansToDegrees(armSim.getAngleRads())); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmOpenLoopControlCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmOpenLoopControlCommand.java new file mode 100644 index 00000000..f924303b --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmOpenLoopControlCommand.java @@ -0,0 +1,29 @@ +package org.jmhsrobotics.frc2024.subsystems.arm.commands; + +import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ArmOpenLoopControlCommand extends Command { + + ArmSubsystem armSubsystem; + ControlBoard control; + + public ArmOpenLoopControlCommand(ArmSubsystem armSubsystem, ControlBoard control) { + this.armSubsystem = armSubsystem; + this.control = control; + addRequirements(armSubsystem); + } + + @Override + public void initialize() { + this.armSubsystem.setArmPivot(0); + + } + @Override + public void execute() { + armSubsystem.setArmPivot(control.pitchInput()); + } + +} From 2d3af2cc3e994b17f6c45607b5530ee8cabb8ffd Mon Sep 17 00:00:00 2001 From: m10653 Date: Thu, 1 Feb 2024 17:37:42 +0000 Subject: [PATCH 15/15] Update sim to use constant --- .../org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index e21548c9..31596ac1 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -1,5 +1,6 @@ package org.jmhsrobotics.frc2024.subsystems.arm; +import org.jmhsrobotics.frc2024.Constants; import org.jmhsrobotics.warcore.nt.NT4Util; import com.revrobotics.CANSparkBase.IdleMode; @@ -79,7 +80,7 @@ public void initSim() { public void simulationPeriodic() { double armVolts = MathUtil.clamp(armPivot.get() * 12, -12, 12); armSim.setInputVoltage(armVolts); - armSim.update(0.02); + armSim.update(Constants.ksimDtSec); pitchEncoder.setPosition(Units.radiansToDegrees(armSim.getAngleRads())); }