Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto climb, auto aim, shooter tuned, intake rotation tuned

Browse Source
This commit is contained in:
Mehooliu
2026-03-05 10:09:30 -05:00
parent 420fd90b12
commit cc41ea3f1b
18 changed files with 369 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/main/deploy/pathplanner/autos/HP Auto + Climb.auto
View File

@@ -19,7 +19,7 @@
{
"type": "wait",
"data": {
"waitTime": 3.0
"waitTime": 1.5
}
},
{
@@ -34,6 +34,12 @@
"name": "Shoot_Fuel_Command"
}
},
{
"type": "named",
"data": {
"name": "Assist_Shooter"
}
},
{
"type": "wait",
"data": {
@@ -51,6 +57,24 @@
"data": {
"pathName": "Hub to side of Climb"
}
},
{
"type": "path",
"data": {
"pathName": "Side into Climb"
}
},
{
"type": "wait",
"data": {
"waitTime": 0.5
}
},
{
"type": "named",
"data": {
"name": "Lift_Robot"
}
}
]
}

2
src/main/deploy/pathplanner/navgrid.json
View File

File diff suppressed because one or more lines are too long

2
src/main/deploy/pathplanner/paths/HP to Hub.path
View File

@@ -45,7 +45,7 @@
"rotation": 38.99099404250546
},
"reversed": false,
"folder": null,
"folder": "Start to HP to Hub to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0

54
src/main/deploy/pathplanner/paths/Hub to Shoot.path Normal file
View File

@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.5185568181818176,
"y": 4.024693181818182
},
"prevControl": null,
"nextControl": {
"x": 2.879534090909091,
"y": 4.024693181818182
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.281738636363636,
"y": 4.024693181818182
},
"prevControl": {
"x": 2.8279999999999994,
"y": 4.024693181818182
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": "Middle Shoot to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

14
src/main/deploy/pathplanner/paths/Hub to side of Climb.path
View File

@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 0.8,
"y": 2.0
"x": 1.08,
"y": 2.0457840909090907
},
"prevControl": {
"x": 1.2698463103929543,
"y": 2.1710100716628347
"x": 1.5498463103929543,
"y": 2.2167941625719254
},
"nextControl": null,
"isLocked": false,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -80.0
"rotation": -99.66394495351781
},
"reversed": false,
"folder": null,
"folder": "Start to HP to Hub to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 45.0
"rotation": 22.013232308372665
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/Side into Climb.path Normal file
View File

@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.08,
"y": 2.046
},
"prevControl": null,
"nextControl": {
"x": 1.0861477272727271,
"y": 2.5817386363636365
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.25,
"y": 3.5
},
"prevControl": {
"x": 1.256147727272727,
"y": 3.190795454545455
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 0.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -99.664
},
"reversed": false,
"folder": "Start to HP to Hub to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": -99.664
},
"useDefaultConstraints": false
}

10
src/main/deploy/pathplanner/paths/Starting to HP.path
View File

@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 0.5,
"y": 0.65
"x": 0.4471249999999998,
"y": 0.63375
},
"prevControl": {
"x": 1.0519022424324624,
"y": 0.6499999999999999
"x": 0.999027242432462,
"y": 0.6337499999999999
},
"nextControl": null,
"isLocked": false,
@@ -45,7 +45,7 @@
"rotation": 180.0
},
"reversed": false,
"folder": null,
"folder": "Start to HP to Hub to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0

20
src/main/deploy/pathplanner/paths/test_1_Meter.path
View File

@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 1.0,
"y": 7.0
"x": 15.474,
"y": 3.437204545454546
},
"prevControl": null,
"nextControl": {
"x": 2.0,
"y": 7.0
"x": 16.473999999999986,
"y": 3.437204545454546
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.0,
"y": 7.0
"x": 1.066,
"y": 1.87
},
"prevControl": {
"x": 0.8572206587126847,
"y": 7.0
"x": -0.07677934128731523,
"y": 1.87
},
"nextControl": null,
"isLocked": false,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
"rotation": 90.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
"rotation": 90.0
},
"useDefaultConstraints": false
}

5
src/main/deploy/pathplanner/settings.json
View File

