Skip to content

Commit

Permalink
Fixed some errors
Browse files Browse the repository at this point in the history
  • Loading branch information
Liam Teltow committed Jan 11, 2025
1 parent 1469055 commit 0a2f874
Show file tree
Hide file tree
Showing 10 changed files with 425 additions and 2,170 deletions.
2 changes: 1 addition & 1 deletion .wpilib/wpilib_preferences.json
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
{
"enableCppIntellisense": false,
"currentLanguage": "java",
"projectYear": "2024",
"projectYear": "2025",
"teamNumber": 418
}
153 changes: 76 additions & 77 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,76 +1,77 @@
// 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 frc.robot;

import java.util.Optional;

import org.apache.commons.math3.analysis.interpolation.SplineInterpolator;
import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction;
import org.lasarobotics.drive.AdvancedSwerveKinematics.ControlCentricity;
import org.lasarobotics.drive.MAXSwerveModule;
import org.lasarobotics.drive.swerve.AdvancedSwerveKinematics.ControlCentricity;
import org.lasarobotics.drive.swerve.DriveWheel;
import org.lasarobotics.drive.swerve.child.MAXSwerveModule;
import org.lasarobotics.drive.swerve.child.SwerveX2Module;
import org.lasarobotics.hardware.ctre.CANcoder;
import org.lasarobotics.hardware.ctre.PhoenixCANBus;
import org.lasarobotics.hardware.ctre.TalonFX;
import org.lasarobotics.hardware.kauailabs.NavX2;
import org.lasarobotics.hardware.revrobotics.Spark;
import org.lasarobotics.hardware.revrobotics.SparkPIDConfig;
import org.lasarobotics.led.LEDStrip;
import org.lasarobotics.utils.FFConstants;
import org.lasarobotics.utils.PIDConstants;
import org.lasarobotics.vision.AprilTagCamera.Resolution;

import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.units.Units;
import frc.robot.subsystems.drive.PurplePathPose;
import frc.robot.subsystems.vision.AprilTagCamera.Resolution;
/**
* The Constants class provides a convenient place for teams to hold robot-wide
* numerical or boolean
* constants. This class should not be used for any other purpose. All constants
* should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>
* It is advised to statically import this class (or one of its inner classes)
* wherever the
* constants are needed, to reduce verbosity.
*/
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Dimensionless;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.units.measure.Mass;
import edu.wpi.first.units.measure.Time;

public final class Constants {

public static class Field {
public static final double FIELD_WIDTH = 8.21;
public static final double FIELD_LENGTH = 16.54;

public static final Translation2d CENTER = new Translation2d(FIELD_LENGTH / 2, FIELD_WIDTH / 2);
public static final Pair<Integer,Translation2d> BLUE_SPEAKER = new Pair<Integer,Translation2d>(7, new Translation2d(0.00, 5.55));
public static final Pair<Integer,Translation2d> RED_SPEAKER = new Pair<Integer,Translation2d>(4, new Translation2d(15.64, 5.55));

public static final PurplePathPose AMP = new PurplePathPose(
new Pose2d(Units.Meters.of(1.85), Units.Meters.of(7.77), Rotation2d.fromDegrees(-90.0)),
new Pose2d(Units.Meters.of(14.66), Units.Meters.of(7.77), Rotation2d.fromDegrees(-90.0)),
Units.Meters.of(0.5),
true
);
public static final PurplePathPose SOURCE = new PurplePathPose(
new Pose2d(Units.Meters.of(15.48), Units.Meters.of(0.84), Rotation2d.fromDegrees(+120.00)),
new Pose2d(Units.Meters.of(1.07), Units.Meters.of(0.82), Rotation2d.fromDegrees(+60.0)),
Units.Meters.of(0.5),
true
);
public static final AprilTagFieldLayout FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo);
public static final AprilTag BLUE_SPEAKER = getTag(7).get();
public static final AprilTag RED_SPEAKER = getTag(4).get();

/**
* Get AprilTag from field
* @param id Tag ID
* @return AprilTag matching ID
*/
public static Optional<AprilTag> getTag(int id) {
return FIELD_LAYOUT.getTags().stream().filter((tag) -> tag.ID == id).findFirst();
}
}

public static class HID {
public static final int PRIMARY_CONTROLLER_PORT = 0;
public static final int SECONDARY_CONTROLLER_PORT = 1;
public static final double CONTROLLER_DEADBAND = 0.1;
public static final Dimensionless CONTROLLER_DEADBAND = Units.Percent.of(10);
}

