diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..750af95 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 3.8 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..e47fb5c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "path", + "data": { + "pathName": "Pickup Top" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot Top" + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Pickup Bottom.path b/src/main/deploy/pathplanner/paths/Pickup Bottom.path new file mode 100644 index 0000000..d3300dd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Pickup Bottom.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.5, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 4.0 + }, + "prevControl": { + "x": 1.5, + "y": 4.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Pickup Middle.path b/src/main/deploy/pathplanner/paths/Pickup Middle.path new file mode 100644 index 0000000..8c683b1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Pickup Middle.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.5, + "y": 5.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 5.5 + }, + "prevControl": { + "x": 1.5, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Pickup Top.path b/src/main/deploy/pathplanner/paths/Pickup Top.path new file mode 100644 index 0000000..fca0d39 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Pickup Top.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.5, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 7.0 + }, + "prevControl": { + "x": 1.5, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot Bottom.path b/src/main/deploy/pathplanner/paths/Shoot Bottom.path new file mode 100644 index 0000000..cac3dfe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot Bottom.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.5, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5, + "y": 5.5 + }, + "prevControl": { + "x": 2.5, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot Middle.path b/src/main/deploy/pathplanner/paths/Shoot Middle.path new file mode 100644 index 0000000..068fede --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot Middle.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 3.5, + "y": 5.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5, + "y": 5.5 + }, + "prevControl": { + "x": 2.5, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot Top.path b/src/main/deploy/pathplanner/paths/Shoot Top.path new file mode 100644 index 0000000..c272e37 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot Top.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.5, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5, + "y": 5.5 + }, + "prevControl": { + "x": 2.5, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java index b6e354b..d7ac810 100644 --- a/src/main/java/org/ghrobotics/frc2024/Robot.java +++ b/src/main/java/org/ghrobotics/frc2024/Robot.java @@ -17,8 +17,10 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import org.ghrobotics.frc2024.Superstructure.Position; +import org.ghrobotics.frc2024.auto.AutoSelector; import org.ghrobotics.frc2024.commands.ArmPID; import org.ghrobotics.frc2024.commands.DriveTeleop; +import org.ghrobotics.frc2024.commands.RunRobot; import org.ghrobotics.frc2024.subsystems.Arm; // import org.ghrobotics.frc2024.subsystems.Climber; import org.ghrobotics.frc2024.subsystems.Drive; @@ -43,8 +45,11 @@ public class Robot extends TimedRobot { // private final ArmPID arm_command = new ArmPID(); + Command runrobot = new RunRobot(drive_); + // Robot State private final RobotState robot_state_ = new RobotState(drive_); + private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_, feeder_); // Telemetry private final Telemetry telemetry_ = new Telemetry(robot_state_, arm_); @@ -61,12 +66,13 @@ public class Robot extends TimedRobot { // Playstation controller just for testing // private final CommandPS4Controller ps4_controller_ = new CommandPS4Controller(0); // Superstructure - private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_, feeder_); + // private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_, feeder_); + private final AutoSelector auto_selector_ = new AutoSelector(drive_, robot_state_, superstructure_, arm_); public Command test() { return new SequentialCommandGroup( - superstructure_.setPosition(Position.STOW), + // superstructure_.setPosition(Position.STOW), new WaitCommand(3.0) ); } @@ -95,10 +101,22 @@ public void robotPeriodic() { @Override - public void autonomousInit() {} + public void autonomousInit() { + // robot_state_.reset(auto_selector_.getStartPosition()); + // //SmartDashboard.putNumber("before", 1); + auto_selector_.basicAutoCommand().schedule(); + //SmartDashboard.putNumber("after", 2); + // runrobot.schedule(); + drive_.setBrakeMode(true); + arm_.setBrakeMode(true); + + } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + // shooter_.setPercent(-0.50); + + } // Might need this in the future // robot_state_.reset(robot_state_.getPosition()); @@ -152,7 +170,7 @@ private void setupTeleopControls() { // Driver Control driver_controller_.rightTrigger().whileTrue(superstructure_.setShooter(-0.75)); - driver_controller_.leftTrigger().whileTrue(superstructure_.setIntake(-0.50)); + driver_controller_.leftTrigger().whileTrue(superstructure_.setIntake(-0.40)); driver_controller_.leftBumper().whileTrue(superstructure_.setIntake(0.15)); @@ -183,7 +201,7 @@ private void setupTeleopControls() { operator_controller_.x().onTrue(new ArmPID(arm_, 60)); - operator_controller_.pov(0).whileTrue(superstructure_.setArmPercent(0.1)); + operator_controller_.pov(0).whileTrue(superstructure_.setArmPercent(0.20)); operator_controller_.pov(180).whileTrue(superstructure_.setArmPercent(-0.1)); } diff --git a/src/main/java/org/ghrobotics/frc2024/auto/AutoSelector.java b/src/main/java/org/ghrobotics/frc2024/auto/AutoSelector.java new file mode 100644 index 0000000..2c12e79 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/auto/AutoSelector.java @@ -0,0 +1,118 @@ +package org.ghrobotics.frc2024.auto; + +import org.ghrobotics.frc2024.RobotState; +import org.ghrobotics.frc2024.Superstructure; +import org.ghrobotics.frc2024.commands.ArmPID; +import org.ghrobotics.frc2024.commands.RunRobot; +import org.ghrobotics.frc2024.subsystems.Arm; +import org.ghrobotics.frc2024.subsystems.Drive; + +// import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +// import com.pathplanner.lib.util.PIDConstants; +// import com.pathplanner.lib.util.ReplanningConfig; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class AutoSelector { + // private final SendableChooser auto_chooser_; + + private final Drive drive_; + private final RobotState robot_state_; + private final Superstructure superstructure_; + private final Arm arm_; + + + Timer time_ = new Timer(); + + public AutoSelector(Drive drive, RobotState robot_state, Superstructure superstructure, Arm arm) { + + drive_ = drive; + robot_state_ = robot_state; + superstructure_ = superstructure; + arm_ = arm; + + + + // AutoBuilder.configureHolonomic( + // robot_state_::getPosition, + // robot_state_::reset, + // drive_::getSpeeds, + // drive_::setSpeeds, + // new HolonomicPathFollowerConfig( + // new PIDConstants(Constants.kP, 0.0, 0.0), + // new PIDConstants(Constants.kP, 0.0, 0.0), + // 3.0, + // 0.375, + // new ReplanningConfig() + // ), + // () -> { + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent()) { + // return alliance.get() == DriverStation.Alliance.Red; + // } + // return false; + // }, + // drive_ + // ); + + // auto_chooser_ = AutoBuilder.buildAutoChooser(); + // auto_chooser_.setDefaultOption("One Note Middle Taxi", new OneNoteMiddleTaxi(drive_, superstructure_)); + // auto_chooser_.addOption("One Note Top Taxi", new OneNoteTopTaxi(drive_, superstructure_)); + // auto_chooser_.addOption("One Note Bottom Taxi", new OneNoteBottomTaxi(drive_, superstructure_)); + // auto_chooser_.addOption("Two Note Top", new TwoNoteTop(drive_, superstructure_)); + // auto_chooser_.addOption("Two Note Middle", new TwoNoteMiddle(drive_, superstructure_)); + // auto_chooser_.addOption("Two Note Bottom", new TwoNoteBottom(drive_, superstructure_)); + + // SmartDashboard.putData("Auto Chooser", auto_chooser_); + } + + // public Command getAutonomousCommand() { + // return auto_chooser_.getSelected(); + // } + + public Command basicAutoCommand() { + return moveCommand(); + } + + public Command moveCommand() { + return new SequentialCommandGroup( + new ArmPID(arm_, 2).withTimeout(1.5), + new RunRobot(drive_) + ); + } + + // this is here just so it stopped giving me errors we need to fix this + // PathPlannerPath pickup_top = PathPlannerPath.fromPathFile("Pickup Top"); + // PathPlannerPath pickup_middle = PathPlannerPath.fromPathFile("Pickup Middle"); + // PathPlannerPath pickup_bottom = PathPlannerPath.fromPathFile("Pickup Bottom"); + + // public Pose2d getStartPosition() { + // if (auto_chooser_.getSelected() instanceof TwoNoteBottom || auto_chooser_.getSelected() instanceof OneNoteBottomTaxi) { + // return pickup_bottom.getStartingDifferentialPose(); + // } + // else if (auto_chooser_.getSelected() instanceof TwoNoteMiddle || auto_chooser_.getSelected() instanceof OneNoteMiddleTaxi) { + // return pickup_middle.getStartingDifferentialPose(); + // } + // else if (auto_chooser_.getSelected() instanceof TwoNoteTop || auto_chooser_.getSelected() instanceof OneNoteTopTaxi) { + // return pickup_top.getStartingDifferentialPose(); + // } + // return pickup_top.getStartingDifferentialPose(); + // } + + public static class Constants { + public static double kP = 4.5; + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/commands/DriveTeleop.java b/src/main/java/org/ghrobotics/frc2024/commands/DriveTeleop.java index aa8ee8a..bf4b33b 100644 --- a/src/main/java/org/ghrobotics/frc2024/commands/DriveTeleop.java +++ b/src/main/java/org/ghrobotics/frc2024/commands/DriveTeleop.java @@ -6,7 +6,11 @@ import org.ghrobotics.frc2024.RobotState; import org.ghrobotics.frc2024.subsystems.Drive; +import org.ghrobotics.frc2024.RobotState; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; @@ -66,6 +70,9 @@ public void execute() { else{ drive_.setSpeeds(speeds, Drive.OutputType.OPEN_LOOP); } + + if (controller_.leftBumper().getAsBoolean()) + robot_state_.reset(new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0))); } // Constants diff --git a/src/main/java/org/ghrobotics/frc2024/commands/RunRobot.java b/src/main/java/org/ghrobotics/frc2024/commands/RunRobot.java new file mode 100644 index 0000000..45b3862 --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/commands/RunRobot.java @@ -0,0 +1,50 @@ +// 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 org.ghrobotics.frc2024.commands; + +import org.ghrobotics.frc2024.subsystems.Drive; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class RunRobot extends Command { + /** Creates a new RunRobot. */ + + private final Drive drive_; + + Timer time_ = new Timer(); + public RunRobot(Drive drive) { + // Use addRequirements() here to declare subsystem dependencies. + drive_ = drive; + addRequirements(drive_); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + time_.reset(); + time_.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + drive_.setSpeeds(new ChassisSpeeds(1.5, 0, 0)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drive_.setSpeeds(new ChassisSpeeds(0, 0, 0)); + time_.reset(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return time_.get() > 3.0; + } +} diff --git a/src/main/java/org/ghrobotics/frc2024/commands/StayPut.java b/src/main/java/org/ghrobotics/frc2024/commands/StayPut.java new file mode 100644 index 0000000..d1c163e --- /dev/null +++ b/src/main/java/org/ghrobotics/frc2024/commands/StayPut.java @@ -0,0 +1,36 @@ +// // 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 org.ghrobotics.frc2024.commands; + +// import edu.wpi.first.wpilibj2.command.Command; + +// public class StayPut extends Command { + +// private final Arm arm_; + + +// /** Creates a new StayPut. */ +// public StayPut() { +// // Use addRequirements() here to declare subsystem dependencies. +// } + +// // 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; +// } +// } diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Drive.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Drive.java index cee6d60..a0a0c54 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Drive.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Drive.java @@ -1,13 +1,23 @@ package org.ghrobotics.frc2024.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.ghrobotics.frc2024.RobotState; + import com.kauailabs.navx.frc.AHRS; +// import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +// import com.pathplanner.lib.util.PIDConstants; +// import com.pathplanner.lib.util.ReplanningConfig; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -28,7 +38,7 @@ public class Drive extends SubsystemBase { SwerveDriveKinematics kinematics_ = new SwerveDriveKinematics( Constants.kFrontLeftLocation, Constants.kFrontRightLocation, Constants.kBackLeftLocation, Constants.kBackRightLocation); - + // IO private final IO io_ = new IO(); private OutputType output_type_ = OutputType.OPEN_LOOP; @@ -87,6 +97,12 @@ public SwerveDriveKinematics getKinematics() { return kinematics_; } + + // Getter for ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + public ChassisSpeeds getSpeeds() { + return io_.speeds; + } + // Set Speeds public void setSpeeds(ChassisSpeeds speeds, OutputType output_type) { io_.speeds = speeds; @@ -111,7 +127,7 @@ public void HoldPosition() { back_left_.setAngle(-45); back_right_.setAngle(45); } - + // Output Type public enum OutputType { VELOCITY, OPEN_LOOP diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java index 3985e9f..b5acc97 100644 --- a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java +++ b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java @@ -36,6 +36,7 @@ public void periodic() { // Read inputs. io_.current_left_ = leader_left_.getOutputCurrent(); io_.current_right_ = leader_right_.getOutputCurrent(); + leader_left_.set(io_.left_demand); leader_right_.set(io_.right_demand); } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..ff15fab --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.2.3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.2.3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.2.3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file