From 3a6be7a1138362f478c67da26ee06e6d57d8f0e2 Mon Sep 17 00:00:00 2001 From: Kingsley Wong Date: Sun, 9 Jun 2024 11:56:56 +0800 Subject: [PATCH 01/11] Did elevator to learn java --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/Robot.java | 5 ++++ .../java/frc/robot/subsystems/elevator.java | 29 +++++++++++++++++++ 3 files changed, 36 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/elevator.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f3e917a..5e9c3e0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,4 +12,6 @@ public final class Constants { public static final double climberI = 0; public static final double climberD = 0; public static final int intakePort = 35; + public static final int upperElevatorPort = 35; + public static final int lowerElevatorPort = 35; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8a1e6e0..c9d38c4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.elevator; public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -28,6 +29,7 @@ public class Robot extends TimedRobot { private Climber m_climber; private SendableChooser m_chooser = new SendableChooser<>(); private Intake m_intake; + private elevator m_elevator; private Command getAutonomousCommand() { return m_chooser.getSelected(); @@ -106,6 +108,9 @@ public Robot() { m_chooser.setDefaultOption("Simple Auto", m_shooter.spinup(1)); SmartDashboard.putData(m_chooser); + m_elevator = + new elevator(new CANSparkMax(Constants.upperElevatorPort, CANSparkMaxLowLevel.MotorType.kBrushless), new CANSparkMax(Constants.lowerElevatorPort, CANSparkMaxLowLevel.MotorType.kBrushless)); + configureBindings(); } diff --git a/src/main/java/frc/robot/subsystems/elevator.java b/src/main/java/frc/robot/subsystems/elevator.java new file mode 100644 index 0000000..c405f47 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class elevator extends SubsystemBase { + private CANSparkMax m_upperMotor; + private CANSparkMax m_lowerMotor; + + public elevator(CANSparkMax upperMotor, CANSparkMax lowerMotor) { + m_upperMotor = upperMotor; + m_lowerMotor = lowerMotor; + } + + public Command elevatorUp() { + return Commands.run(() -> m_upperMotor.setVoltage(1)) + .withTimeout(2) + .andThen(runOnce(() -> m_upperMotor.setVoltage(1))); + } + + public Command elevatorDown() { + return Commands.run(() -> m_lowerMotor.setVoltage(1)) + .withTimeout(2) + .andThen(runOnce(() -> m_lowerMotor.setVoltage(1))); + } +} \ No newline at end of file From 9c86546437cde84cad1ca439bb8e1ea83f2d55c4 Mon Sep 17 00:00:00 2001 From: Kingsley Wong Date: Sun, 21 Jul 2024 16:47:06 +0800 Subject: [PATCH 02/11] Uhh gonns fix later things --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/Robot.java | 12 +++++----- src/main/java/frc/robot/subsystems/Arm.java | 22 +++++++++---------- .../subsystems/CommandSwerveDrivetrain.java | 8 +++++++ .../java/frc/robot/subsystems/Shooter.java | 2 +- 5 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9196a6f..ad28bb0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,6 +13,7 @@ public final class Constants { public static final double shooterP = 0.5; public static final double shooterI = 0; public static final double shooterD = 0; + public static final double shootFromFarSpeed = 500; public static final int climberPort = 32; public static final double climberP = 0.35; public static final double climberI = 0; @@ -25,6 +26,8 @@ public final class Constants { public static final double armG = 0; public static final double armV = 0; public static final double armA = 0; + public static final double armLength = 0.65; + public static final double armPositionOffset = 0; public static final int armLeadPort = 21; public static final int armFollowerPort = 26; public static final int armEncoderPort = 3; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9a2b377..82accad 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -58,13 +58,13 @@ private void configureBindings() { .withVelocityY( Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed)) .withRotationalRate( - -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate))); - - m_driver.a().whileTrue(drivetrain.applyRequest(() -> brake)); - m_driver.x().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); + Utils.deadzone(-m_driver.getRightX() * Constants.DrivebaseMaxAngularRate)))); - m_driver.leftTrigger().onTrue(m_arm.runOnce(() -> m_arm.shootFromFar())); - m_driver.leftTrigger().onTrue(m_shooter.runOnce(() -> m_shooter.shootFromFar())); + m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> brake)); + m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); + // m_driver.leftTrigger().onTrue(m_arm.runOnce(() -> m_arm.shootFromFar())); + // m_driver.leftTrigger().onTrue(m_shooter.runOnce(() -> m_shooter.shootFromFar())); + m_driver.leftTrigger().onTrue(m_arm.runOnce(m_arm::shootFromFar)).onTrue(m_shooter.runOnce(m_shooter::shootFromFar)); } public Robot() { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 1db1c9f..ef4b445 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -15,6 +15,7 @@ import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -157,22 +158,21 @@ public Command goToSetpoint(Setpoint setpoint) { return moveToPosition(position); } - public double calculateArmAngle() { - double arm_length = 0.65; - double H = 2.46 - (Math.sin(m_encoder.getAbsolutePosition())*arm_length); // Height from top of the arm to the speaker shooting area - double arm_vertical_offset_from_floor = 0.3; // offset of floor to bottom of arm - double horizontal_robot_offset_from_speaker_wall = 0.5; // offset of the robot to the speaker wall when the robot is flush against the speaker - double K3 = arm_length*Math.sin(129); + public double calculateArmAngle(CommandSwerveDrivetrain drivetrain) { + double H = 2.46 - (Math.sin(m_encoder.getAbsolutePosition() * 360 + Constants.armPositionOffset) * Constants.armLength); // Height from top of the arm to the speaker shooting area + double arm_vertical_offset_from_floor = Math.sin(m_encoder.getAbsolutePosition()); // converts it from 0-1 to 0-365 and adds an offset depending where 0 degrees is. + double horizontal_robot_offset_from_speaker_wall = drivetrain.getEstimatedPosition(); // to be changed to wherever the robot is on field (get its value from drivetrain/drivebase)// offset of the robot to the speaker wall when the robot is flush against the speaker + double K3 = Constants.armLength * Math.sin(129); double K2 = horizontal_robot_offset_from_speaker_wall; - double K1 = H-arm_vertical_offset_from_floor; - double R = Math.sqrt(K2*K2-K1*K1); - double a = Math.atan(K3/K1); - return 51 - a + Math.acos(K3/R); + double K1 = H - arm_vertical_offset_from_floor; + double R = Math.sqrt(K2 * K2-K1 * K1); + double a = Math.atan(K3 / K1); + return 51 - a + Math.acos(K3 / R); } public Command shootFromFar() { return Commands.run(() -> moveToPosition(calculateArmAngle())); - /** you have to press a button on the controller. then you need to no move the arm angle using the thing and then shoot note */ + /* you have to press a button on the controller. then you need to no move the arm angle using the thing and then shoot note */ } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 32b1ba8..8492cd4 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -13,6 +13,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DataLogManager; @@ -22,10 +23,13 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; + import java.util.ArrayList; import java.util.Arrays; import java.util.function.Supplier; +import edu.wpi.first.math.geometry.Pose2d; + /** * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used * in command-based projects easily. @@ -203,4 +207,8 @@ public Command followTrajectory(String name, boolean isRed) { () -> isRed, this); } + + public Pose2d getEstimatedPosition() { + return m_odometry.getEstimatedPosition(); + } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 84aa784..6f8a056 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -94,6 +94,6 @@ public Trigger atSetpoint() { } public Command shootFromFar() { - return Commands.run(() -> spinup(500)); + return Commands.run(() -> spinup(Constants.shootFromFarSpeed)); } } From b002e9789d8be8c723de3041c4606b242fd09699 Mon Sep 17 00:00:00 2001 From: Kingsley Wong Date: Wed, 24 Jul 2024 21:52:07 +0800 Subject: [PATCH 03/11] improved shoot from far code, fixed errors --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/subsystems/Arm.java | 45 +++++++-------------- 3 files changed, 16 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ad28bb0..e0c25bd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,6 +28,7 @@ public final class Constants { public static final double armA = 0; public static final double armLength = 0.65; public static final double armPositionOffset = 0; + public static final double robotHeight = 0; // height of robot from ground to bottom of arm public static final int armLeadPort = 21; public static final int armFollowerPort = 26; public static final int armEncoderPort = 3; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 82accad..0aee45a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -62,9 +62,8 @@ private void configureBindings() { m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> brake)); m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); - // m_driver.leftTrigger().onTrue(m_arm.runOnce(() -> m_arm.shootFromFar())); + m_driver.leftTrigger().onTrue(m_arm.runOnce(() -> m_arm.goToSetpoint(m_drivetrain.getEstimatedPosition())).andThen(m_shooter.runOnce(() -> m_shooter.shootFromFar()))); // m_driver.leftTrigger().onTrue(m_shooter.runOnce(() -> m_shooter.shootFromFar())); - m_driver.leftTrigger().onTrue(m_arm.runOnce(m_arm::shootFromFar)).onTrue(m_shooter.runOnce(m_shooter::shootFromFar)); } public Robot() { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index ef4b445..811cee6 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -11,16 +11,15 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; @@ -134,34 +133,20 @@ public Command goToSetpoint(Setpoint setpoint) { log_setpoint.append(setpoint.name()); switch (setpoint) { - case kAmp: - position = 5.34; - break; - - case kIntake: - position = 3.7; - break; - - case kSpeaker: - position = 3.7; - break; - - case kStowed: - position = 3.7; - break; - - case kFarShoot: - position = calculateArmAngle(); - break; + case kAmp -> position = 5.34; + case kIntake -> position = 3.7; + case kSpeaker -> position = 3.7; + case kStowed -> position = 3.7; + case kFarShoot -> DataLogManager.log("WARNING: Invalid state kFarShoot with no pose."); } - return moveToPosition(position); + return moveToPosition(position); } - public double calculateArmAngle(CommandSwerveDrivetrain drivetrain) { - double H = 2.46 - (Math.sin(m_encoder.getAbsolutePosition() * 360 + Constants.armPositionOffset) * Constants.armLength); // Height from top of the arm to the speaker shooting area - double arm_vertical_offset_from_floor = Math.sin(m_encoder.getAbsolutePosition()); // converts it from 0-1 to 0-365 and adds an offset depending where 0 degrees is. - double horizontal_robot_offset_from_speaker_wall = drivetrain.getEstimatedPosition(); // to be changed to wherever the robot is on field (get its value from drivetrain/drivebase)// offset of the robot to the speaker wall when the robot is flush against the speaker + public double calculateArmAngle(Pose2d currentPose) { + double H = 2.46 - (Math.sin(m_encoder.getAbsolutePosition() * 360 + Constants.armPositionOffset) * Constants.armLength); // converts it from 0-1 to 0-365 and adds an offset depending where 0 degrees is. + double arm_vertical_offset_from_floor = (Constants.armLength * Math.sin(m_encoder.getAbsolutePosition())) + Constants.robotHeight; + double horizontal_robot_offset_from_speaker_wall = currentPose.getX(); // to be changed to wherever the robot is on field (get its value from drivetrain/drivebase)// offset of the robot to the speaker wall when the robot is flush against the speaker double K3 = Constants.armLength * Math.sin(129); double K2 = horizontal_robot_offset_from_speaker_wall; double K1 = H - arm_vertical_offset_from_floor; @@ -169,10 +154,8 @@ public double calculateArmAngle(CommandSwerveDrivetrain drivetrain) { double a = Math.atan(K3 / K1); return 51 - a + Math.acos(K3 / R); } - - public Command shootFromFar() { - return Commands.run(() -> moveToPosition(calculateArmAngle())); - /* you have to press a button on the controller. then you need to no move the arm angle using the thing and then shoot note */ + + public void goToSetpoint(Pose2d currentPose) { + double position = calculateArmAngle(currentPose); } - } \ No newline at end of file From 27795849d8387a450d75b1ce92f0470105a27296 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Wed, 24 Jul 2024 13:57:58 +0000 Subject: [PATCH 04/11] Formatting fixes --- src/main/java/frc/robot/Robot.java | 13 +++++-- src/main/java/frc/robot/subsystems/Arm.java | 38 +++++++++++-------- .../subsystems/CommandSwerveDrivetrain.java | 5 +-- .../java/frc/robot/subsystems/Shooter.java | 2 - 4 files changed, 32 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0aee45a..032c5f4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; @@ -58,11 +57,17 @@ private void configureBindings() { .withVelocityY( Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed)) .withRotationalRate( - Utils.deadzone(-m_driver.getRightX() * Constants.DrivebaseMaxAngularRate)))); + Utils.deadzone( + -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate)))); m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> brake)); m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); - m_driver.leftTrigger().onTrue(m_arm.runOnce(() -> m_arm.goToSetpoint(m_drivetrain.getEstimatedPosition())).andThen(m_shooter.runOnce(() -> m_shooter.shootFromFar()))); + m_driver + .leftTrigger() + .onTrue( + m_arm + .runOnce(() -> m_arm.goToSetpoint(m_drivetrain.getEstimatedPosition())) + .andThen(m_shooter.runOnce(() -> m_shooter.shootFromFar()))); // m_driver.leftTrigger().onTrue(m_shooter.runOnce(() -> m_shooter.shootFromFar())); } @@ -97,7 +102,7 @@ public Robot() { m_chooser.addOption("WompWompKieran Red", new WompWompKieran(m_drivetrain, true)); m_chooser.setDefaultOption("WompWompKieran Blue", new WompWompKieran(m_drivetrain, false)); SmartDashboard.putData(m_chooser); - + configureBindings(); } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 811cee6..7f2b6ff 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -4,11 +4,8 @@ package frc.robot.subsystems; -import java.util.function.DoubleSupplier; - import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; - import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -22,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; +import java.util.function.DoubleSupplier; /** Our Arm Subsystem */ public class Arm extends SubsystemBase { @@ -125,7 +123,6 @@ public Command manualControl(DoubleSupplier speed) { * Makes the arm go to a setpoint from the {@link Setpoint} enum * * @param setpoint The setpoint to go to, a {@link Setpoint} - * * @return A {@link Command} to go to the setpoint */ public Command goToSetpoint(Setpoint setpoint) { @@ -133,29 +130,38 @@ public Command goToSetpoint(Setpoint setpoint) { log_setpoint.append(setpoint.name()); switch (setpoint) { - case kAmp -> position = 5.34; - case kIntake -> position = 3.7; - case kSpeaker -> position = 3.7; - case kStowed -> position = 3.7; - case kFarShoot -> DataLogManager.log("WARNING: Invalid state kFarShoot with no pose."); + case kAmp -> position = 5.34; + case kIntake -> position = 3.7; + case kSpeaker -> position = 3.7; + case kStowed -> position = 3.7; + case kFarShoot -> DataLogManager.log("WARNING: Invalid state kFarShoot with no pose."); } - return moveToPosition(position); + return moveToPosition(position); } public double calculateArmAngle(Pose2d currentPose) { - double H = 2.46 - (Math.sin(m_encoder.getAbsolutePosition() * 360 + Constants.armPositionOffset) * Constants.armLength); // converts it from 0-1 to 0-365 and adds an offset depending where 0 degrees is. - double arm_vertical_offset_from_floor = (Constants.armLength * Math.sin(m_encoder.getAbsolutePosition())) + Constants.robotHeight; - double horizontal_robot_offset_from_speaker_wall = currentPose.getX(); // to be changed to wherever the robot is on field (get its value from drivetrain/drivebase)// offset of the robot to the speaker wall when the robot is flush against the speaker + double H = + 2.46 + - (Math.sin(m_encoder.getAbsolutePosition() * 360 + Constants.armPositionOffset) + * Constants + .armLength); // converts it from 0-1 to 0-365 and adds an offset depending where + // 0 degrees is. + double arm_vertical_offset_from_floor = + (Constants.armLength * Math.sin(m_encoder.getAbsolutePosition())) + Constants.robotHeight; + double horizontal_robot_offset_from_speaker_wall = + currentPose.getX(); // to be changed to wherever the robot is on field (get its value from + // drivetrain/drivebase)// offset of the robot to the speaker wall when the + // robot is flush against the speaker double K3 = Constants.armLength * Math.sin(129); double K2 = horizontal_robot_offset_from_speaker_wall; double K1 = H - arm_vertical_offset_from_floor; - double R = Math.sqrt(K2 * K2-K1 * K1); + double R = Math.sqrt(K2 * K2 - K1 * K1); double a = Math.atan(K3 / K1); return 51 - a + Math.acos(K3 / R); } - + public void goToSetpoint(Pose2d currentPose) { double position = calculateArmAngle(currentPose); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 8492cd4..ef16a12 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -13,8 +13,8 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -23,13 +23,10 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; - import java.util.ArrayList; import java.util.Arrays; import java.util.function.Supplier; -import edu.wpi.first.math.geometry.Pose2d; - /** * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used * in command-based projects easily. diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 6f8a056..58d9807 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -7,7 +7,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; import edu.wpi.first.util.datalog.DataLog; @@ -75,7 +74,6 @@ public Command stop() { * * @return A {@link Command} to hold the speed at the setpoint. */ - public Command maintain() { return achieveSpeeds(m_pid.getSetpoint()); } From ef88fa987aebfdb7c57c7e4500d28f056591fc19 Mon Sep 17 00:00:00 2001 From: Kingsley Wong Date: Thu, 25 Jul 2024 19:01:36 +0800 Subject: [PATCH 05/11] fixed requests --- src/main/java/frc/robot/Robot.java | 6 +++--- .../robot/subsystems/CommandSwerveDrivetrain.java | 6 +++--- src/main/java/frc/robot/subsystems/Shooter.java | 13 ------------- 3 files changed, 6 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0aee45a..1940654 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import java.util.HashMap; + import com.choreo.lib.Auto; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; @@ -26,7 +28,6 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; -import java.util.HashMap; public class Robot extends TimedRobot { private final CommandXboxController m_driver = new CommandXboxController(Constants.driverport); @@ -62,8 +63,7 @@ private void configureBindings() { m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> brake)); m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); - m_driver.leftTrigger().onTrue(m_arm.runOnce(() -> m_arm.goToSetpoint(m_drivetrain.getEstimatedPosition())).andThen(m_shooter.runOnce(() -> m_shooter.shootFromFar()))); - // m_driver.leftTrigger().onTrue(m_shooter.runOnce(() -> m_shooter.shootFromFar())); + m_driver.leftTrigger().onTrue(m_arm.run(() -> m_arm.goToSetpoint(m_drivetrain.getState().Pose)).andThen(m_shooter.run(() -> m_shooter.shootFromFar()))); } public Robot() { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 8492cd4..4cca0a7 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -208,7 +208,7 @@ public Command followTrajectory(String name, boolean isRed) { this); } - public Pose2d getEstimatedPosition() { - return m_odometry.getEstimatedPosition(); - } + // public Pose2d getEstimatedPosition() { + // return m_odometry.getEstimatedPosition(); + // } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 6f8a056..2cfd1e7 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -80,19 +80,6 @@ public Command maintain() { return achieveSpeeds(m_pid.getSetpoint()); } - /** - * Checks if the Shooter is at its setpoint and the loop is stable. - * - * @return A {@link Trigger} from the result. - */ - public Trigger atSetpoint() { - return new Trigger( - () -> - m_pid.getSetpoint() - == Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity()) - && m_pid.atSetpoint()); - } - public Command shootFromFar() { return Commands.run(() -> spinup(Constants.shootFromFarSpeed)); } From 205d0dffbfdd2469eb6102f6ce071cda55fa0950 Mon Sep 17 00:00:00 2001 From: Kingsley Wong Date: Thu, 25 Jul 2024 19:24:19 +0800 Subject: [PATCH 06/11] pulled from master, fixed merge errors (please check) 2 --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/Arm.java | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5cb9f2f..5be332c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -56,7 +56,7 @@ private void configureBindings() { m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake)); m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); - m_driver.leftTrigger().onTrue(m_arm.run(() -> m_arm.goToSetpoint(m_drivetrain.getState().Pose)).andThen(m_shooter.run(() -> m_shooter.shootFromFar()))); + m_driver.leftTrigger().onTrue(m_arm.goToSetpoint(m_drivetrain.getState().Pose).andThen(m_shooter.shootFromFar())); } public Robot() { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index af8098d..2f23267 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -147,7 +147,8 @@ public double calculateArmAngle(Pose2d currentPose) { return 51 - a + Math.acos(K3 / R); } - public void goToSetpoint(Pose2d currentPose) { + public Command goToSetpoint(Pose2d currentPose) { double position = calculateArmAngle(currentPose); + return moveToPosition(position); } } \ No newline at end of file From 6aeb9399c8f9cc949c4e2e2cb801229112cf938a Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 28 Jul 2024 03:16:45 +0000 Subject: [PATCH 07/11] Formatting fixes --- src/main/java/frc/robot/Robot.java | 7 ++++--- src/main/java/frc/robot/subsystems/Arm.java | 8 +++----- .../frc/robot/subsystems/CommandSwerveDrivetrain.java | 1 - 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 919a8b8..5372d9b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,6 @@ package frc.robot; -import java.util.HashMap; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; @@ -23,6 +21,7 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; +import java.util.HashMap; public class Robot extends CommandRobot { private final CommandXboxController m_driver = new CommandXboxController(Constants.driverport); @@ -56,7 +55,9 @@ private void configureBindings() { m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake)); m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); - m_driver.leftTrigger().onTrue(m_arm.goToSetpoint(m_drivetrain.getState().Pose).andThen(m_shooter.shootFromFar())); + m_driver + .leftTrigger() + .onTrue(m_arm.goToSetpoint(m_drivetrain.getState().Pose).andThen(m_shooter.shootFromFar())); } public Robot() { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index b3dd31e..dd46f0f 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -4,11 +4,8 @@ package frc.robot.subsystems; -import java.util.function.DoubleSupplier; - import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; - import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -22,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; +import java.util.function.DoubleSupplier; /** Our Arm Subsystem */ public class Arm extends SubsystemBase { @@ -154,9 +152,9 @@ public double calculateArmAngle(Pose2d currentPose) { double a = Math.atan(K3 / K1); return 51 - a + Math.acos(K3 / R); } - + public Command goToSetpoint(Pose2d currentPose) { double position = calculateArmAngle(currentPose); - return moveToPosition(position); + return moveToPosition(position); } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 2e86a6d..d1f5bb3 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -14,7 +14,6 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; From f8d94bca7b300c6dea6f7e650b633b360c7946f5 Mon Sep 17 00:00:00 2001 From: Kingsley Wong Date: Sun, 28 Jul 2024 11:25:34 +0800 Subject: [PATCH 08/11] seeing is there are any more failed changes --- src/main/java/frc/robot/Robot.java | 21 +++------------------ src/main/java/frc/robot/subsystems/Arm.java | 2 +- 2 files changed, 4 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 919a8b8..5bf5727 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -41,24 +42,6 @@ public class Robot extends CommandRobot { private final SwerveRequest.SwerveDriveBrake m_brake = new SwerveRequest.SwerveDriveBrake(); private final Telemetry m_logger = new Telemetry(); - private void configureBindings() { - m_drivetrain.setDefaultCommand( - m_drivetrain.applyRequest( - () -> - m_drive - .withVelocityX( - Utils.deadzone(-m_driver.getLeftY() * Constants.DrivebaseMaxSpeed)) - .withVelocityY( - Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed)) - .withRotationalRate( - Utils.deadzone( - -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate)))); - - m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake)); - m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); - m_driver.leftTrigger().onTrue(m_arm.goToSetpoint(m_drivetrain.getState().Pose).andThen(m_shooter.shootFromFar())); - } - public Robot() { HashMap aliases = new HashMap<>(); aliases.put(31, "Shooter"); @@ -108,5 +91,7 @@ public Robot() { m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake)); m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); + m_driver.leftTrigger().onTrue(m_arm.goToSetpoint(m_drivetrain.getState().Pose).andThen(m_shooter.shootFromFar())); + } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index b3dd31e..75f0565 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -154,7 +154,7 @@ public double calculateArmAngle(Pose2d currentPose) { double a = Math.atan(K3 / K1); return 51 - a + Math.acos(K3 / R); } - + public Command goToSetpoint(Pose2d currentPose) { double position = calculateArmAngle(currentPose); return moveToPosition(position); From 344f2b6392b6b3474a5338c8801c3463dd9b145e Mon Sep 17 00:00:00 2001 From: Kingsley Wong Date: Sun, 28 Jul 2024 11:31:12 +0800 Subject: [PATCH 09/11] seeing is there are any more failed changes 3 --- src/main/java/frc/robot/Robot.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5bf5727..49201bc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,12 +4,9 @@ package frc.robot; -import java.util.HashMap; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -24,6 +21,7 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; +import java.util.HashMap; public class Robot extends CommandRobot { private final CommandXboxController m_driver = new CommandXboxController(Constants.driverport); @@ -91,7 +89,8 @@ public Robot() { m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake)); m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); - m_driver.leftTrigger().onTrue(m_arm.goToSetpoint(m_drivetrain.getState().Pose).andThen(m_shooter.shootFromFar())); - + m_driver + .leftTrigger() + .onTrue(m_arm.goToSetpoint(m_drivetrain.getState().Pose).andThen(m_shooter.shootFromFar())); } } From 83656c8433cd4789530ec2855b9ec03a7a85dec6 Mon Sep 17 00:00:00 2001 From: Sean Chan Date: Sun, 11 Aug 2024 10:33:00 +0800 Subject: [PATCH 10/11] fixed build error --- src/main/java/frc/robot/subsystems/Shooter.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 00d9162..ae6d4c7 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -16,6 +16,7 @@ import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; From 44a70342455e8f8f74ffbc4c809b81e8e9c13ec5 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 11 Aug 2024 02:35:58 +0000 Subject: [PATCH 11/11] Formatting fixes --- src/main/java/frc/robot/subsystems/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 39ebd56..e71b99f 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -8,10 +8,10 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.util.datalog.StringLogEntry;