public static class Drive {
public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(7.0, 0.0, 0.2, 0.0, 0.0);
public static final double DRIVE_SLIP_RATIO = 0.05;
public static final double DRIVE_TURN_SCALAR = 60.0;
public static final double DRIVE_LOOKAHEAD = 6;
public static final DriveWheel DRIVE_WHEEL = DriveWheel.create(Units.Inches.of(4.0), Units.Value.of(1.3), Units.Value.of(1.2));
public static final PIDConstants DRIVE_PID = PIDConstants.of(0.3, 0.0, 0.001, 0.0, 0.0);
public static final FFConstants DRIVE_FF = FFConstants.of(0.2, 0.0, 0.0, 0.0);
public static final PIDConstants ROTATE_PID = PIDConstants.of(2.0, 0.0, 0.1, 0.0, 0.0);
public static final FFConstants ROTATE_FF = FFConstants.of(0.2, 0.0, 0.0, 0.0);
public static final PIDConstants DRIVE_ROTATE_PID = PIDConstants.of(8.0, 0.0, 0.3, 0.0, 0.0);
public static final PIDConstants DRIVE_AUTO_AIM_PID = PIDConstants.of(12.0, 0.0, 0.1, 0.0, 0.0);
public static final Dimensionless DRIVE_SLIP_RATIO = Units.Percent.of(3.0);
public static final Angle DRIVE_TURN_SCALAR = Units.Degrees.of(90.0);
public static final Time DRIVE_LOOKAHEAD = Units.Seconds.of(0.2);

public static final Distance DRIVE_WHEELBASE = Units.Meters.of(0.5588);
public static final Distance DRIVE_TRACK_WIDTH = Units.Meters.of(0.5588);
public static final Mass MASS = Units.Pounds.of(110.0);
public static final Time AUTO_LOCK_TIME = Units.Seconds.of(3.0);
public static final Current DRIVE_CURRENT_LIMIT = Units.Amps.of(90.0);

public static final ControlCentricity DRIVE_CONTROL_CENTRICITY = ControlCentricity.FIELD_CENTRIC;

Expand All @@ -83,10 +84,27 @@ public static class Drive {
public static final PolynomialSplineFunction DRIVE_THROTTLE_INPUT_CURVE = SPLINE_INTERPOLATOR.interpolate(DRIVE_THROTTLE_INPUT_CURVE_X, DRIVE_THROTTLE_INPUT_CURVE_Y);
public static final PolynomialSplineFunction DRIVE_TURN_INPUT_CURVE = SPLINE_INTERPOLATOR.interpolate(DRIVE_TURN_INPUT_CURVE_X, DRIVE_TURN_INPUT_CURVE_Y);

public static final MAXSwerveModule.GearRatio GEAR_RATIO = MAXSwerveModule.GearRatio.L3;
public static final SwerveX2Module.GearRatio CTRE_GEAR_RATIO = SwerveX2Module.GearRatio.X4_3;
public static final MAXSwerveModule.GearRatio REV_GEAR_RATIO = MAXSwerveModule.GearRatio.L3;
}

public static class CTREDriveHardware {
public static final NavX2.ID NAVX_ID = new NavX2.ID("DriveHardware/NavX2");
public static final TalonFX.ID LEFT_FRONT_DRIVE_MOTOR_ID = new TalonFX.ID("DriveHardware/Swerve/LeftFront/Drive", PhoenixCANBus.CANIVORE, 2);
public static final TalonFX.ID LEFT_FRONT_ROTATE_MOTOR_ID = new TalonFX.ID("DriveHardware/Swerve/LeftFront/Rotate", PhoenixCANBus.CANIVORE, 3);
public static final CANcoder.ID LEFT_FRONT_ROTATE_ENCODER_ID = new CANcoder.ID("DriveHardware/Swerve/LeftFront/Encoder", PhoenixCANBus.CANIVORE, 10);
public static final TalonFX.ID RIGHT_FRONT_DRIVE_MOTOR_ID = new TalonFX.ID("DriveHardware/Swerve/RightFront/Drive", PhoenixCANBus.CANIVORE, 4);
public static final TalonFX.ID RIGHT_FRONT_ROTATE_MOTOR_ID = new TalonFX.ID("DriveHardware/Swerve/RightFront/Rotate", PhoenixCANBus.CANIVORE, 5);
public static final CANcoder.ID RIGHT_FRONT_ROTATE_ENCODER_ID = new CANcoder.ID("DriveHardware/Swerve/RightFront/Encoder", PhoenixCANBus.CANIVORE, 11);
public static final TalonFX.ID LEFT_REAR_DRIVE_MOTOR_ID = new TalonFX.ID("DriveHardware/Swerve/LeftRear/Drive", PhoenixCANBus.CANIVORE, 6);
public static final TalonFX.ID LEFT_REAR_ROTATE_MOTOR_ID = new TalonFX.ID("DriveHardware/Swerve/LeftRear/Rotate", PhoenixCANBus.CANIVORE, 7);
public static final CANcoder.ID LEFT_REAR_ROTATE_ENCODER_ID = new CANcoder.ID("DriveHardware/Swerve/LeftRear/Encoder", PhoenixCANBus.CANIVORE, 12);
public static final TalonFX.ID RIGHT_REAR_DRIVE_MOTOR_ID = new TalonFX.ID("DriveHardware/Swerve/RightRear/Drive", PhoenixCANBus.CANIVORE, 8);
public static final TalonFX.ID RIGHT_REAR_ROTATE_MOTOR_ID = new TalonFX.ID("DriveHardware/Swerve/RightRear/Rotate", PhoenixCANBus.CANIVORE, 9);
public static final CANcoder.ID RIGHT_REAR_ROTATE_ENCODER_ID = new CANcoder.ID("DriveHardware/Swerve/RightRear/Encoder", PhoenixCANBus.CANIVORE, 13);
}

public static class DriveHardware {
public static class REVDriveHardware {
public static final NavX2.ID NAVX_ID = new NavX2.ID("DriveHardware/NavX2");
public static final Spark.ID LEFT_FRONT_DRIVE_MOTOR_ID = new Spark.ID("DriveHardware/Swerve/LeftFront/Drive", 2);
public static final Spark.ID LEFT_FRONT_ROTATE_MOTOR_ID = new Spark.ID("DriveHardware/Swerve/LeftFront/Rotate", 3);
Expand All @@ -96,50 +114,31 @@ public static class DriveHardware {
public static final Spark.ID LEFT_REAR_ROTATE_MOTOR_ID = new Spark.ID("DriveHardware/Swerve/LeftRear/Rotate", 7);
public static final Spark.ID RIGHT_REAR_DRIVE_MOTOR_ID = new Spark.ID("DriveHardware/Swerve/RightRear/Drive", 8);
public static final Spark.ID RIGHT_REAR_ROTATE_MOTOR_ID = new Spark.ID("DriveHardware/Swerve/RightRear/Rotate", 9);
public static final LEDStrip.ID LED_STRIP_ID = new LEDStrip.ID("DriveHardware/LEDStrip", 0, 200);
}

public static class VisionHardware {
public static final String CAMERA_A_NAME = "Arducam_OV9782_USB_Camera_A";
public static final Transform3d CAMERA_A_LOCATION = new Transform3d(
new Translation3d(0.381, 0.133, 0.102),
new Rotation3d(0.0, Math.toRadians(-20.0), 0.0)
new Translation3d(-0.1016, -0.2921, 0.521),
new Rotation3d(0.0, Math.toRadians(-26.0), Math.toRadians(+180.0))
);
public static final Resolution CAMERA_A_RESOLUTION = Resolution.RES_1280_720;
public static final Rotation2d CAMERA_A_FOV = Rotation2d.fromDegrees(79.7);

public static final String CAMERA_B_NAME = "Arducam_OV9782_USB_Camera_B";
public static final Transform3d CAMERA_B_LOCATION = new Transform3d(
new Translation3d(0.148, 0.2667, 0.47),
new Rotation3d(0.0, Math.toRadians(-25.0), Math.toRadians(+180.0))
new Translation3d(0.0254, -0.2921, 0.584),
new Rotation3d(0.0, Math.toRadians(-25.6), 0.0)
);
public static final Resolution CAMERA_B_RESOLUTION = Resolution.RES_1280_720;
public static final Rotation2d CAMERA_B_FOV = Rotation2d.fromDegrees(79.7);

public static final String CAMERA_OBJECT_NAME = "Arducam_OV9782_USB_Camera_C";
public static final Transform3d CAMERA_OBJECT_LOCATION = new Transform3d(
new Translation3d(0.3, 0.0, 0.5),
new Rotation3d(0, Math.toRadians(+15.0), 0)
new Rotation3d(0, Math.toRadians(+15.0), Math.toRadians(180))
);
public static final Resolution CAMERA_OBJECT_RESOLUTION = Resolution.RES_1280_720;
public static final Rotation2d CAMERA_OBJECT_FOV = Rotation2d.fromDegrees(79.7);
}

public static class WiggleStick {
public static final SparkPIDConfig WIGGLE_STICK_CONFIG = new SparkPIDConfig(
new PIDConstants(0.2, 0, 0, 0, 0),
false,
false,
1.0,
0.0,
15.0,
true
);
public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(10, 20);
}

public static class SmartDashboard {
public static final String SMARTDASHBOARD_DEFAULT_TAB = "SmartDashboard";
public static final String SMARTDASHBOARD_AUTO_MODE = "Auto Mode";
}
}
}
77 changes: 15 additions & 62 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,72 +4,31 @@

package frc.robot;

import org.lasarobotics.battery.BatteryTracker;
import org.lasarobotics.utils.GlobalConstants;
import java.nio.file.Path;

import org.lasarobotics.hardware.PurpleManager;
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;

import com.pathplanner.lib.pathfinding.Pathfinding;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

public class Robot extends LoggedRobot {
private Command m_autonomousCommand;

private RobotContainer m_robotContainer;
private final RobotContainer m_robotContainer;

public Robot() {
super(GlobalConstants.ROBOT_LOOP_PERIOD);
}

@Override
@SuppressWarnings("resource")
public void robotInit() {
// AdvantageKit Logging
BatteryTracker batteryTracker = new BatteryTracker(BatteryTracker.initializeHardware());
Logger.recordMetadata("ProjectName", "PurpleSwerve");
Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
Logger.recordMetadata("BatteryName", batteryTracker.scanBattery());

// Set pathfinding algorithm to be AdvantageKit compatible
Pathfinding.setPathfinder(new LocalADStarAK());

if (isReal()) {
// If robot is real, log to USB drive and publish data to NetworkTables
Logger.addDataReceiver(new WPILOGWriter("/media/sda1/"));
Logger.addDataReceiver(new NT4Publisher());
new PowerDistribution();
// Battery Tracking
if (batteryTracker.isBatteryReused())
DriverStation.reportError(batteryTracker.scanBattery() + " is being reused!", false);
else batteryTracker.writeCurrentBattery();
} else {
// Else just publish to NetworkTables for simulation or replay log file if var is set
String replay = System.getenv(GlobalConstants.REPLAY_ENVIRONMENT_VAR);
if (replay == null || replay.isBlank()) Logger.addDataReceiver(new NT4Publisher());
else {
// Run as fast as possible
setUseTiming(false);
// Pull the replay log from AdvantageScope (or prompt the user)
String logPath = LogFileUtil.findReplayLog();
// Read replay log
Logger.setReplaySource(new WPILOGReader(logPath));
// Save outputs to a new log
Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
}
}

// Start logging! No more data receivers, replay sources, or metadata values may be added.
Logger.start();

PurpleManager.initialize(
this,
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
Path.of("/media/sda1"),
BuildConstants.MAVEN_NAME,
BuildConstants.GIT_SHA,
BuildConstants.BUILD_DATE,
true
);
m_robotContainer = new RobotContainer();
}

Expand All @@ -78,7 +37,7 @@ public void robotPeriodic() {
PurpleManager.update();
CommandScheduler.getInstance().run();
}
// burrito wuz here

@Override
public void disabledInit() {}

Expand Down Expand Up @@ -116,14 +75,8 @@ public void teleopPeriodic() {}
@Override
public void teleopExit() {}

@Override
public void simulationPeriodic() {
m_robotContainer.simulationPeriodic();
}

@Override
public void testInit() {

CommandScheduler.getInstance().cancelAll();
}

Expand Down
8 changes: 5 additions & 3 deletions src/main/java/frc/robot/subsystems/drive/AutoTrajectory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@

import java.util.List;

import org.lasarobotics.drive.swerve.SwerveDrive;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.GoalEndState;
Expand All @@ -18,7 +20,7 @@
import edu.wpi.first.wpilibj2.command.Commands;

public class AutoTrajectory {
DriveSubsystem m_driveSubsystem;
SwerveDrive m_driveSubsystem;
Command m_swerveCommand;
Pair<String,List<PathPlannerPath>> m_auto;

Expand All @@ -27,7 +29,7 @@ public class AutoTrajectory {
* @param driveSubsystem DriveSubsystem to drive the robot
* @param autoName PathPlanner auto name
*/
public AutoTrajectory(DriveSubsystem driveSubsystem, String autoName) {
public AutoTrajectory(SwerveDrive driveSubsystem, String autoName) {
this.m_driveSubsystem = driveSubsystem;

// Get path
Expand All @@ -40,7 +42,7 @@ public AutoTrajectory(DriveSubsystem driveSubsystem, String autoName) {
* @param waypoints List of x, y coordinate pairs in trajectory
* @param pathConstraints Path following constraints
*/
public AutoTrajectory(DriveSubsystem driveSubsystem, List<Pose2d> waypoints, PathConstraints pathConstraints) {
public AutoTrajectory(SwerveDrive driveSubsystem, List<Pose2d> waypoints, PathConstraints pathConstraints) {
this.m_driveSubsystem = driveSubsystem;

// Generate path from waypoints
Expand Down
Loading

0 comments on commit 0a2f874

Please sign in to comment.