Skip to content

Commit

Permalink
[#533] More cleaning, Mostly constants
Browse files Browse the repository at this point in the history
  • Loading branch information
@MattD8957
MattD8957 committed Apr 12, 2024
1 parent 9b5e6ab commit d9bec8f
Show file tree
Hide file tree
Showing 13 changed files with 121 additions and 110 deletions.
25 changes: 25 additions & 0 deletions src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,25 @@
]
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{
"type": "named",
"data": {
"name": "Stop-Flywheel"
}
}
]
}
},
{
"type": "deadline",
"data": {
Expand All @@ -39,6 +58,12 @@
"data": {
"name": "Smart-Collect"
}
},
{
"type": "named",
"data": {
"name": "Stop-Flywheel"
}
}
]
}
Expand Down
110 changes: 53 additions & 57 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -36,16 +35,16 @@ public class Constants {

public static final String HOOT_PATH = "U/logs"; // "/home/lvuser/logs";

public class DrivetrainConstants { // TODO Get new for new robot
public static final double MaxSpeed = 6; // 6 meters per second desired top speed
public class DrivetrainConstants {
public static final double MaxSpeed = Units.feetToMeters(16.5); // 16.5 ft/s to meters per second top speed (5.0292m/s)
private static final double WHEELBASE = TunerConstants.kFrontLeftXPosInches * 2; // 2 * x distance from center of robot to wheel
public static final double MaxAngularRate = 2 * Math.PI * ( // convert to radians per second
TunerConstants.kSpeedAt12VoltsMps / Math.PI * Math.sqrt(2 * Math.pow(WHEELBASE, 2))); // free speed / circumference of circle with radius of wheelbase

public static final double ROT_MULT = 0.04; // TODO Tune for Driver

public static final double SLOW_ROT_MULT = 0.3; // TODO Tune for Driver
public static final double SLOW_SPEED_MULT = 0.4; // TODO Tune for Driver
public static final double SLOW_ROT_MULT = 0.3;
public static final double SLOW_SPEED_MULT = 0.4;

public static final double SYS_TEST_SPEED_DRIVE = 0.5;
public static final double SYS_TEST_SPEED_TURN = 0.7d;
Expand Down Expand Up @@ -79,7 +78,6 @@ public class CAN {

public static final int PigeonId = 23;

// TODO check
public static final int COLLECTOR_MOTOR = 9;
public static final int INDEXER_MOTOR = 10;
public static final int PIVOT_ANGLE_MOTOR = 11;
Expand All @@ -88,11 +86,7 @@ public class CAN {
public static final int CLIMB_RIGHT = 14;
public static final int CLIMB_LEFT = 15;

// TODO check
// Cancoders
public static final int FLYWHEEL_CANCODER = 35;
public static final int CLIMB_CANCODERR = 36;
public static final int CLIMB_CANCODERL = 37;
public static final int PIVOT_ANGLE_CANCODER = 35;

public static final String CANBUS_FD = "Canivore";
Expand Down Expand Up @@ -146,26 +140,23 @@ public static class AutonomousConstants {

public static final double CONTROL_LOOP_PERIOD = 0.02;

public static final ReplanningConfig REPLANNING_CONFIG = new ReplanningConfig(true, false); // TODO Should we enable dynamic replaning
public static final ReplanningConfig REPLANNING_CONFIG = new ReplanningConfig(true, false); // Expirement with dynamic replaning in offseason
public static final PathConstraints PATHFINDING_CONSTRAINTS = new PathConstraints(2.0, 1.0, 3.0, 1.5);
public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get constants
public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5);

public static final Pose2d AMP_LOCATION_RED = new Pose2d(new Translation2d(14.4, 7.62), new Rotation2d(90));
}

public static class ChaseConstants {
public static final double BLUE_CHASE_BOUNDARY = 8.35; // The highest X value the robot can be at before ending. Prevents going over center line.
public static final double RED_CHASE_BOUNDARY = 8.25;

public static final double BLUE_SLOW_CHASE_RANGE = 8.05;
public static final double RED_SLOW_CHASE_RANGE = 8.55;
public static final double CHASE_BOUNDARY = 8.3; // The highest X value the robot can be at before ending. Prevents going over center line.

public static final Pose2d SOURCE_SUB_A_STARTPOSE_BLUE = new Pose2d(new Translation2d(0.72, 6.69), new Rotation2d(60));
public static final Pose2d SOURCE_SUB_B_STARTPOSE_BLUE = new Pose2d(new Translation2d(1.34, 5.55), new Rotation2d(0));
public static final Pose2d SOURCE_SUB_C_STARTPOSE_BLUE = new Pose2d(new Translation2d(0.72, 4.39), new Rotation2d(-60));

public static final Pose2d SOURCE_SUB_A_STARTPOSE_RED = new Pose2d(new Translation2d(15.85, 6.70), new Rotation2d(120));
public static final Pose2d SOURCE_SUB_B_STARTPOSE_RED = new Pose2d(new Translation2d(15.2, 5.55), new Rotation2d(180));
public static final Pose2d SOURCE_SUB_C_STARTPOSE_RED = new Pose2d(new Translation2d(15.85, 4.35), new Rotation2d(-120));

public static final Pose2d AMP_LOCATION_RED = new Pose2d(new Translation2d(14.4, 7.62), new Rotation2d(90));
public static final PIDController CHASE_CONTROLLER = new PIDController(0.05, 0, 0);
public static final double CHASE_PIECE_ALIGNMENT_TOLERANCE = 3d;
}

public static class TunerConstants {
Expand Down Expand Up @@ -275,22 +266,13 @@ public static final Swerve getDrivetrain() {
}

public class VisionConstants {
// This is a magic number from gridlock, may need to be changed or removed
// entirely
public static final double PROCESS_LATENCY = 0.0472; // TODO test
public static final Translation2d FIELD_LIMIT = new Translation2d(Units.feetToMeters(54.0), Units.feetToMeters(26.0));
public static final Translation2d VISION_LIMIT = new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5));
public static final double CHASE_PIECE_ALIGNMENT_TOLERANCE = 3d;
// This is a magic number from LL2+, may need to be changed or removed entirely
public static final double PROCESS_LATENCY = 0.0472; // TODO test this value

public static final double POINTATTAG_ALIGNMENT_TOLERANCE = 1d;
public static final PIDController POINT_AIM_CONTROLLER = new PIDController(0.2, 0, 0.015, 0.01);
public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.1, 0, 0.01);
public static final PIDController COMBO_CONTROLLER = new PIDController(0.1, 0, 0.01);
public static final PIDController CHASE_CONTROLLER = new PIDController(0.05, 0, 0);

public static final double HALF_FIELD_HEIGHT = Units.feetToMeters(13);

public static final Translation3d BLUE_SPEAKER_LOCATION = new Translation3d(0, 5.547593, 1.2);
public static final Translation3d RED_SPEAKER_LOCATION = new Translation3d(16.4592, 5.547593, 1.2);

public static final int[] SPEAKER_FILTERS = {4, 7};
public static final int[] ALL_TAG_FILTERS = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16};
Expand All @@ -309,6 +291,36 @@ public class Pipelines {
}
}

public class PoseConstants {
public static class StartingPoseConstants {
public static final Pose2d SOURCE_SUB_A_STARTPOSE_BLUE = new Pose2d(new Translation2d(0.72, 6.69), new Rotation2d(60));
public static final Pose2d SOURCE_SUB_B_STARTPOSE_BLUE = new Pose2d(new Translation2d(1.34, 5.55), new Rotation2d(0));
public static final Pose2d SOURCE_SUB_C_STARTPOSE_BLUE = new Pose2d(new Translation2d(0.72, 4.39), new Rotation2d(-60));

public static final Pose2d SOURCE_SUB_A_STARTPOSE_RED = new Pose2d(new Translation2d(15.85, 6.70), new Rotation2d(120));
public static final Pose2d SOURCE_SUB_B_STARTPOSE_RED = new Pose2d(new Translation2d(15.2, 5.55), new Rotation2d(180));
public static final Pose2d SOURCE_SUB_C_STARTPOSE_RED = new Pose2d(new Translation2d(15.85, 4.35), new Rotation2d(-120));
}

public static class PathfindPoseConstants {
public static final Pose2d PATHFIND_CENTER_STAGE_START_POSE_BLUE = new Pose2d(7.43, 4.16, new Rotation2d(0));
public static final Pose2d PATHFIND_HIGH_STAGE_START_POSE_BLUE = new Pose2d(3.55, 6.16, new Rotation2d(Units.degreesToRadians(-53.13)));
public static final Pose2d PATHFIND_LOW_STAGE_START_POSE_BLUE = new Pose2d(3.54, 1.91, new Rotation2d(Units.degreesToRadians(63.43)));

public static final Pose2d PATHFIND_CENTER_STAGE_START_POSE_RED = new Pose2d(9.07, 4.16, new Rotation2d(0));
public static final Pose2d PATHFIND_HIGH_STAGE_START_POSE_RED = new Pose2d(12.96, 6.16, new Rotation2d(Units.degreesToRadians(-53.13)));
public static final Pose2d PATHFIND_LOW_STAGE_START_POSE_RED = new Pose2d(12.95, 1.91, new Rotation2d(Units.degreesToRadians(63.43)));
}

public static final double FAR_WING_X = 3.3;

public static final Translation3d BLUE_SPEAKER_LOCATION = new Translation3d(0, 5.547593, 1.2);
public static final Translation3d RED_SPEAKER_LOCATION = new Translation3d(16.4592, 5.547593, 1.2);

public static final double HALF_FIELD_HEIGHT = Units.feetToMeters(13);
public static final Translation2d FIELD_LIMIT = new Translation2d(Units.feetToMeters(54.0), Units.feetToMeters(26.0));
}

public class PathFindingConstants {
public static final Pose2d TEST_POSE = new Pose2d(9, 4, new Rotation2d(90));
public static final Translation2d RED_ORIGIN = new Translation2d(16.4592, 0);
Expand Down Expand Up @@ -410,7 +422,7 @@ public class FlywheelConstants {
public static final double FLYWHEEL_SYSTEST_RPM = 1000;
}

public class IndexerConstants { // TODO: get real
public class IndexerConstants {
public static final boolean MOTOR_INVERT = true;
public static final int MOTOR_STATOR_CURRENT_LIMIT = 100;

Expand All @@ -419,10 +431,10 @@ public enum PieceState {
}

public static final boolean INDEXER_MOTOR_BRAKE_MODE = true;

public static final double INDEXER_DEFAULT_POWER = 1d;
public static final double INDEXER_MANUAL_POWER = 1d;
public static final double INDEXER_DEBOUNCE_TIME = 0.1d;

public static final double INDEXER_SYSTEST_POWER = 0.25d;
}

Expand Down Expand Up @@ -473,12 +485,7 @@ public class RhapsodyPivotConstants {
public static final double MOTOR_KS = 50d;
public static final double MOTOR_KA = 0d;

// Not currently using Motion magic
public static final double MAGIC_CRUISE_VEL = 0.01;
public static final double MAGIC_ACCEL = 0.02;
public static final double MAGIC_JERK = 0.2;

public static final double ANGLE_TOLERANCE = 0.00208d;
public static final double ANGLE_TOLERANCE = 0.00208d; // .75 degrees

public static final double ENCODER_OFFSET = -0.913834; // In rotations
public static final SensorDirectionValue ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive;
Expand All @@ -491,16 +498,14 @@ public class RhapsodyPivotConstants {

public static final double MAX_INDEX_ANGLE = 40d;

public static final double MIN_ANGLE = 25d;
public static final double MIN_ANGLE = 25d; // TODO get new value
public static final double MAX_ANGLE = 105d;

public static final double PIVOT_SYSTEST_ANGLE = 90d;
}

public class ShooterConstants {

public static final double FAR_WING_X = 3.3;

// Distance in meters, angle in degrees
public static final InterpolationMap TUBE_ANGLE_MAP = new InterpolationMap() {
{
Expand Down Expand Up @@ -566,9 +571,11 @@ public class ShooterConstants {
public enum ShootingState {
AIM, SHOOT, SHOT
}

public static final double TIME_TO_SHOOT = 1d; // Time in seconds it takes from indexer start to flywheel exit
}

public class CandConstants { // TODO get real
public class CandConstants {
// Amp
public static final double AMP_TOP_RPM = 325;
public static final double AMP_BOTTOM_RPM = 625;
Expand Down Expand Up @@ -609,12 +616,9 @@ public class CandConstants { // TODO get real
// Pass
public static final double NOTE_PASS_ANGLE = 55d;
public static final double NOTE_PASS_RPM = 4500d;

// TODO find time to shoot
public static final double TIME_TO_SHOOT = 1d; // Time in seconds it takes from indexer start to flywheel exit
}

public class ClimbConstants { // TODO: find real values
public class ClimbConstants {
public static final boolean CLIMB_RIGHT_MOTOR_INVERT = false;
public static final boolean CLIMB_LEFT_MOTOR_INVERT = true;
public static final int CLIMB_MOTOR_STATOR_CURRENT_LIMIT = 60;
Expand All @@ -640,14 +644,6 @@ public class ClimbConstants { // TODO: find real values
public static final double CLIMB_RETURN_TO_GROUND_MAX_POWER = 0.05;

public static final double CLIMB_SYSTEST_POWER = 0.1;

public static final Pose2d PATHFIND_CENTER_STAGE_START_POSE_BLUE = new Pose2d(7.43, 4.16, new Rotation2d(0));
public static final Pose2d PATHFIND_HIGH_STAGE_START_POSE_BLUE = new Pose2d(3.55, 6.16, new Rotation2d(Units.degreesToRadians(-53.13)));
public static final Pose2d PATHFIND_LOW_STAGE_START_POSE_BLUE = new Pose2d(3.54, 1.91, new Rotation2d(Units.degreesToRadians(63.43)));

public static final Pose2d PATHFIND_CENTER_STAGE_START_POSE_RED = new Pose2d(9.07, 4.16, new Rotation2d(0));
public static final Pose2d PATHFIND_HIGH_STAGE_START_POSE_RED = new Pose2d(12.96, 6.16, new Rotation2d(Units.degreesToRadians(-53.13)));
public static final Pose2d PATHFIND_LOW_STAGE_START_POSE_RED = new Pose2d(12.95, 1.91, new Rotation2d(Units.degreesToRadians(63.43)));
}

public class LEDsConstants {
Expand Down
34 changes: 10 additions & 24 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
import frc.robot.Constants.IndexerConstants;
import frc.robot.Constants.PathFindingConstants;
import frc.robot.Constants.LEDsConstants.LED_STATES;
import frc.robot.Constants.PoseConstants.StartingPoseConstants;
import frc.robot.Constants.TunerConstants;
import frc.robot.command.AutonSmartCollect;
import frc.robot.command.ChasePieces;
Expand Down Expand Up @@ -100,7 +101,7 @@ public class RobotContainer extends LightningContainer {
Telemetry logger;

private Boolean triggerInit;
private static double bias = 0d;
public static double bias = 0d;

@Override
protected void initializeSubsystems() {
Expand Down Expand Up @@ -134,7 +135,7 @@ protected void initializeSubsystems() {
triggerInit = false;

boolean setPath = SignalLogger.setPath(Constants.HOOT_PATH).isOK();
SignalLogger.enableAutoLogging(true); // TODO Return during COMPS
SignalLogger.enableAutoLogging(true);
boolean startedLogs = SignalLogger.start().isOK();

if (startedLogs && setPath) {
Expand Down Expand Up @@ -329,22 +330,7 @@ protected void configureButtonBindings() {
// new Trigger(() -> LightningShuffleboard.getBool("Swerve", "Swap", false))
// .onTrue(new InstantCommand(() -> drivetrain.swap(driver, coPilot)))
// .onFalse(new InstantCommand(() -> drivetrain.swap(driver, coPilot)));
// BLUE Alliance set
// new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED A",
// false))
// .onTrue(new InstantCommand(
// () ->
// drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_A_STARTPOSE_RED)));
// new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED B",
// false))
// .onTrue(new InstantCommand(
// () ->
// drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_B_STARTPOSE_RED)));
// new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED C",
// false))
// .onTrue(new InstantCommand(
// () ->
// drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_C_STARTPOSE_RED)));


/* Button Box */
// new Trigger(() -> buttonBox.getRawButton(ButtonBox.PINK)).whileTrue(new
Expand All @@ -356,29 +342,29 @@ protected void configureButtonBindings() {
new Trigger(
() -> DriverStation.isDisabled() ? LightningShuffleboard.getBool("Auton", "POSE BLUE A", false) : false)
.onTrue(new InstantCommand(() -> drivetrain
.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_A_STARTPOSE_BLUE)));
.setDrivetrainPose(StartingPoseConstants.SOURCE_SUB_A_STARTPOSE_BLUE)));
new Trigger(
() -> DriverStation.isDisabled() ? LightningShuffleboard.getBool("Auton", "POSE BLUE B", false) : false)
.onTrue(new InstantCommand(() -> drivetrain
.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_B_STARTPOSE_BLUE)));
.setDrivetrainPose(StartingPoseConstants.SOURCE_SUB_B_STARTPOSE_BLUE)));
new Trigger(
() -> DriverStation.isDisabled() ? LightningShuffleboard.getBool("Auton", "POSE BLUE C", false) : false)
.onTrue(new InstantCommand(() -> drivetrain
.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_C_STARTPOSE_BLUE)));
.setDrivetrainPose(StartingPoseConstants.SOURCE_SUB_C_STARTPOSE_BLUE)));

// BLUE Alliance set
new Trigger(
() -> DriverStation.isDisabled() ? LightningShuffleboard.getBool("Auton", "POSE RED A", false) : false)
.onTrue(new InstantCommand(() -> drivetrain
.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_A_STARTPOSE_RED)));
.setDrivetrainPose(StartingPoseConstants.SOURCE_SUB_A_STARTPOSE_RED)));
new Trigger(
() -> DriverStation.isDisabled() ? LightningShuffleboard.getBool("Auton", "POSE RED B", false) : false)
.onTrue(new InstantCommand(() -> drivetrain
.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_B_STARTPOSE_RED)));
.setDrivetrainPose(StartingPoseConstants.SOURCE_SUB_B_STARTPOSE_RED)));
new Trigger(
() -> DriverStation.isDisabled() ? LightningShuffleboard.getBool("Auton", "POSE RED C", false) : false)
.onTrue(new InstantCommand(() -> drivetrain
.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_C_STARTPOSE_RED)));
.setDrivetrainPose(StartingPoseConstants.SOURCE_SUB_C_STARTPOSE_RED)));
}

@Override
Expand Down
Loading

0 comments on commit d9bec8f

Please sign in to comment.