From a7e140d20b499f02cdd9a5691eb9b0d784901f6b Mon Sep 17 00:00:00 2001 From: Ayush Sagar Date: Fri, 22 Sep 2023 20:03:57 -0400 Subject: [PATCH] buggy code will fix later --- .../deploy/pathplanner/LowScoreTaxiBump.path | 49 ++++++++++++ src/main/deploy/pathplanner/New Path.path | 49 ++++++++++++ src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotState.java | 10 ++- src/main/java/frc/robot/Superstructure.java | 9 ++- src/main/java/frc/robot/auto/ArmSimTest.java | 26 +++++- .../java/frc/robot/auto/AutoSelector.java | 11 +-- .../java/frc/robot/auto/DriveTrajectory.java | 79 +++++++++++++++++++ src/main/java/frc/robot/subsystems/Arm.java | 2 +- src/main/java/frc/robot/subsystems/Drive.java | 25 +++--- .../java/frc/robot/subsystems/Intake.java | 2 - 11 files changed, 236 insertions(+), 28 deletions(-) create mode 100644 src/main/deploy/pathplanner/LowScoreTaxiBump.path create mode 100644 src/main/deploy/pathplanner/New Path.path create mode 100644 src/main/java/frc/robot/auto/DriveTrajectory.java diff --git a/src/main/deploy/pathplanner/LowScoreTaxiBump.path b/src/main/deploy/pathplanner/LowScoreTaxiBump.path new file mode 100644 index 0000000..5f16ff2 --- /dev/null +++ b/src/main/deploy/pathplanner/LowScoreTaxiBump.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.92, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 3.0042032480370326, + "y": 0.5 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.371669585444434, + "y": 0.5 + }, + "prevControl": { + "x": 5.336015096499199, + "y": 0.5 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..ab4bfc7 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 3.599129905026728, + "y": 3.264765928028098 + }, + "prevControl": null, + "nextControl": { + "x": 3.622136041599105, + "y": 3.264765928028098 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.341533284570328, + "y": 0.924179893472571 + }, + "prevControl": { + "x": 5.341533284570328, + "y": 0.924179893472571 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index aa3a361..54fcc2e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -35,7 +35,7 @@ public class Robot extends TimedRobot { private final RobotState robot_state_ = new RobotState(drive_); // Superstructure - private final Superstructure superstructure_ = new Superstructure(arm_); + private final Superstructure superstructure_ = new Superstructure(arm_, intake_); // Xbox Controller private final CommandXboxController driver_controller_ = new CommandXboxController(0); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 763cbd4..178bc8e 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.subsystems.Drive; public class RobotState { @@ -38,6 +39,13 @@ public double getDegree() { return pose_estimator_.getEstimatedPosition().getRotation().getDegrees(); } + // Get Rotation2d + public Rotation2d getRotation2d() { + return pose_estimator_.getEstimatedPosition().getRotation(); + } + // Reset Position - public void reset(Pose2d pose) {} + public void reset(Pose2d pose) { + pose_estimator_.resetPosition(getRotation2d(), drive_.getSwerveModulePositions(), pose); + } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 37f9f90..7b726c7 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -5,16 +5,19 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.ArmToPosition; import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Intake; public class Superstructure { // Subsystems private final Arm arm_; + private final Intake intake_; // Position State public String state = "STOW"; - public Superstructure(Arm arm) { + public Superstructure(Arm arm, Intake intake) { arm_ = arm; + intake_ = intake; } @@ -31,6 +34,10 @@ public String getState() { return state; } + public Command setIntakePercent(double value) { + return new InstantCommand(() -> intake_.setPercent(value)); + } + diff --git a/src/main/java/frc/robot/auto/ArmSimTest.java b/src/main/java/frc/robot/auto/ArmSimTest.java index 38ce42d..30675c5 100644 --- a/src/main/java/frc/robot/auto/ArmSimTest.java +++ b/src/main/java/frc/robot/auto/ArmSimTest.java @@ -1,17 +1,37 @@ package frc.robot.auto; +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Superstructure; import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Drive; public class ArmSimTest extends SequentialCommandGroup{ + // Trajectories + private final PathPlannerTrajectory traj1 = PathPlanner.loadPath("LowScoreTaxiBump", new PathConstraints(4, 3)); + + private final Drive drive_; + private final frc.robot.RobotState robot_state_; + + // private final DriveTrajectory follow = new DriveTrajectory() + + // Constructor - public ArmSimTest(Superstructure superstructure, Arm arm) { + public ArmSimTest(Superstructure superstructure, Arm arm, Drive drive, frc.robot.RobotState robot_state) { + drive_ = drive; + robot_state_ = robot_state; addCommands( superstructure.setPosition(Superstructure.Position.STOW), new WaitCommand(1.0), - superstructure.setPosition(Superstructure.Position.CUBE_L2) + superstructure.setPosition(Superstructure.Position.CUBE_L1), + new WaitCommand(2.5), + superstructure.setIntakePercent(0.5), + new InstantCommand(() -> new DriveTrajectory(traj1).followTrajectoryCommand(true, robot_state_, drive_)) + ); } -} +} diff --git a/src/main/java/frc/robot/auto/AutoSelector.java b/src/main/java/frc/robot/auto/AutoSelector.java index f286d20..c795cac 100644 --- a/src/main/java/frc/robot/auto/AutoSelector.java +++ b/src/main/java/frc/robot/auto/AutoSelector.java @@ -1,10 +1,5 @@ package frc.robot.auto; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotState; -import frc.robot.Superstructure; -import frc.robot.subsystems.Arm; - public class AutoSelector { // private Command auto_command_ = null; @@ -12,7 +7,7 @@ public AutoSelector() { ; } - public Command run(RobotState robot_state, Superstructure superstructure, Arm arm) { - return new ArmSimTest(superstructure, arm); - } + // public Command run(RobotState robot_state, Superstructure superstructure, Arm arm) { + // return new ArmSimTest(superstructure, arm); + // } } diff --git a/src/main/java/frc/robot/auto/DriveTrajectory.java b/src/main/java/frc/robot/auto/DriveTrajectory.java new file mode 100644 index 0000000..e82daf6 --- /dev/null +++ b/src/main/java/frc/robot/auto/DriveTrajectory.java @@ -0,0 +1,79 @@ +package frc.robot.auto; + +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; +import com.pathplanner.lib.commands.PPSwerveControllerCommand; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotState; +import frc.robot.subsystems.Drive; + +public class DriveTrajectory extends CommandBase{ + private final PathPlannerTrajectory traj_; + private final Timer timer_; + private double t; + + public DriveTrajectory(PathPlannerTrajectory traj) { + traj_ = traj; + timer_ = new Timer(); + } + + + @Override + public void initialize() { + timer_.start(); + } + + @Override + public void execute() { + t = timer_.get(); + } + + public Pose2d getTrajectoryPose(double time) { + final PathPlannerState state = (PathPlannerState) traj_.sample(t); + return new Pose2d(new Translation2d(state.poseMeters.getX(), state.poseMeters.getY()), state.holonomicRotation); + } + + public Command followTrajectoryCommand(boolean isFirstTrajectory, RobotState robot_state_, Drive drive_) { + return new SequentialCommandGroup( + new InstantCommand(() -> { + if(isFirstTrajectory) { + robot_state_.reset(traj_.getInitialHolonomicPose()); + SmartDashboard.putNumber("Trajectory Total Time", traj_.getTotalTimeSeconds()); + } + }), + new PPSwerveControllerCommand( + traj_, + robot_state_::getPosition, + drive_.getKinematics(), + Constants.kXController, + Constants.kYController, + Constants.kThetaController, + drive_::setModuleStates, + true, + drive_ + ) + ); + } + + public double getTrajectoryTime() { + return traj_.getTotalTimeSeconds(); + } + + public static class Constants { + public static double kP = 0.5; + + public static PIDController kXController = new PIDController(kP, 0, 0); + public static PIDController kYController = new PIDController(kP, 0, 0); + public static PIDController kThetaController = new PIDController(kP, 0, 0); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 3978f6c..393cabd 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -193,7 +193,7 @@ private static class Constants { public static final int kMotorId = 9; // Physical Constants - public static final double kGearRatio = 22.0 / 34.0; + public static final double kGearRatio = 49.0 * 34.0 / 22.0; public static final double kMinAngle = Math.toRadians(-40); public static final double kMaxAngle = Math.toRadians(170); public static final double kArmLength = 0.30; diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 9f898a0..5cc2d59 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -61,17 +61,7 @@ public void periodic() { // Calculate outputs SwerveModuleState[] module_states = kinematics_.toSwerveModuleStates(io_.speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(module_states, Constants.kMaxSpeed); - - // Set outputs and telemetry - for (int i = 0; i < modules_.length; i++) { - modules_[i].setDesiredState(module_states[i], output_type_); - - SmartDashboard.putNumber(String.format("Module [%d] Speed", i), - modules_[i].getDriveVelocity()); - SmartDashboard.putNumber(String.format("Module [%d] Angle", i), - modules_[i].getSteerPosition().getDegrees()); - } + setModuleStates(module_states); } // Get Module Positions @@ -105,6 +95,19 @@ public void setSpeeds(ChassisSpeeds speeds) { setSpeeds(speeds, OutputType.VELOCITY); } + // Set Module States + public void setModuleStates(SwerveModuleState[] desired_states) { + SwerveDriveKinematics.desaturateWheelSpeeds(desired_states, Constants.kMaxSpeed); + for (int i = 0; i < modules_.length; i++){ + modules_[i].setDesiredState(desired_states[i], output_type_); + + SmartDashboard.putNumber(String.format("Module [%d] Speed", i), + modules_[i].getDriveVelocity()); + SmartDashboard.putNumber(String.format("Module [%d] Angle", i), + modules_[i].getSteerPosition().getDegrees()); + } + } + // Puts wheels in X shape for brake public void HoldPosition() { front_left_.setAngle(45); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index edfd9c5..e62d64f 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,7 +1,5 @@ package frc.robot.subsystems; -import javax.lang.model.util.ElementScanner14; - import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase;