Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
  • Loading branch information
anivanchen committed Feb 2, 2024
2 parents 9e5bb0b + 8164a75 commit 43405bb
Show file tree
Hide file tree
Showing 11 changed files with 245 additions and 11 deletions.
24 changes: 22 additions & 2 deletions .Glass/glass.json
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,17 @@
"Launcher": {
"Amp": {
"open": true
}
},
"Swerve": {
"Modules": {
"Back Left": {
"open": true
},
"Back Right": {
"open": true
},
"open": true
},
"open": true
},
Expand All @@ -30,7 +41,15 @@
},
"types": {
"/FMSInfo": "FMSInfo",
"/SmartDashboard/Autonomous": "String Chooser"
"/SmartDashboard/Autonomous": "String Chooser",
"/SmartDashboard/Field": "Field2d"
},
"windows": {
"/SmartDashboard/Autonomous": {
"window": {
"visible": true
}
}
}
},
"NetworkTables Info": {
Expand Down Expand Up @@ -83,5 +102,6 @@
}
]
}
}
},
"enterKey": 92
}
12 changes: 12 additions & 0 deletions .pathplanner/settings.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
{
"robotWidth": 0.9,
"robotLength": 0.9,
"holonomicMode": false,
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"maxModuleSpeed": 4.5
}
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.1.1"
id "edu.wpi.first.GradleRIO" version "2024.2.1"
id "com.diffplug.spotless" version "6.22.0"
}

Expand Down
31 changes: 31 additions & 0 deletions src/main/deploy/pathplanner/autos/One Piece.auto
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
{
"version": 1.0,
"startingPose": null,
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "1 Note Part 1"
}
},
{
"type": "path",
"data": {
"pathName": "1 Note Part 2"
}
},
{
"type": "named",
"data": {
"name": "LauncherLaunch"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}
1 change: 1 addition & 0 deletions src/main/deploy/pathplanner/navgrid.json
Original file line number Diff line number Diff line change
@@ -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]]}
49 changes: 49 additions & 0 deletions src/main/deploy/pathplanner/paths/1 Note Part 1.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.8574827844100136,
"y": 4.028517436664504
},
"prevControl": null,
"nextControl": {
"x": 0.8617584596280307,
"y": 4.017650095485377
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.8779768624352067,
"y": 4.781214428169598
},
"prevControl": {
"x": 2.0779768624352064,
"y": 4.781214428169598
},
"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": null,
"useDefaultConstraints": false
}
49 changes: 49 additions & 0 deletions src/main/deploy/pathplanner/paths/1 Note Part 2.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 2.88,
"y": 4.78
},
"prevControl": null,
"nextControl": {
"x": 2.235378507794053,
"y": 4.985437304207942
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.3921797241742635,
"y": 5.5760149436965545
},
"prevControl": {
"x": 2.4431478839872605,
"y": 5.5760149436965545
},
"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": -50.079173387765984,
"rotateFast": false
},
"reversed": true,
"folder": null,
"previewStartingState": null,
"useDefaultConstraints": false
}
16 changes: 14 additions & 2 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,12 @@

