From 018bde231b8db57e5ee34b1ac5fcf4abbd9dfcf5 Mon Sep 17 00:00:00 2001 From: Aarav Date: Mon, 6 Mar 2023 18:36:32 -0700 Subject: [PATCH 01/20] I, Aarav Shah, endorse war crimes in various 3rd world countries --- .../java/frc4388/robot/RobotContainer.java | 14 ++++- src/main/java/frc4388/robot/RobotMap.java | 5 ++ .../frc4388/robot/commands/ArmCommand.java | 61 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Claw.java | 3 +- 4 files changed, 81 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ArmCommand.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1444e27..e8bb7de 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -8,6 +8,7 @@ package frc4388.robot; import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -17,6 +18,7 @@ import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.Arm; +import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.commands.PlaybackChooser; @@ -43,6 +45,8 @@ public class RobotContainer { public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); + public final Claw m_robotClaw = new Claw(m_robotMap.servo); + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -135,14 +139,22 @@ public class RobotContainer { // * Operator Buttons + // TODO: use the claw subsystem new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> { servo.setRaw(servo_open ? 1000 : 2000); servo_open = !servo_open; })); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotClaw.setClaw(servo_open))); + servo_open = !servo_open; new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit())); + .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive /* claw */)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 362b401..60ce4f5 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -12,6 +12,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; +import edu.wpi.first.wpilibj.PWM; import frc4388.robot.Constants.ArmConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -160,4 +161,8 @@ public class RobotMap { tele.configForwardSoftLimitEnable(false); tele.configReverseSoftLimitEnable(false); } + + // claw stuff (i will punch someone) + PWM servo = new PWM(0); + } diff --git a/src/main/java/frc4388/robot/commands/ArmCommand.java b/src/main/java/frc4388/robot/commands/ArmCommand.java new file mode 100644 index 0000000..6afe6b4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ArmCommand.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Arm; +import frc4388.robot.subsystems.Claw; + +public class ArmCommand extends PelvicInflammatoryDisease { + private Arm arm; + private Claw claw; + private boolean toggle; + + /** Creates a new ArmCommand. */ + public ArmCommand(Arm arm, Claw claw, boolean open) { + // Use addRequirements() here to declare subsystem dependencies. + super(0.6, 0, 0, 0); + this.arm = arm; + this.claw = claw; + this.toggle = open; + addRequirements(arm, claw); + } + + public ArmCommand(Arm arm, Claw claw) { + this(arm, claw, claw.isClawOpen()); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + super.initialize(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } + + @Override + public double getError() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public void runWithOutput(double output) { + // TODO Auto-generated method stub + + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index 37793b1..6f1199a 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -1,7 +1,8 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Claw { +public class Claw extends SubsystemBase { private PWM m_clawMotor; private boolean m_open = false; From 218eb0e7282f55b41c9fb34d524e5a31ebe7cc4a Mon Sep 17 00:00:00 2001 From: Aarav Date: Mon, 6 Mar 2023 19:40:50 -0700 Subject: [PATCH 02/20] including ukraine --- src/main/java/frc4388/robot/RobotContainer.java | 3 +-- src/main/java/frc4388/robot/subsystems/Claw.java | 4 ++++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e8bb7de..933fbf5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -147,8 +147,7 @@ public class RobotContainer { })); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotClaw.setClaw(servo_open))); - servo_open = !servo_open; + .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index 6f1199a..c2e6b87 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -18,6 +18,10 @@ public class Claw extends SubsystemBase { m_clawMotor.setRaw(open ? 0 : 2000); } + public void toggle() { + setClaw(!m_open); + } + public boolean isClawOpen() { return m_open; } From 0935c35b262b7d64535b6350cbbf478e8e0fa178 Mon Sep 17 00:00:00 2001 From: Aarav Date: Mon, 6 Mar 2023 19:45:38 -0700 Subject: [PATCH 03/20] (I am not sponsored by the russian government) --- src/main/java/frc4388/robot/RobotContainer.java | 10 +++++----- src/main/java/frc4388/robot/RobotMap.java | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 933fbf5..a27a46f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -140,11 +140,11 @@ public class RobotContainer { // * Operator Buttons // TODO: use the claw subsystem - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> { - servo.setRaw(servo_open ? 1000 : 2000); - servo_open = !servo_open; - })); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> { + // servo.setRaw(servo_open ? 1000 : 2000); + // servo_open = !servo_open; + // })); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 60ce4f5..4bf5968 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -162,7 +162,7 @@ public class RobotMap { tele.configReverseSoftLimitEnable(false); } - // claw stuff (i will punch someone) + // claw stuff (WHAT IS A SOAP ENGINEER) PWM servo = new PWM(0); } From 814fa4bcc29489a3e4d1af1a4f8ebd91147943b1 Mon Sep 17 00:00:00 2001 From: Aarav Date: Mon, 6 Mar 2023 19:54:55 -0700 Subject: [PATCH 04/20] (I am a soap engineer) --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a27a46f..1c5f7e4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -153,7 +153,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive /* claw */)); + .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw)); } /** From e98c3a09e1b49b8448dd482d879428515ebcd799 Mon Sep 17 00:00:00 2001 From: Aarav Date: Sat, 11 Mar 2023 14:03:36 -0700 Subject: [PATCH 05/20] commit --- src/main/java/frc4388/robot/Constants.java | 6 +- src/main/java/frc4388/robot/Robot.java | 23 ++++++ .../java/frc4388/robot/RobotContainer.java | 18 ++--- src/main/java/frc4388/robot/RobotMap.java | 5 ++ .../java/frc4388/robot/subsystems/Arm.java | 70 +++++++++++++++++-- .../java/frc4388/robot/subsystems/Claw.java | 27 ++++++- .../frc4388/robot/subsystems/SwerveDrive.java | 24 +++++-- 7 files changed, 143 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a16900a..83ac014 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,8 +24,8 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = -0.7; - public static final double MIN_ROT_SPEED = -0.3; + public static final double MAX_ROT_SPEED = 1.5; + public static final double MIN_ROT_SPEED = 0.8; public static double ROTATION_SPEED = MAX_ROT_SPEED; public static final class IDs { @@ -65,7 +65,7 @@ public final class Constants { public static final class Conversions { public static final int CANCODER_TICKS_PER_ROTATION = 4096; - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.8; + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ac78650..91a0851 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,10 +10,16 @@ package frc4388.robot; import java.lang.System; import java.lang.reflect.Array; import java.util.Arrays; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.can.MotControllerJNI; + import java.io.File; import java.io.IOException; import java.io.PrintWriter; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -84,13 +90,27 @@ public class Robot extends TimedRobot { * You can use it to reset any subsystem information you want to clear when * the robot is disabled. */ + boolean dis = false; @Override public void disabledInit() { + // m_robotContainer.m_robotClaw.setClaw(false); m_robotTime.endMatchTime(); + m_robotContainer.m_robotClaw.disable(); + + m_handle = m_robotContainer.m_robotMap.leftBackWheel.getHandle(); } + long m_handle = 0; @Override public void disabledPeriodic() { + // if (dis) { + + // MotControllerJNI.Set_4(m_handle, ControlMode.PercentOutput.value, 1, 1, DemandType.Neutral.value); + // // m_robotContainer.m_robotMap.leftBackSteer.set(ControlMode.PercentOutput, 1, DemandType.Neutral, 0); + // // m_robotContainer.m_robotSwerveDrive.driveWithInput(new Translation2d(.5, 0), new Translation2d(), true); + // } + + // System.out.println("hi from disabled"); } /** @@ -118,6 +138,9 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.m_robotSwerveDrive.resetGyro(); + dis = true; + + m_robotContainer.m_robotClaw.enable(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1c5f7e4..920f6cf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -67,8 +67,6 @@ public class RobotContainer { // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private PlaybackChooser playbackChooser; - private PWM servo = new PWM(0); - private boolean servo_open = true; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -117,8 +115,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive)); // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) @@ -138,16 +136,12 @@ public class RobotContainer { .onFalse(new InstantCommand()); // * Operator Buttons - - // TODO: use the claw subsystem - // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> { - // servo.setRaw(servo_open ? 1000 : 2000); - // servo_open = !servo_open; - // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> System.out.println("Claw Button"))); .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4bf5968..3dd4ea2 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -116,6 +116,11 @@ public class RobotMap { rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // set neutral mode + leftFrontWheel.setNeutralMode(NeutralMode.Brake); + rightFrontWheel.setNeutralMode(NeutralMode.Brake); + leftBackWheel.setNeutralMode(NeutralMode.Brake); + rightBackWheel.setNeutralMode(NeutralMode.Brake); + leftFrontSteer.setNeutralMode(NeutralMode.Brake); rightFrontSteer.setNeutralMode(NeutralMode.Brake); leftBackSteer.setNeutralMode(NeutralMode.Brake); diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 2deaf79..fe8975e 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -10,6 +10,8 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration; import frc4388.robot.Constants.ArmConstants; import frc4388.robot.Constants.SwerveDriveConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { @@ -42,6 +44,32 @@ public class Arm extends SubsystemBase { CANCoderConfiguration config = new CANCoderConfiguration(); config.magnetOffsetDegrees = ArmConstants.OFFSET; m_pivotEncoder.configAllSettings(config); + // m_pivot.configFactoryDefault(); + // m_pivot.clearStickyFaults(); + // m_pi + + SmartDashboard.putNumber("kP Pivot", 0); + SmartDashboard.putNumber("kI Pivot", 0); + SmartDashboard.putNumber("kD Pivot", 0); + + SmartDashboard.putData("Set PID", new InstantCommand(() -> { + TalonFXConfiguration pivotConfig_ = new TalonFXConfiguration(); + pivotConfig_.slot0.kP = SmartDashboard.getNumber("kP Pivot", 0); + pivotConfig_.slot0.kI = SmartDashboard.getNumber("kI Pivot", 0); + pivotConfig_.slot0.kD = SmartDashboard.getNumber("kD Pivot", 0); + + // pivotConfig_.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + // pivotConfig_.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + // pivotConfig_.configSelectedFeedbackFilter();// FeedbackDevice.RemoteSensor0; + m_pivot.configAllSettings(pivotConfig_); + m_pivot.configRemoteFeedbackFilter(encoder.getDeviceID(), RemoteSensorSource.CANCoder, 0); + m_pivot.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + // m_pivot.selectProfileSlot(0, 0); + })); + + SmartDashboard.putData("set pos 1", new RunCommand(() -> m_pivot.set(ControlMode.Position, 225), this)); + SmartDashboard.putData("set pos 2", new RunCommand(() -> m_pivot.set(ControlMode.Position, 135), this)); + SmartDashboard.putData("Kill Self", new InstantCommand(() -> {}, this)); } public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) { @@ -49,11 +77,11 @@ public class Arm extends SubsystemBase { } public void setRotVel(double vel) { - m_pivot.set(ControlMode.PercentOutput, vel / 5); + m_pivot.set(ControlMode.PercentOutput, .4 * vel); } public void setTeleVel(double vel) { - m_tele.set(ControlMode.PercentOutput, -0.25 * vel); + m_tele.set(ControlMode.PercentOutput, -0.5 * vel); } public void armSetRotation(double rot) { @@ -129,7 +157,8 @@ public class Arm extends SubsystemBase { tele_softLimit = !tele_softLimit; } - boolean resetable = true; + boolean resetable = true; + boolean tele_reset = true; @Override public void periodic() { @@ -141,8 +170,8 @@ public class Arm extends SubsystemBase { SmartDashboard.putNumber("start pivot", pivot_soft); SmartDashboard.putNumber("start tele", tele_soft); - m_pivot.configForwardSoftLimitEnable(true); - m_pivot.configReverseSoftLimitEnable(true); + m_pivot.configForwardSoftLimitEnable(soft_limits); + m_pivot.configReverseSoftLimitEnable(soft_limits); SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value); SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value); resetable = false; @@ -150,12 +179,41 @@ public class Arm extends SubsystemBase { resetable = true; } + if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) { + var tele_soft = m_tele.getSelectedSensorPosition(); + m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); + m_tele.configReverseSoftLimitThreshold(1000 - tele_soft); + m_tele.configForwardSoftLimitEnable(true); + m_tele.configReverseSoftLimitEnable(true); + tele_reset = false; + } else if (m_tele.isFwdLimitSwitchClosed() == 0) { + tele_reset = true; + } + + SmartDashboard.putBoolean("reverse", m_tele.isFwdLimitSwitchClosed() == 1); + double x = Math.cos(degrees); - SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED); + // SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED); // if (m_debug) // SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition()); SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition()); SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition()); SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition()); } + + boolean soft_limits = true; + public void killSoftLimits() { + var pivot_soft = m_pivot.getSelectedSensorPosition(); + var tele_soft = m_tele.getSelectedSensorPosition(); + + SmartDashboard.putNumber("start pivot", pivot_soft); + SmartDashboard.putNumber("start tele", tele_soft); + + m_pivot.configForwardSoftLimitEnable(!soft_limits); + m_pivot.configReverseSoftLimitEnable(!soft_limits); + SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value); + SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value); + + soft_limits = !soft_limits; + } } diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index c2e6b87..8daa6b5 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -1,10 +1,12 @@ package frc4388.robot.subsystems; +import edu.wpi.first.hal.PWMJNI; import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Claw extends SubsystemBase { private PWM m_clawMotor; - private boolean m_open = false; + private boolean m_open = false; + private boolean m_disabled = false; // Opens claw public Claw(PWM m_clawMotor) { @@ -13,16 +15,37 @@ public class Claw extends SubsystemBase { } public void setClaw(boolean open) { + if (m_disabled) return; // Open claw + // m_clawMotor.setRaw(150); m_open = open; - m_clawMotor.setRaw(open ? 0 : 2000); + System.out.println("setClaw()"); + // m_clawMotor.setPosition(0.5); + // m_clawMotor.setRaw(0); + // m_clawMotor.setRaw(m_open ? 0 : 255); + // m_clawMotor.setSpeed(m_open ? -1 : 1); + PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1); + // PWMJNI.setPWMDisabled(0); + System.out.println("Claw Pos: " + m_clawMotor.getRaw()); } public void toggle() { + System.out.println("toggle()"); setClaw(!m_open); } public boolean isClawOpen() { return m_open; } + + public void disable() { + m_disabled = true; + // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle())); + PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5); + // PWMJNI.setPWMDisabled(m_clawMotor.getHandle()); + } + + public void enable() { + m_disabled = false; + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index aa29cdf..fc216a6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -82,19 +82,21 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { + + double rot = 0; if (rightStick.getNorm() > 0.1) { rotTarget = gyro.getRotation2d(); + rot = rightStick.getX(); + } else { + rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); } - double rot = rightStick.getX(); - - // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. + // Use the left joystick to set speed. Apply a cubic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); - + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); @@ -235,8 +237,16 @@ public class SwerveDrive extends SubsystemBase { * Shifts gear from high to low, or vice versa. * @param shift true to shift to high, false to shift to low */ - public void highSpeed(boolean shift) { - this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + // public void highSpeed(boolean shift) { + // this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + // } + + public void toggleGear() { + if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + } else { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + } } } From e3234fb51d2ab2487ef687674175c5623b16edec Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 13 Mar 2023 16:00:09 -0600 Subject: [PATCH 06/20] cummit --- .../java/frc4388/robot/RobotContainer.java | 27 +++++++++++----- .../java/frc4388/robot/subsystems/Arm.java | 31 ++++++++++--------- 2 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 920f6cf..c8cffac 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,8 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; + import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -136,18 +138,29 @@ public class RobotContainer { .onFalse(new InstantCommand()); // * Operator Buttons - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> System.out.println("Claw Button"))); - .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + // // .onTrue(new InstantCommand(() -> System.out.println("Claw Button"))); + // .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); + + + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 200), m_robotArm)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 135), m_robotArm)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 225), m_robotArm)); + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw)); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index fe8975e..a7e76c3 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; @@ -16,7 +17,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { private WPI_TalonFX m_tele; - private WPI_TalonFX m_pivot; + public WPI_TalonFX m_pivot; private CANCoder m_pivotEncoder; private boolean m_debug; @@ -29,15 +30,16 @@ public class Arm extends SubsystemBase { tele.configFactoryDefault(); m_pivot.configFactoryDefault(); - TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); - pivotConfig.slot0.kP = ArmConstants.kP; - pivotConfig.slot0.kI = ArmConstants.kI; - pivotConfig.slot0.kD = ArmConstants.kD; + // TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); + // pivotConfig.slot0.kP = 0.5;//ArmConstants.kP; + // pivotConfig.slot0.kI = 0.0;//ArmConstants.kI; + // pivotConfig.slot0.kD = 0.0;//ArmConstants.kD; - pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - m_pivot.configAllSettings(pivotConfig); + // pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + // pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + // pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + + // m_pivot.configAllSettings(pivotConfig); resetTeleSoftLimit(); @@ -58,17 +60,18 @@ public class Arm extends SubsystemBase { pivotConfig_.slot0.kI = SmartDashboard.getNumber("kI Pivot", 0); pivotConfig_.slot0.kD = SmartDashboard.getNumber("kD Pivot", 0); + pivotConfig_.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + pivotConfig_.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + pivotConfig_.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + // pivotConfig_.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); // pivotConfig_.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; // pivotConfig_.configSelectedFeedbackFilter();// FeedbackDevice.RemoteSensor0; m_pivot.configAllSettings(pivotConfig_); - m_pivot.configRemoteFeedbackFilter(encoder.getDeviceID(), RemoteSensorSource.CANCoder, 0); - m_pivot.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + // m_pivot.configRemoteFeedbackFilter(encoder.getDeviceID(), RemoteSensorSource.CANCoder, 0); + // m_pivot.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); // m_pivot.selectProfileSlot(0, 0); })); - - SmartDashboard.putData("set pos 1", new RunCommand(() -> m_pivot.set(ControlMode.Position, 225), this)); - SmartDashboard.putData("set pos 2", new RunCommand(() -> m_pivot.set(ControlMode.Position, 135), this)); SmartDashboard.putData("Kill Self", new InstantCommand(() -> {}, this)); } From e3a031bba7e4a4812cdd103ab6afc25e6b4e605f Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 14 Mar 2023 10:35:53 -0600 Subject: [PATCH 07/20] added deferred blocks --- src/main/java/frc4388/robot/Robot.java | 7 ++++++ .../java/frc4388/robot/subsystems/Arm.java | 6 ++++- .../java/frc4388/utility/DeferredBlock.java | 23 +++++++++++++++++++ 3 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/utility/DeferredBlock.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 91a0851..30a8e93 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -25,6 +25,7 @@ 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 frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; import frc4388.robot.subsystems.Location; @@ -113,6 +114,12 @@ public class Robot extends TimedRobot { // System.out.println("hi from disabled"); } + @Override + public void disabledExit() { + DeferredBlock.execute(); + super.disabledExit(); + } + /** * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index a7e76c3..4cb95b3 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -10,6 +10,7 @@ import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderConfiguration; import frc4388.robot.Constants.ArmConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.DeferredBlock; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -30,6 +31,9 @@ public class Arm extends SubsystemBase { tele.configFactoryDefault(); m_pivot.configFactoryDefault(); + // * Example of deferred code + new DeferredBlock(() -> resetTeleSoftLimit()); + // TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); // pivotConfig.slot0.kP = 0.5;//ArmConstants.kP; // pivotConfig.slot0.kI = 0.0;//ArmConstants.kI; @@ -41,7 +45,7 @@ public class Arm extends SubsystemBase { // m_pivot.configAllSettings(pivotConfig); - resetTeleSoftLimit(); + // resetTeleSoftLimit(); CANCoderConfiguration config = new CANCoderConfiguration(); config.magnetOffsetDegrees = ArmConstants.OFFSET; diff --git a/src/main/java/frc4388/utility/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java new file mode 100644 index 0000000..20d3c57 --- /dev/null +++ b/src/main/java/frc4388/utility/DeferredBlock.java @@ -0,0 +1,23 @@ +package frc4388.utility; + +import java.util.ArrayList; + +public class DeferredBlock { + private static ArrayList m_blocks = new ArrayList<>(); + private static boolean m_hasRun = false; + + public DeferredBlock(Runnable block) { + m_blocks.add(block); + } + + public static void execute() { + if (m_hasRun) return; + + for (Runnable block : m_blocks) { + block.run(); + } + + m_blocks.clear(); // for garbage collection + m_hasRun = true; + } +} From 6027b5ffb22a43b20baa393f69c3bf88db5a4c1e Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 14 Mar 2023 10:47:17 -0600 Subject: [PATCH 08/20] added more arm command stuff --- .../frc4388/robot/commands/ArmCommand.java | 55 +++++++------------ .../frc4388/robot/commands/AutoBalance.java | 2 +- .../commands/PelvicInflammatoryDisease.java | 23 ++++---- 3 files changed, 33 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ArmCommand.java b/src/main/java/frc4388/robot/commands/ArmCommand.java index 6afe6b4..87ab2b1 100644 --- a/src/main/java/frc4388/robot/commands/ArmCommand.java +++ b/src/main/java/frc4388/robot/commands/ArmCommand.java @@ -4,58 +4,43 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; +import org.opencv.osgi.OpenCVInterface; + import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; public class ArmCommand extends PelvicInflammatoryDisease { - private Arm arm; - private Claw claw; - private boolean toggle; + private final Arm arm; + private final Claw claw; + private final boolean toggle; + private final double target; /** Creates a new ArmCommand. */ - public ArmCommand(Arm arm, Claw claw, boolean open) { - // Use addRequirements() here to declare subsystem dependencies. - super(0.6, 0, 0, 0); - this.arm = arm; - this.claw = claw; + public ArmCommand(Arm arm, Claw claw, double target, boolean open) { + super(0.6, 0, 0, 0, 0); + this.arm = arm; + this.claw = claw; this.toggle = open; + this.target = target; addRequirements(arm, claw); } - public ArmCommand(Arm arm, Claw claw) { - this(arm, claw, claw.isClawOpen()); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - super.initialize(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; + public ArmCommand(Arm arm, Claw claw, double target) { + this(arm, claw, target, claw.isClawOpen()); } @Override public double getError() { - // TODO Auto-generated method stub - return 0; + return (arm.getArmRotation() - target) / 360; } @Override public void runWithOutput(double output) { - // TODO Auto-generated method stub - + arm.setRotVel(output); + } + + @Override + public void end(boolean interrupted) { + claw.setClaw(toggle); } } diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 2da529c..1c3523f 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -16,7 +16,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { /** Creates a new AutoBalanceTF2. */ public AutoBalance(RobotGyro gyro, SwerveDrive drive) { - super(0.6, 0, 0, 0); + super(0.6, 0, 0, 0, 0); this.gyro = gyro; this.drive = drive; diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index fb6a871..7135326 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -8,16 +8,19 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; public abstract class PelvicInflammatoryDisease extends CommandBase { - protected Gains gains; - private double output = 0; + protected Gains gains; + private double output = 0; + private double tolerance = 0; /** Creates a new PelvicInflammatoryDisease. */ - public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) { - gains = new Gains(kp, ki, kd, kf, 0); + public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) { + gains = new Gains(kp, ki, kd, kf, 0); + this.tolerance = tolerance; } - public PelvicInflammatoryDisease(Gains gains) { - this.gains = gains; + public PelvicInflammatoryDisease(Gains gains, double tolerance) { + this.gains = gains; + this.tolerance = tolerance; } /** produces the error from the setpoint */ @@ -48,11 +51,9 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { runWithOutput(output); } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - // Returns true when the command should end. @Override - public boolean isFinished() { return false; } + public boolean isFinished() { + return Math.abs(getError()) < tolerance; + } } From 4af2986ea6167eec0d9821052dfe5fc5774fb59a Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 14 Mar 2023 10:54:00 -0600 Subject: [PATCH 09/20] improved dynamic soft limits --- .../frc4388/robot/commands/ArmCommand.java | 2 -- .../java/frc4388/robot/subsystems/Arm.java | 20 +++++++------------ 2 files changed, 7 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ArmCommand.java b/src/main/java/frc4388/robot/commands/ArmCommand.java index 87ab2b1..90ec94d 100644 --- a/src/main/java/frc4388/robot/commands/ArmCommand.java +++ b/src/main/java/frc4388/robot/commands/ArmCommand.java @@ -4,8 +4,6 @@ package frc4388.robot.commands; -import org.opencv.osgi.OpenCVInterface; - import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 4cb95b3..f6b86a1 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -112,21 +112,18 @@ public class Arm extends SubsystemBase { } public double getArmLength() { - return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) / - (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT); + return m_tele.getSelectedSensorPosition(); } public double getArmRotation() { - return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) / - (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + return m_pivotEncoder.getAbsolutePosition(); } public void runPivotTele(double pivot, double tele) { - double rot = 0; - - if (checkLimits(tele, rot)) { - armSetRotation(pivot); - armSetLength(tele); + // TODO: tele has to go through some kind of transformation + if (pivot > 0 || tele < 0 || checkLimits(tele, getArmRotation())) { + setRotVel(pivot); + setTeleVel(tele); } } @@ -142,10 +139,7 @@ public class Arm extends SubsystemBase { var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta); var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x)); - if (y < minHeight) - return false; - - return true; + return y < minHeight; } boolean tele_softLimit = false; From bc8e97a7be1dc06a2b3eea06fef5866efc8fb46b Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 14 Mar 2023 10:58:27 -0600 Subject: [PATCH 10/20] finished dynamic limits (NOTE: fix actual soft limits) --- src/main/java/frc4388/robot/subsystems/Arm.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index f6b86a1..b68e5df 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -120,8 +120,11 @@ public class Arm extends SubsystemBase { } public void runPivotTele(double pivot, double tele) { - // TODO: tele has to go through some kind of transformation - if (pivot > 0 || tele < 0 || checkLimits(tele, getArmRotation())) { + double abs_pivot = Math.toRadians(getArmRotation() - 135); + double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) / + (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); + + if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) { setRotVel(pivot); setTeleVel(tele); } From 7d4ffd46603655a3d96b77b9218029409a26a576 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 14 Mar 2023 11:05:38 -0600 Subject: [PATCH 11/20] separated arm commands --- .../{ArmCommand.java => PivotCommand.java} | 20 ++---------- .../frc4388/robot/commands/TeleCommand.java | 32 +++++++++++++++++++ 2 files changed, 35 insertions(+), 17 deletions(-) rename src/main/java/frc4388/robot/commands/{ArmCommand.java => PivotCommand.java} (55%) create mode 100644 src/main/java/frc4388/robot/commands/TeleCommand.java diff --git a/src/main/java/frc4388/robot/commands/ArmCommand.java b/src/main/java/frc4388/robot/commands/PivotCommand.java similarity index 55% rename from src/main/java/frc4388/robot/commands/ArmCommand.java rename to src/main/java/frc4388/robot/commands/PivotCommand.java index 90ec94d..3f8235c 100644 --- a/src/main/java/frc4388/robot/commands/ArmCommand.java +++ b/src/main/java/frc4388/robot/commands/PivotCommand.java @@ -5,26 +5,17 @@ package frc4388.robot.commands; import frc4388.robot.subsystems.Arm; -import frc4388.robot.subsystems.Claw; -public class ArmCommand extends PelvicInflammatoryDisease { +public class PivotCommand extends PelvicInflammatoryDisease { private final Arm arm; - private final Claw claw; - private final boolean toggle; private final double target; /** Creates a new ArmCommand. */ - public ArmCommand(Arm arm, Claw claw, double target, boolean open) { + public PivotCommand(Arm arm, double target) { super(0.6, 0, 0, 0, 0); this.arm = arm; - this.claw = claw; - this.toggle = open; this.target = target; - addRequirements(arm, claw); - } - - public ArmCommand(Arm arm, Claw claw, double target) { - this(arm, claw, target, claw.isClawOpen()); + addRequirements(arm); } @Override @@ -36,9 +27,4 @@ public class ArmCommand extends PelvicInflammatoryDisease { public void runWithOutput(double output) { arm.setRotVel(output); } - - @Override - public void end(boolean interrupted) { - claw.setClaw(toggle); - } } diff --git a/src/main/java/frc4388/robot/commands/TeleCommand.java b/src/main/java/frc4388/robot/commands/TeleCommand.java new file mode 100644 index 0000000..02c8c27 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/TeleCommand.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import frc4388.robot.Constants.ArmConstants; +import frc4388.robot.subsystems.Arm; + +public class TeleCommand extends PelvicInflammatoryDisease { + private final Arm arm; + private final double target; + + /** Creates a new ArmCommand. */ + public TeleCommand(Arm arm, double target) { + super(0.6, 0, 0, 0, 0); + this.arm = arm; + this.target = target; + addRequirements(arm); + } + + @Override + public double getError() { + return (arm.getArmLength() - target) / + (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); + } + + @Override + public void runWithOutput(double output) { + arm.setTeleVel(output); + } +} From 29c6a2970242f304ad1742519ba85cf7a2653f8a Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 14 Mar 2023 11:07:46 -0600 Subject: [PATCH 12/20] Create LimeAlign.java --- .../frc4388/robot/commands/LimeAlign.java | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/LimeAlign.java diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java new file mode 100644 index 0000000..c811a3e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/LimeAlign.java @@ -0,0 +1,34 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; + +public class LimeAlign extends CommandBase { + public LimeAlign(SwerveDrive drive) { + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} From 03c938348fe2fa9dc3214be06e8bfb32eb4d7409 Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Tue, 14 Mar 2023 14:26:47 -0600 Subject: [PATCH 13/20] artificial soft limits --- .../java/frc4388/robot/RobotContainer.java | 5 +-- .../java/frc4388/robot/subsystems/Arm.java | 32 ++++++++----------- .../java/frc4388/robot/subsystems/Claw.java | 10 +++--- 3 files changed, 22 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c8cffac..054a85b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; +import frc4388.robot.commands.PivotCommand; import frc4388.robot.commands.PlaybackChooser; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -145,13 +146,13 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 200), m_robotArm)); + .whileTrue(new RunCommand(() -> new PivotCommand(m_robotArm, 135))); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 135), m_robotArm)); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 225), m_robotArm)); + .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index b68e5df..e20958f 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { private WPI_TalonFX m_tele; - public WPI_TalonFX m_pivot; + public WPI_TalonFX m_pivot; private CANCoder m_pivotEncoder; private boolean m_debug; @@ -32,7 +32,7 @@ public class Arm extends SubsystemBase { m_pivot.configFactoryDefault(); // * Example of deferred code - new DeferredBlock(() -> resetTeleSoftLimit()); + // new DeferredBlock(() -> resetTeleSoftLimit()); // TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); // pivotConfig.slot0.kP = 0.5;//ArmConstants.kP; @@ -84,7 +84,17 @@ public class Arm extends SubsystemBase { } public void setRotVel(double vel) { - m_pivot.set(ControlMode.PercentOutput, .4 * vel); + var degrees = Math.abs(getArmRotation()) - 135; + SmartDashboard.putNumber("arm degrees", degrees); + SmartDashboard.putNumber("arm rot vel", vel); + + if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) { + m_pivot.set(ControlMode.PercentOutput, 0); + } else if (degrees > 90 && vel > 0) { + m_pivot.set(ControlMode.PercentOutput, .2 * vel); + } else { + m_pivot.set(ControlMode.PercentOutput, .4 * vel); + } } public void setTeleVel(double vel) { @@ -167,21 +177,6 @@ public class Arm extends SubsystemBase { @Override public void periodic() { double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135); - if (degrees < 2 && resetable) { - var pivot_soft = m_pivot.getSelectedSensorPosition(); - var tele_soft = m_tele.getSelectedSensorPosition(); - - SmartDashboard.putNumber("start pivot", pivot_soft); - SmartDashboard.putNumber("start tele", tele_soft); - - m_pivot.configForwardSoftLimitEnable(soft_limits); - m_pivot.configReverseSoftLimitEnable(soft_limits); - SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value); - SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value); - resetable = false; - } else if (degrees > 2) { - resetable = true; - } if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) { var tele_soft = m_tele.getSelectedSensorPosition(); @@ -207,6 +202,7 @@ public class Arm extends SubsystemBase { boolean soft_limits = true; public void killSoftLimits() { + resetTeleSoftLimit(); var pivot_soft = m_pivot.getSelectedSensorPosition(); var tele_soft = m_tele.getSelectedSensorPosition(); diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index 8daa6b5..c4d3bda 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -22,9 +22,9 @@ public class Claw extends SubsystemBase { System.out.println("setClaw()"); // m_clawMotor.setPosition(0.5); // m_clawMotor.setRaw(0); - // m_clawMotor.setRaw(m_open ? 0 : 255); + m_clawMotor.setRaw(m_open ? 1000 : 2000); // m_clawMotor.setSpeed(m_open ? -1 : 1); - PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1); + // PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1); // PWMJNI.setPWMDisabled(0); System.out.println("Claw Pos: " + m_clawMotor.getRaw()); } @@ -39,9 +39,9 @@ public class Claw extends SubsystemBase { } public void disable() { - m_disabled = true; - // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle())); - PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5); + // m_disabled = true; + // // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle())); + // PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5); // PWMJNI.setPWMDisabled(m_clawMotor.getHandle()); } From d6569c78d3afa618a1cb08093bbd573f89b40e0c Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Tue, 14 Mar 2023 15:13:41 -0600 Subject: [PATCH 14/20] arm pids --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- .../frc4388/robot/commands/PelvicInflammatoryDisease.java | 2 ++ src/main/java/frc4388/robot/commands/PivotCommand.java | 6 ++++-- src/main/java/frc4388/robot/subsystems/Arm.java | 4 ++-- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 054a85b..1b6fb4c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -146,10 +146,10 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .whileTrue(new RunCommand(() -> new PivotCommand(m_robotArm, 135))); + .onTrue(new PivotCommand(m_robotArm, 135)); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 135), m_robotArm)); + .onTrue(new PivotCommand(m_robotArm, 210)); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 7135326..24e8d9b 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -4,6 +4,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; @@ -54,6 +55,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { + SmartDashboard.putBoolean("isFinished", Math.abs(getError()) < tolerance); return Math.abs(getError()) < tolerance; } } diff --git a/src/main/java/frc4388/robot/commands/PivotCommand.java b/src/main/java/frc4388/robot/commands/PivotCommand.java index 3f8235c..a84690d 100644 --- a/src/main/java/frc4388/robot/commands/PivotCommand.java +++ b/src/main/java/frc4388/robot/commands/PivotCommand.java @@ -4,6 +4,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.subsystems.Arm; public class PivotCommand extends PelvicInflammatoryDisease { @@ -12,7 +13,7 @@ public class PivotCommand extends PelvicInflammatoryDisease { /** Creates a new ArmCommand. */ public PivotCommand(Arm arm, double target) { - super(0.6, 0, 0, 0, 0); + super(6, 1.5, 0, 0, 0.015); this.arm = arm; this.target = target; addRequirements(arm); @@ -20,11 +21,12 @@ public class PivotCommand extends PelvicInflammatoryDisease { @Override public double getError() { - return (arm.getArmRotation() - target) / 360; + return (target - arm.getArmRotation()) / 360; } @Override public void runWithOutput(double output) { + SmartDashboard.putNumber("pivot output", output); arm.setRotVel(output); } } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index e20958f..e03897e 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -91,9 +91,9 @@ public class Arm extends SubsystemBase { if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) { m_pivot.set(ControlMode.PercentOutput, 0); } else if (degrees > 90 && vel > 0) { - m_pivot.set(ControlMode.PercentOutput, .2 * vel); + m_pivot.set(ControlMode.PercentOutput, .15 * vel); } else { - m_pivot.set(ControlMode.PercentOutput, .4 * vel); + m_pivot.set(ControlMode.PercentOutput, .3 * vel); } } From 4dab41ffee425fc7736d153d88c57a41e41d17d6 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Tue, 14 Mar 2023 15:20:05 -0600 Subject: [PATCH 15/20] Update Robot.java --- src/main/java/frc4388/robot/Robot.java | 38 ++++---------------------- 1 file changed, 5 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 30a8e93..fbdf90c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -45,9 +45,6 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - private Location location = new Location(); - - /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -72,18 +69,13 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); - - final Tag pos = location.getPosRot(); - if (pos != null) { - SmartDashboard.putNumber("x position", pos.x); - } - - //ystem.out.print(apriltagPos[0]); } /** @@ -91,28 +83,13 @@ public class Robot extends TimedRobot { * You can use it to reset any subsystem information you want to clear when * the robot is disabled. */ - boolean dis = false; @Override public void disabledInit() { - // m_robotContainer.m_robotClaw.setClaw(false); m_robotTime.endMatchTime(); - m_robotContainer.m_robotClaw.disable(); - - m_handle = m_robotContainer.m_robotMap.leftBackWheel.getHandle(); } - long m_handle = 0; @Override - public void disabledPeriodic() { - // if (dis) { - - // MotControllerJNI.Set_4(m_handle, ControlMode.PercentOutput.value, 1, 1, DemandType.Neutral.value); - // // m_robotContainer.m_robotMap.leftBackSteer.set(ControlMode.PercentOutput, 1, DemandType.Neutral, 0); - // // m_robotContainer.m_robotSwerveDrive.driveWithInput(new Translation2d(.5, 0), new Translation2d(), true); - // } - - // System.out.println("hi from disabled"); - } + public void disabledPeriodic() {} @Override public void disabledExit() { @@ -145,9 +122,6 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.m_robotSwerveDrive.resetGyro(); - dis = true; - - m_robotContainer.m_robotClaw.enable(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to @@ -164,13 +138,11 @@ public class Robot extends TimedRobot { * This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} /** * This function is called periodically during test mode. */ @Override - public void testPeriodic() { - } + public void testPeriodic() {} } From 1ae819013345b94f22d0f555234bf2c66b74b7a6 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Tue, 14 Mar 2023 15:21:46 -0600 Subject: [PATCH 16/20] Update AutoBalance.java --- src/main/java/frc4388/robot/commands/AutoBalance.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 1c3523f..cace0a8 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -14,7 +14,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { RobotGyro gyro; SwerveDrive drive; - /** Creates a new AutoBalanceTF2. */ + /** Creates a new AutoBalance. */ public AutoBalance(RobotGyro gyro, SwerveDrive drive) { super(0.6, 0, 0, 0, 0); From e58349c22f34428c44394996fe0cdfa19ef2e0ce Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Tue, 14 Mar 2023 15:22:40 -0600 Subject: [PATCH 17/20] Update PelvicInflammatoryDisease.java --- .../java/frc4388/robot/commands/PelvicInflammatoryDisease.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 24e8d9b..6c54024 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -55,7 +55,6 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - SmartDashboard.putBoolean("isFinished", Math.abs(getError()) < tolerance); return Math.abs(getError()) < tolerance; } } From aa32a223d6ef7e7ff1203d150f89da68654edd15 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Tue, 14 Mar 2023 15:25:53 -0600 Subject: [PATCH 18/20] Update Arm.java --- .../java/frc4388/robot/subsystems/Arm.java | 46 +------------------ 1 file changed, 2 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index e03897e..b03a25a 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -32,51 +32,11 @@ public class Arm extends SubsystemBase { m_pivot.configFactoryDefault(); // * Example of deferred code - // new DeferredBlock(() -> resetTeleSoftLimit()); - - // TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); - // pivotConfig.slot0.kP = 0.5;//ArmConstants.kP; - // pivotConfig.slot0.kI = 0.0;//ArmConstants.kI; - // pivotConfig.slot0.kD = 0.0;//ArmConstants.kD; - - // pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - // pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - // pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - - // m_pivot.configAllSettings(pivotConfig); - - // resetTeleSoftLimit(); + new DeferredBlock(() -> resetTeleSoftLimit()); CANCoderConfiguration config = new CANCoderConfiguration(); config.magnetOffsetDegrees = ArmConstants.OFFSET; m_pivotEncoder.configAllSettings(config); - // m_pivot.configFactoryDefault(); - // m_pivot.clearStickyFaults(); - // m_pi - - SmartDashboard.putNumber("kP Pivot", 0); - SmartDashboard.putNumber("kI Pivot", 0); - SmartDashboard.putNumber("kD Pivot", 0); - - SmartDashboard.putData("Set PID", new InstantCommand(() -> { - TalonFXConfiguration pivotConfig_ = new TalonFXConfiguration(); - pivotConfig_.slot0.kP = SmartDashboard.getNumber("kP Pivot", 0); - pivotConfig_.slot0.kI = SmartDashboard.getNumber("kI Pivot", 0); - pivotConfig_.slot0.kD = SmartDashboard.getNumber("kD Pivot", 0); - - pivotConfig_.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - pivotConfig_.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - pivotConfig_.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - - // pivotConfig_.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - // pivotConfig_.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - // pivotConfig_.configSelectedFeedbackFilter();// FeedbackDevice.RemoteSensor0; - m_pivot.configAllSettings(pivotConfig_); - // m_pivot.configRemoteFeedbackFilter(encoder.getDeviceID(), RemoteSensorSource.CANCoder, 0); - // m_pivot.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); - // m_pivot.selectProfileSlot(0, 0); - })); - SmartDashboard.putData("Kill Self", new InstantCommand(() -> {}, this)); } public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) { @@ -192,9 +152,7 @@ public class Arm extends SubsystemBase { SmartDashboard.putBoolean("reverse", m_tele.isFwdLimitSwitchClosed() == 1); double x = Math.cos(degrees); - // SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED); - // if (m_debug) - // SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition()); + SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition()); SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition()); SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition()); From a10e3f4573fc4daff07cdd0b528127dce25e62b5 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Tue, 14 Mar 2023 15:27:53 -0600 Subject: [PATCH 19/20] Update Claw.java --- .../java/frc4388/robot/subsystems/Claw.java | 22 +------------------ 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index c4d3bda..7d6d12c 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -15,22 +15,12 @@ public class Claw extends SubsystemBase { } public void setClaw(boolean open) { - if (m_disabled) return; // Open claw - // m_clawMotor.setRaw(150); m_open = open; - System.out.println("setClaw()"); - // m_clawMotor.setPosition(0.5); - // m_clawMotor.setRaw(0); m_clawMotor.setRaw(m_open ? 1000 : 2000); - // m_clawMotor.setSpeed(m_open ? -1 : 1); - // PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1); - // PWMJNI.setPWMDisabled(0); - System.out.println("Claw Pos: " + m_clawMotor.getRaw()); } public void toggle() { - System.out.println("toggle()"); setClaw(!m_open); } @@ -38,14 +28,4 @@ public class Claw extends SubsystemBase { return m_open; } - public void disable() { - // m_disabled = true; - // // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle())); - // PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5); - // PWMJNI.setPWMDisabled(m_clawMotor.getHandle()); - } - - public void enable() { - m_disabled = false; - } -} \ No newline at end of file +} From e91b9547210a52350df521668bb0c25766ebae50 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Tue, 14 Mar 2023 15:30:49 -0600 Subject: [PATCH 20/20] deleted odometry and poseestimator --- .../frc4388/robot/subsystems/SwerveDrive.java | 128 ------------------ 1 file changed, 128 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index fc216a6..fcaadb9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -35,10 +35,6 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - // private SwerveDriveOdometry odometry; - - private SwerveDrivePoseEstimator poseEstimator; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public Rotation2d rotTarget = new Rotation2d(); @@ -53,30 +49,6 @@ public class SwerveDrive extends SubsystemBase { this.gyro = gyro; - // this.odometry = new SwerveDriveOdometry( - // kinematics, - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // }, - // getOdometry() - // ); - - this.poseEstimator = new SwerveDrivePoseEstimator( - kinematics, - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - }, - new Pose2d(0,0, new Rotation2d(0)) - ); - this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } @@ -123,82 +95,8 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - // setOdometry(getOdometry()); rotTarget = new Rotation2d(0); } - - /** - * Updates the odometry of the SwerveDrive. - */ - // public void updateOdometry() { - // odometry.update( - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // } - // ); - // } - - public void updatePoseEstimator() { - poseEstimator.update( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - } - ); - } - - /** - * Gets the odometry of the SwerveDrive. - * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). - */ - // public Pose2d getOdometry() { - // return odometry.getPoseMeters(); - // } - - public Pose2d getPoseEstimator() { - return poseEstimator.getEstimatedPosition(); - } - - /** - * Sets the odometry of the SwerveDrive. - * @param pose Pose to set the odometry to. - */ - // public void setOdometry(Pose2d pose) { - // odometry.resetPosition( - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // }, - // pose - // ); - // } - - public void setPoseEstimator(Pose2d pose) { - poseEstimator.resetPosition( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - }, - pose - ); - } - - public void resetPoseEstimator() { - setPoseEstimator(new Pose2d()); - } public void stopModules() { for (SwerveModule module : this.modules) { @@ -206,14 +104,6 @@ public class SwerveDrive extends SubsystemBase { } } - /** - * Resets the odometry of the SwerveDrive to 0. - * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. - */ - // public void resetOdometry() { - // setOdometry(new Pose2d()); - // } - public SwerveDriveKinematics getKinematics() { return this.kinematics; } @@ -221,26 +111,8 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - - // updateOdometry(); - updatePoseEstimator(); - - // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); - // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); - // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); - - // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); - // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); } - /** - * Shifts gear from high to low, or vice versa. - * @param shift true to shift to high, false to shift to low - */ - // public void highSpeed(boolean shift) { - // this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - // } - public void toggleGear() { if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) { this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;