@@ -2,7 +2,10 @@
"robotWidth": 0.9,
"robotLength": 0.9,
"holonomicMode": true,
"pathFolders": [],
"pathFolders": [
"Middle Shoot to Climb",
"Start to HP to Hub to Climb"
],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,

2
src/main/deploy/swerve/neo/modules/physicalproperties.json
View File

@@ -1,6 +1,6 @@
{
"optimalVoltage": 12,
"robotMass": 140,
"robotMass": 51.75,
"wheelGripCoefficientOfFriction": 1.85,
"currentLimit": {
"drive": 40,

58
src/main/java/frc/robot/Constants.java
View File

@@ -38,10 +38,10 @@ public final class Constants {
private static ShuffleboardTab programmingTab = Shuffleboard.getTab("Programming");
public static final double ROBOT_MASS = 140 * 0.453592; // 32lbs * kg per pound
public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound
public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag
public static final double MAX_SPEED = Units.feetToMeters(25);
public static final double MAX_SPEED = Units.feetToMeters(14.5);
// Maximum speed of the robot in meters per second, used to limit acceleration.
// public static final class AutonConstants
@@ -68,14 +68,14 @@ public final class Constants {
}
public static class ShooterConstants {
private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
// private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000)
// .withWidget(BuiltInWidgets.kNumberBar).getEntry();
public static double SHOOTER_RPM;
public static double SHOOTER_RPM = -2700;
public static void updateShooterRPM() {
SHOOTER_RPM = shooterRPM.getDouble(-4000);
}
// public static void updateShooterRPM() {
// SHOOTER_RPM = shooterRPM.getDouble(-4000);
// }
public static final int CENTER_SHOOTER_MOTOR_ID = 42;
public static final int LEFT_SHOOTER_MOTOR_ID = 41;
@@ -128,31 +128,45 @@ public final class Constants {
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
public static class IntakeRotatorPID {
public static final double INTAKE_ROTATOR_P = .5;
public static final double INTAKE_ROTATOR_I = 0.01;
public static final double INTAKE_ROTATOR_D = 0;
public static final double INTAKE_ROTATOR_P = .1;
public static final double INTAKE_ROTATOR_I = 0.0;
public static final double INTAKE_ROTATOR_D = 0.;
}
public static final double INTAKE_MOTOR_P = 0.0001;
public static final double INTAKE_MOTOR_I = 0;
public static final double INTAKE_MOTOR_D = 0;
public static final double INTAKE_COLLECT_ENCODER_VALUE = 0;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5;
public static final double INTAKE_RETRACT_ENCODER_VALUE = -5;
public static final double INTAKE_COLLECT_ENCODER_VALUE = -0.2;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3;
public static final double INTAKE_RETRACT_ENCODER_VALUE = -5.5;
public static final double INTAKE_THROUGHBORE_ENCODER_DEPLOY = .13;
public static final double INTAKE_THROUGHBORE_ENCODER_RETRACT = .49;
public static final double INTAKE_THROUGHBORE_ENCODER_MIDDLE = .389;
public static final double INTAKE_MANUAL_SPEED = .1;
}
// create object and a new widget under programming tab in Elastic where object
// retrieves value from widget
public static class TargetingConstants {
public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
public static final Pose2d BLUE_LEFT_CLIMB_POSE = new Pose2d(1.066, 4.633, Rotation2d.fromDegrees(90));
public static final Pose2d BLUE_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(1.066, 5.133,
Rotation2d.fromDegrees(90));
public static final Pose2d BLUE_RIGHT_CLIMB_POSE = new Pose2d(1.066, 2.87, Rotation2d.fromDegrees(-90));
public static final Pose2d BLUE_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(1.066, 2.37,
Rotation2d.fromDegrees(-90));
public static final Pose2d RED_RIGHT_CLIMB_POSE = new Pose2d(15.474, 5.22, Rotation2d.fromDegrees(90));
public static final Pose2d RED_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(15.474, 5.72,
Rotation2d.fromDegrees(90));
public static final Pose2d RED_LEFT_CLIMB_POSE = new Pose2d(15.474, 3.437, Rotation2d.fromDegrees(-90));
public static final Pose2d RED_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(15.474, 2.937,
Rotation2d.fromDegrees(-90));
public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Back Left Camera");
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Back Right Camera");
@@ -165,16 +179,17 @@ public final class Constants {
public static final double HUB_X_POSE_BLUE = 4.625;
public static final double HUB_Y_POSE_BLUE = 4.03;
public static final double HUB_X_POSE_RED = 4.625;
public static final double HUB_Y_POSE_RED = 0;
public static final double HUB_X_POSE_RED = 11.915;
public static final double HUB_Y_POSE_RED = 4.03;
public static final Transform3d ROBOT_TO_BACK_LEFT_CAM = new Transform3d(
new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25), Units.inchesToMeters(13.1875)),
new Rotation3d(0, 0, Math.toRadians(-205)));
new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25),
Units.inchesToMeters(13.1875)),
new Rotation3d(0, 0, Math.toRadians(205)));
public static final Transform3d ROBOT_TO_BACK_RIGHT_CAM = new Transform3d(
new Translation3d(-Units.inchesToMeters(12.75), -Units.inchesToMeters(6.25), Units.inchesToMeters(14)),
new Rotation3d(0, 0, Math.toRadians(200)));
new Rotation3d(0, 0, Math.toRadians(-200)));
public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d(
new Translation3d(Units.inchesToMeters(23.375), 0, Units.inchesToMeters(13)),
@@ -202,6 +217,5 @@ public final class Constants {
public static class LEDConstants {
public static final int LED_PWM_PORT = 6;
}
}

12
src/main/java/frc/robot/Robot.java
View File

@@ -4,13 +4,17 @@
package frc.robot;
import java.util.logging.Logger;
import org.photonvision.targeting.PhotonPipelineResult;
import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
@@ -44,6 +48,7 @@ public class Robot extends TimedRobot {
private static NetworkTable table;
private static GenericEntry distanceFromLimelight;
private static StructPublisher<Pose2d> advantageScopePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose hahaha", Pose2d.struct).publish();
public Robot() {
instance = this;
@@ -99,13 +104,16 @@ public class Robot extends TimedRobot {
// robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
TargetingSubsystems.updateShooterRPM(m_robotContainer.getSwerveDrive().getPose());
SmartDashboard.putNumber("Estimated Shooter RPM", Constants.ShooterConstants.SHOOTER_RPM);
//Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
//Constants.IntakeConstants.updateIntakeWheelsRPM();
Constants.ShooterConstants.updateShooterRPM();
//Constants.ShooterConstants.updateShooterRPM();
TargetingSubsystems.getHubPoseTheta(m_robotContainer.getSwerveDrive());
//Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
advantageScopePublisher.set(m_robotContainer.getSwerveDrive().getPose());
}
/**

51
src/main/java/frc/robot/RobotContainer.java
View File

@@ -4,6 +4,7 @@
package frc.robot;
import com.fasterxml.jackson.databind.util.Named;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
@@ -22,7 +23,9 @@ import edu.wpi.first.wpilibj.XboxController;
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.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.OperatorConstants;
@@ -56,6 +59,8 @@ public class RobotContainer {
// Replace with CommandPS4Controller or CommandJoystick if needed
final CommandXboxController driverXbox = new CommandXboxController(0);
final CommandXboxController operatorXbox = new CommandXboxController(1);
private final static CommandJoystick topButtons = new CommandJoystick(2);
// The robot's subsystems and commands are defined here...
private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
@@ -66,8 +71,8 @@ public class RobotContainer {
private final SendableChooser<Command> autoChooser;
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
// private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
private final TargetingSubsystems m_TargetingSubsystems = new
TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
/**
@@ -139,16 +144,13 @@ public class RobotContainer {
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
NamedCommands.registerCommand("Shoot_Fuel_Command",
m_ShooterSubsystem.shootFuelCommand()
.andThen(m_IntakeSubsystem.startIntakeMotorCommand())
.andThen(m_IntakeSubsystem.assistFuelIntakeCommand(
Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE)
.repeatedly()
.withTimeout(2)));
NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem
.goToPositionCommand(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE));
.andThen(m_IntakeSubsystem.startIntakeMotorCommand()));
NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand());
NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand());
NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand());
NamedCommands.registerCommand("Assist_Shooter", m_IntakeSubsystem.assistFuelIntakeCommand());
NamedCommands.registerCommand("Lift_Robot", m_ClimberSubsystem.liftRobotCommand());
// Have the autoChooser pull in all PathPlanner autos as options
autoChooser = AutoBuilder.buildAutoChooser();
@@ -197,18 +199,18 @@ public class RobotContainer {
// command for
// full shooting system including linear actuators
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly());
driverXbox.rightBumper().whileTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
//driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly());
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand());
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand());
driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));
driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT));
//driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));
//driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT));
//driverXbox.povLeft().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE));
//driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand());
//driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand());
driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand());
driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand());
//driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
@@ -228,6 +230,15 @@ public class RobotContainer {
operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
.onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
topButtons.axisGreaterThan(1, 0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -1)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
topButtons.axisGreaterThan(1, -0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
topButtons.button(3).onTrue(killAllCommand());
topButtons.button(6).whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
topButtons.button(1).onTrue(drivebase.driveToClimbPose(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
topButtons.button(2).onTrue(drivebase.driveToClimbPose(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
//topButtons.button(1).onTrue(drivebase.driveToPose(Constants.))
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
@@ -302,4 +313,12 @@ public class RobotContainer {
return drivebase;
}
public void killAll() {
CommandScheduler.getInstance().cancelAll();
}
public Command killAllCommand(){
return Commands.runOnce(()->killAll());
}
}

54
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@@ -49,18 +49,28 @@ public class IntakeSubsystem extends SubsystemBase {
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D, m_Constraints);
intakeRotatorProfiledPIDController.setTolerance(0.05);
//intakeRotatorProfiledPIDController.setGoal(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT);
intakeRotatorConfig.closedLoop
//Slot 0
.p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P)
.i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
.d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
.p(.13, ClosedLoopSlot.kSlot1)
//Slot 1
.p(.05, ClosedLoopSlot.kSlot1)
.i(0, ClosedLoopSlot.kSlot1)
.d(0, ClosedLoopSlot.kSlot1);
.d(.05, ClosedLoopSlot.kSlot1)
//Slot 2
.p(1,ClosedLoopSlot.kSlot2)
.i(.001, ClosedLoopSlot.kSlot2)
.d(0.1, ClosedLoopSlot.kSlot2);
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorConfig.smartCurrentLimit(40);
//intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P,
@@ -68,14 +78,29 @@ public class IntakeSubsystem extends SubsystemBase {
Constants.IntakeConstants.INTAKE_MOTOR_D);
intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorMotor.getEncoder().setPosition(-5);
intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
}
public void goToPosition(double goalPosition) {
double pidVal = intakeRotatorProfiledPIDController.calculate(encoderValue, goalPosition);
intakeRotatorMotor.setVoltage(-pidVal * 12);
/* public void goToPosition(double goalPosition) {
intakeRotatorProfiledPIDController.setGoal(goalPosition);
//double pidVal = intakeRotatorProfiledPIDController.calculate(encoderValue, goalPosition);
}
public Command goToPositionCommand(double goalPosition) {
return runOnce(() -> goToPosition(goalPosition));
}
*/
public void rotateIntake(double speed)
{
intakeRotatorMotor.set(speed);
}
public Command rotateIntakeCommand(double speed)
{
return runOnce(()->intakeRotatorMotor.set(speed));
}
public void startIntakeMotor() {
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM,
@@ -103,14 +128,10 @@ public class IntakeSubsystem extends SubsystemBase {
return runOnce(() -> stopIntakeMotor());
}
public Command goToPositionCommand(double goalPosition) {
return runOnce(() -> goToPosition(goalPosition));
}
public Command assistFuelIntakeCommand(double deployedPosition, double assistPosition) {
/* public Command assistFuelIntakeCommand(double deployedPosition, double assistPosition) {
return runOnce(() -> goToPositionCommand(assistPosition).andThen(new WaitCommand(1.5))
.andThen(goToPositionCommand(deployedPosition)).andThen(new WaitCommand(1.5)));
}
} */
public static void resetIntakeRotationEncoder() {
intakeRotatorMotor.getEncoder().setPosition(-5);
@@ -137,19 +158,20 @@ public class IntakeSubsystem extends SubsystemBase {
public void assistFuelIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.
INTAKE_MIDDLE_ENCODER_VALUE,
ControlType.kPosition, ClosedLoopSlot.kSlot1);
ControlType.kPosition, ClosedLoopSlot.kSlot2);
}
public Command assistFuelIntakeCommand() {
return runOnce(() -> assistFuelIntake()).andThen(new
WaitCommand(1.5)).andThen(deployIntakeCommand())
.andThen(new WaitCommand(1.5));
WaitCommand(1)).andThen(deployIntakeCommand())
.andThen(new WaitCommand(1));
}
@Override
public void periodic() {
encoderValue = intakeRotatorEncoder.get();
//encoderValue = intakeRotatorEncoder.get();
//intakeRotatorMotor.setVoltage(intakeRotatorProfiledPIDController.calculate(encoderValue) * -12);
SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
SmartDashboard.putNumber("Intake Rotator Encoder Value", intakeRotatorEncoder.get());
SmartDashboard.putNumber("Voltage Deploy", intakeRotatorProfiledPIDController.calculate(encoderValue, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));

12
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View File

@@ -205,18 +205,18 @@ public class ShooterSubsystem extends SubsystemBase {
}*/
//public Command shootFuelCommand() {
// return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
// .andThen(() -> setIndexerAndRampMotorRPM());
// };
public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
.andThen(() -> setIndexerAndRampMotorRPM());
};
public Command shootFuelCommand() {
/* public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM())
.andThen(new WaitUntilCommand(() -> {
return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM);
}))
.andThen(() -> setIndexerAndRampMotorRPM());
};
};*/
public void stopShooters() {

72
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
View File

@@ -53,15 +53,21 @@ import frc.robot.Constants;
public class TargetingSubsystems extends SubsystemBase {
PIDController photonAimPIDController = new PIDController(3, 0, 0.001);
PIDController photonAimPIDController = new PIDController(3, 0.01, 0);
static Pose2d allianceHubPose;
static Pose2d allianceHubPose = new Pose2d();
public static Rotation2d hubThetaPose = new Rotation2d();
public static Optional<Alliance> alliance = DriverStation.getAlliance();
private static ShuffleboardTab cameras;
public TargetingSubsystems() {
photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI);
cameras = Shuffleboard.getTab("Vision");
// cameras.addCamera("Rear Left Camera","Rear Left
// Camera","http://photonvision.local:5800/#/cameras");
// cameras.addCamera("Rear Right Camera", "Rear Right Camera",
// "http://photonvision.local:5800/#/cameras");
}
Pose2d currentRobotPose;
@@ -108,16 +114,12 @@ public class TargetingSubsystems extends SubsystemBase {
return new RunCommand(() -> {
currentRobotPose = swerveDrive.getPose();
// Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose,
allianceHubPose);
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
angleDifference.getRadians());
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), allianceHubPose.getRotation().getRadians());
angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0);
@@ -142,29 +144,48 @@ public class TargetingSubsystems extends SubsystemBase {
}, swerveDrive);
}
public static void getHubPoseTheta(SwerveSubsystem swerveDrive) {
if (alliance.isPresent()) {
if (alliance.get() == Alliance.Blue) {
hubThetaPose = new Rotation2d(
Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX()));
public static void getHubPoseTheta(SwerveSubsystem swerveDrive)
{
if(alliance.isPresent()){
if (alliance.get() == Alliance.Blue){
hubThetaPose = new Rotation2d(Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX()));
allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE, Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose);
allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE,
Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose);
}
else{
hubThetaPose = new Rotation2d(Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX()));
allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED, Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose);
else {
hubThetaPose = new Rotation2d(
Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(),
Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX()));
allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED,
Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose);
}
}
}
}
public static void updateShooterRPM(Pose2d currentRobotPose) {
double distance = PhotonUtils.getDistanceToPose(currentRobotPose,
allianceHubPose);
Constants.ShooterConstants.SHOOTER_RPM = -300 * Math.pow(distance, 3)
+ 1221.475 * Math.pow(distance, 2)
- 1955.00131 * distance
- 1630.07168;
}
@Override
public void periodic() {
SmartDashboard.putString("Target Hub Pose", allianceHubPose.getX() + "\n" + allianceHubPose.getY() + "\n" + allianceHubPose.getRotation()) ;
alliance = DriverStation.getAlliance();
SmartDashboard.putString("Target Hub Pose",
allianceHubPose.getX() + " " + allianceHubPose.getY() + " " + allianceHubPose.getRotation());
SmartDashboard.putString("Hub Pose", "x: " + allianceHubPose.getMeasureX() + " y: " + allianceHubPose.getY()
+ " angle: " + allianceHubPose.getRotation());
/*
* Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
* photonVision.getLatestResult().getBestTarget().getYaw());
@@ -174,10 +195,7 @@ public class TargetingSubsystems extends SubsystemBase {
* LimelightHelpers.getTX("limelight"));
* Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
* LimelightHelpers.getFiducialID("limelight"));
* Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null);
* Shuffleboard.getTab("Vision").addCamera("Photon",
* "Arducam_OV9281_USB_Camera",
* "http://photonvision.local:5800");
*/
}
}