package com.stuypulse.robot;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.stuypulse.robot.commands.auton.DoNothingAuton;
import com.stuypulse.robot.commands.drivetrain.DrivetrainDrive;
import com.stuypulse.robot.commands.launcher.LaunchPrepare;
import com.stuypulse.robot.commands.launcher.LaunchPrepare;
import com.stuypulse.robot.commands.launcher.LauncherHoldSpeed;
import com.stuypulse.robot.commands.launcher.LauncherLaunchSpeaker;
import com.stuypulse.robot.commands.launcher.LauncherIntakeNote;
Expand Down Expand Up @@ -44,6 +47,7 @@ public class RobotContainer {
public RobotContainer() {
configureDefaultCommands();
configureButtonBindings();
configureNamedCommands();
configureAutons();
}

Expand All @@ -56,6 +60,14 @@ private void configureDefaultCommands() {
launcher.setDefaultCommand(new LauncherHoldSpeed(Settings.Launcher.LAUNCHER_SPEAKER_SPEED));
}

/**********************/
/*** NAMED COMMANDS ***/
/**********************/

private void configureNamedCommands() {
NamedCommands.registerCommand("LauncherLaunch", new LauncherLaunchSpeaker());
}

/***************/
/*** BUTTONS ***/
/***************/
Expand All @@ -70,7 +82,7 @@ private void configureDriverBindings() {
.whileTrue(new LauncherIntakeNote());

driver.getBottomButton()
.whileTrue(new LaunchPrepare(Settings.Launcher.LAUNCHER_AMP_SPEED, Settings.Launcher.AMP_THRESHOLD_RPM).andThen(new LauncherLaunch(Settings.Launcher.FEEDER_AMP_SPEED, Settings.Launcher.LAUNCHER_AMP_SPEED)));
.whileTrue(new LaunchPrepare(Settings.Launcher.LAUNCHER_SPEAKER_SPEED, Settings.Launcher.SPEAKER_THRESHOLD_RPM).andThen(new LauncherLaunchSpeaker()));
}

private void configureOperatorBindings() {}
Expand All @@ -80,7 +92,7 @@ private void configureOperatorBindings() {}
/**************/

public void configureAutons() {
autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton());
autonChooser = AutoBuilder.buildAutoChooser();

SmartDashboard.putData("Autonomous", autonChooser);
}
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,10 @@ public interface Launcher {
double LAUNCHER_INTAKE_SPEED = -1;
double FEEDER_INTAKE_SPEED = -0.2;

SmartNumber AMP_THRESHOLD_RPM = new SmartNumber("Launcher/Amp/Threshold RPM", 4000);
SmartNumber AMP_THRESHOLD_RPM = new SmartNumber("Launcher/Amp/Threshold RPM", 2250);

SmartNumber LAUNCHER_AMP_SPEED = new SmartNumber("Launcher/Amp/Launcher Speed", 0.7);
SmartNumber FEEDER_AMP_SPEED = new SmartNumber("Launcher/Amp/Feeder Speed", 0.7);
SmartNumber LAUNCHER_AMP_SPEED = new SmartNumber("Launcher/Amp/Launcher Speed", 0.4);
SmartNumber FEEDER_AMP_SPEED = new SmartNumber("Launcher/Amp/Feeder Speed", 0.4);
}

public interface Alignment {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import com.stuypulse.stuylib.math.Angle;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,27 @@
import com.stuypulse.robot.constants.Ports;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.stuylib.math.Angle;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.util.ReplanningConfig;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Drivetrain extends AbstractDrivetrain {

private final DifferentialDrive drivetrain;

private final DifferentialDriveKinematics kinematics;
private final DifferentialDriveOdometry odometry;

private final CANSparkMax leftFront;
private final CANSparkMax leftBack;
private final CANSparkMax rightFront;
Expand All @@ -36,6 +50,25 @@ public Drivetrain() {
Motors.Drivetrain.RIGHT.configure(rightBack);

drivetrain = new DifferentialDrive(leftFront, rightFront);

kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
odometry = new DifferentialDriveOdometry(new Rotation2d(), getLeftDistance(), getRightDistance());

AutoBuilder.configureRamsete(
this::getPose,
this::resetPose,
this::getCurrentSpeeds,
this::drive,
new ReplanningConfig(),
() -> {
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this
);
}

//********** GETTERS **********//
Expand Down Expand Up @@ -74,8 +107,28 @@ public double getDistance() {
public Angle getAngle() {
return Angle.fromDegrees(Math.toDegrees(getLeftDistance() - getRightDistance() / Settings.Drivetrain.TRACK_WIDTH));
}

//********** Drive Methods **********//

private DifferentialDriveWheelPositions getWheelPositions() {
return new DifferentialDriveWheelPositions(getLeftDistance(), getRightDistance());
}

public Pose2d getPose() {
return odometry.getPoseMeters();
}

public void resetPose(Pose2d pose) {
odometry.resetPosition(new Rotation2d(), getWheelPositions(), pose);
}

private double getAngularVelocity() {
return (getLeftSpeed() - getRightSpeed()) / Units.inchesToMeters(26);
}

public ChassisSpeeds getCurrentSpeeds() {
return new ChassisSpeeds(getVelocity(), 0, getAngularVelocity());
}

//********** Drive Methods **********//
public void tankDriveVolts(double leftVolts, double rightVolts) {
leftFront.setVoltage(leftVolts);
rightFront.setVoltage(rightVolts);
Expand All @@ -94,6 +147,10 @@ public void curvatureDrive(double speed, double rotation, boolean isQuickTurn) {
drivetrain.curvatureDrive(speed, rotation, isQuickTurn);
}

public void drive(ChassisSpeeds speeds) {
drivetrain.arcadeDrive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
}

public CANSparkMax[] getMotors() {
return new CANSparkMax[] {
rightFront, leftFront, rightBack, leftBack
Expand Down Expand Up @@ -122,6 +179,8 @@ public void stop() {

@Override
public void periodicChild() {
odometry.update(new Rotation2d(), getWheelPositions());

SmartDashboard.putNumber("Drivetrain/Left Speed", getLeftSpeed());
SmartDashboard.putNumber("Drivetrain/Right Speed", getRightSpeed());

Expand Down

0 comments on commit 43405bb

Please sign in to comment.