33
src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View File

@@ -27,6 +27,9 @@ import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -36,6 +39,7 @@ import frc.robot.subsystems.TargetingSubsystems;
import frc.robot.subsystems.swervedrive.Vision.Cameras;
import java.io.File;
import java.io.IOException;
import java.lang.reflect.Field;
import java.util.Arrays;
import java.util.Optional;
import java.util.concurrent.atomic.AtomicReference;
@@ -70,6 +74,7 @@ public class SwerveSubsystem extends SubsystemBase {
*/
private Vision vision;
private final Field2d rebuiltField = new Field2d();
/**
* Initialize {@link SwerveDrive} with the directory provided.
*
@@ -118,6 +123,7 @@ public class SwerveSubsystem extends SubsystemBase {
swerveDrive.stopOdometryThread();
}
setupPathPlanner();
SmartDashboard.putData("Rebuilt Field", rebuiltField);
}
/**
@@ -149,6 +155,11 @@ public class SwerveSubsystem extends SubsystemBase {
swerveDrive.updateOdometry();
vision.updatePoseEstimation(swerveDrive);
}
SmartDashboard.putString("Robot Pose ", "x: " + swerveDrive.getPose().getX() + "\ny: "
+ swerveDrive.getPose().getY() + "\nrot: " + swerveDrive.getPose().getRotation());
rebuiltField.setRobotPose(getPose());
}
@Override
@@ -275,6 +286,28 @@ public class SwerveSubsystem extends SubsystemBase {
);
}
public Command driveToClimbPose(Pose2d blueAlliancePose, Pose2d redAlliancePose) {
Pose2d goal;
Optional<Alliance> alliance = DriverStation.getAlliance();
if (alliance.get() == Alliance.Blue) {
goal = blueAlliancePose;
} else {
goal = redAlliancePose;
}
// Create the constraints to use while pathfinding
PathConstraints constraints = new PathConstraints(
swerveDrive.getMaximumChassisVelocity(), 4.0,
swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720));
// Since AutoBuilder is configured, we can use it to build pathfinding commands
return AutoBuilder.pathfindToPose(
goal,
constraints,
edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
);
}
/**
* Drive with {@link SwerveSetpointGenerator} from 254, implemented by
* PathPlanner.

7
src/main/java/frc/robot/subsystems/swervedrive/Vision.java
View File

@@ -319,14 +319,15 @@ public class Vision {
BACK_RIGHT_CAM("Rear Right Camera",
Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getRotation(),
Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getTranslation(),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
/**
* Front Left Camera
*/
FRONT_LEFT_CAM("Front Left Camera",
/*FRONT_LEFT_CAM("Front Left Camera",
Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getRotation(),
Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getTranslation(),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));*/
/**
* Front Right Camera