Skip to content

Commit

Permalink
Update PurpleLib
Browse files Browse the repository at this point in the history
  • Loading branch information
@viggy96
viggy96 committed May 2, 2024
1 parent 04aace0 commit 6f31995
Show file tree
Hide file tree
Showing 10 changed files with 92 additions and 154 deletions.
5 changes: 4 additions & 1 deletion .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,4 +178,7 @@ simgui-ds.json
src/main/java/frc/robot/BuildConstants.java

# Battery Tracker
previous_battery.txt
previous_battery.txt

# Odometer
*-odometer.txt
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ dependencies {
nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
simulationRelease wpi.sim.enableRelease()

implementation 'com.github.lasarobotics:PurpleLib:2024.4.6'
implementation 'com.github.lasarobotics:PurpleLib:0ed23bb51d'

implementation 'org.apache.commons:commons-math3:3.+'

Expand Down
72 changes: 15 additions & 57 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,19 +4,12 @@

package frc.robot;

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

import org.lasarobotics.hardware.PurpleManager;
import org.lasarobotics.utils.GlobalConstants;
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.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

Expand All @@ -30,67 +23,32 @@ public Robot() {
}

@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,
Path.of("/media/sda1"),
BuildConstants.MAVEN_NAME,
BuildConstants.GIT_SHA,
BuildConstants.BUILD_DATE,
true
);
m_robotContainer = new RobotContainer();
}

@Override
public void robotPeriodic() {
PurpleManager.update();
CommandScheduler.getInstance().run();
}

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

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

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

@Override
public void autonomousInit() {
Expand Down
47 changes: 6 additions & 41 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,32 +6,24 @@

import java.util.function.BooleanSupplier;

import org.apache.commons.math3.ode.SecondaryEquations;
import org.lasarobotics.hardware.revrobotics.Blinkin;

import com.pathplanner.lib.auto.NamedCommands;
import com.revrobotics.REVPhysicsSim;

import edu.wpi.first.apriltag.AprilTag;
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.units.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
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.Commands;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants.Shooter;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.commands.autonomous.SimpleAuto;
import frc.robot.subsystems.climber.ClimberSubsystem;
import frc.robot.subsystems.drive.AutoTrajectory;
import frc.robot.subsystems.drive.DriveSubsystem;
import frc.robot.subsystems.intake.IntakeSubsystem;
Expand Down Expand Up @@ -70,11 +62,13 @@ public class RobotContainer {
private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance();

private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT);
private static final CommandXboxController OPERATOR_KEYPAD = new CommandXboxController(Constants.HID.SECONDARY_CONTROLLER_PORT);

private static SendableChooser<Command> m_automodeChooser = new SendableChooser<>();

public RobotContainer() {
// Silence warnings
DriverStation.silenceJoystickConnectionWarning(true);

// Set drive command
DRIVE_SUBSYSTEM.setDefaultCommand(
DRIVE_SUBSYSTEM.driveCommand(
Expand Down Expand Up @@ -115,7 +109,7 @@ private void configureBindings() {

// Right trigger button - aim and shoot at speaker, shooting only if speaker tag is visible and robot is in range
// Click DPAD down to override and shoot now
PRIMARY_CONTROLLER.rightTrigger().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.a().getAsBoolean()));
PRIMARY_CONTROLLER.b().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.a().getAsBoolean()));

// Right bumper button - amp score, also use for outtake
PRIMARY_CONTROLLER.rightBumper().whileTrue(SHOOTER_SUBSYSTEM.scoreAmpCommand());
Expand Down Expand Up @@ -172,9 +166,6 @@ private void configureBindings() {

// DPAD left - PARTY BUTTON!!
PRIMARY_CONTROLLER.povLeft().whileTrue(partyMode());

// Operator keypad button 1 - PARTY BUTTON!!
OPERATOR_KEYPAD.button(1).whileTrue(partyMode());
}

/**
Expand Down Expand Up @@ -397,32 +388,6 @@ else if (DRIVE_SUBSYSTEM.getAlliance().equals(Alliance.Red)) {
}
}

/**
* Initialization code for disabled mode
*/
public void disabledInit() {
DRIVE_SUBSYSTEM.disabledInit();
}

/**
* Call while disabled
*/
public void disabledPeriodic() {
// Try to get alliance
var alliance = DriverStation.getAlliance();
if (alliance.isEmpty()) return;

// Set alliance if available
DRIVE_SUBSYSTEM.setAlliance(alliance.get());
}

/**
* Exit code for disabled mode
*/
public void disabledExit() {
DRIVE_SUBSYSTEM.disabledExit();
}

/**
* Get currently selected autonomous command
*
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,6 @@ public Command runClimberCommand() {

@Override
public void periodic() {
m_lClimberMotor.periodic();
m_rClimberMotor.stopMotor();
}
}
65 changes: 26 additions & 39 deletions src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
import org.lasarobotics.hardware.revrobotics.Spark.MotorKind;
import org.lasarobotics.led.LEDStrip.Pattern;
import org.lasarobotics.led.LEDSubsystem;
import org.lasarobotics.utils.CommonTriggers;
import org.lasarobotics.utils.GlobalConstants;
import org.lasarobotics.utils.PIDConstants;
import org.littletonrobotics.junction.Logger;
Expand Down Expand Up @@ -62,6 +63,8 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants;
import frc.robot.subsystems.vision.VisionSubsystem;

Expand Down Expand Up @@ -95,8 +98,8 @@ public Hardware(NavX2 navx,
public static final Measure<Velocity<Angle>> NAVX2_YAW_DRIFT_RATE = Units.DegreesPerSecond.of(0.5 / 60);
public static final Measure<Velocity<Angle>> DRIVE_ROTATE_VELOCITY = Units.RadiansPerSecond.of(12 * Math.PI);
public static final Measure<Velocity<Angle>> AIM_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(5.0);
public static final Measure<Velocity<Angle>> VISION_ANGULAR_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(720.0);
public static final Measure<Velocity<Velocity<Angle>>> DRIVE_ROTATE_ACCELERATION = Units.RadiansPerSecond.of(4 * Math.PI).per(Units.Second);
public static final Translation2d AIM_OFFSET = new Translation2d(0.0, -0.5);
public final Measure<Velocity<Distance>> DRIVE_MAX_LINEAR_SPEED;
public final Measure<Velocity<Velocity<Distance>>> DRIVE_AUTO_ACCELERATION;

Expand All @@ -105,8 +108,8 @@ public Hardware(NavX2 navx,
private static final double TOLERANCE = 1.5;
private static final double TIP_THRESHOLD = 35.0;
private static final double BALANCED_THRESHOLD = 10.0;
private static final double AIM_VELOCITY_COMPENSATION_FUDGE_FACTOR = 0.1;
private static final Matrix<N3, N1> ODOMETRY_STDDEV = VecBuilder.fill(0.03, 0.03, Math.toRadians(1.0));
private static final double AIM_VELOCITY_COMPENSATION_FUDGE_FACTOR = 0.5;
private static final Matrix<N3, N1> ODOMETRY_STDDEV = VecBuilder.fill(0.1, 0.1, Math.toRadians(1.0));
private static final Matrix<N3, N1> VISION_STDDEV = VecBuilder.fill(1.0, 1.0, Math.toRadians(3.0));
private static final PIDConstants AUTO_AIM_PID = new PIDConstants(10.0, 0.0, 0.5, 0.0, 0.0, GlobalConstants.ROBOT_LOOP_PERIOD);
private static final TrapezoidProfile.Constraints AIM_PID_CONSTRAINT = new TrapezoidProfile.Constraints(2160.0, 4320.0);
Expand Down Expand Up @@ -155,6 +158,15 @@ public Hardware(NavX2 navx,

private Alliance m_currentAlliance;

public final Command SET_ALLIANCE_COMMAND = Commands.runOnce(() -> {
// Try to get alliance
var alliance = DriverStation.getAlliance();
if (alliance.isEmpty()) return;

// Set alliance if available
setAlliance(alliance.get());
}).ignoringDisable(true).repeatedly();

public final Command ANTI_TIP_COMMAND = new FunctionalCommand(
() -> LEDSubsystem.getInstance().startOverride(Pattern.RED_STROBE),
() -> antiTip(),
Expand Down Expand Up @@ -242,15 +254,17 @@ public DriveSubsystem(Hardware drivetrainHardware, PIDConstants pidf, ControlCen
m_desiredChassisSpeeds = new ChassisSpeeds();

// Setup anti-tip command
//new Trigger(this::isTipping).whileTrue(ANTI_TIP_COMMAND);
new Trigger(this::isTipping).whileTrue(ANTI_TIP_COMMAND);

// Setup auto-aim PID controller
m_autoAimPIDControllerFront = new ProfiledPIDController(AUTO_AIM_PID.kP, 0.0, AUTO_AIM_PID.kD, AIM_PID_CONSTRAINT, AUTO_AIM_PID.period);
m_autoAimPIDControllerFront.enableContinuousInput(-180.0, +180.0);
m_autoAimPIDControllerFront.setTolerance(TOLERANCE);
m_autoAimPIDControllerFront.setIZone(AUTO_AIM_PID.kIZone);
m_autoAimPIDControllerBack = new ProfiledPIDController(AUTO_AIM_PID.kP, 0.0, AUTO_AIM_PID.kD, AIM_PID_CONSTRAINT, AUTO_AIM_PID.period);
m_autoAimPIDControllerBack.enableContinuousInput(-180.0, +180.0);
m_autoAimPIDControllerBack.setTolerance(TOLERANCE);
m_autoAimPIDControllerBack.setIZone(AUTO_AIM_PID.kIZone);

// Initialise other variables
m_previousPose = new Pose2d();
Expand All @@ -259,6 +273,10 @@ public DriveSubsystem(Hardware drivetrainHardware, PIDConstants pidf, ControlCen
// Initalise PurplePathClient
m_purplePathClient = new PurplePathClient(this);

// Set alliance triggers
CommonTriggers.isDSAttached().onTrue(SET_ALLIANCE_COMMAND);
RobotModeTriggers.disabled().whileTrue(SET_ALLIANCE_COMMAND);

// Initialise field
m_field = new Field2d();
SmartDashboard.putData(m_field);
Expand Down Expand Up @@ -486,9 +504,11 @@ private void updatePose() {
// Exit if no valid vision pose estimates
if (apriltagCameraResults.isEmpty()) return;

// Exit if robot is spinning too fast
if (getRotateRate().gt(VISION_ANGULAR_VELOCITY_THRESHOLD)) return;

// Add vision measurements to pose estimator
for (var result : apriltagCameraResults) {
//if (result.estimatedPose.toPose2d().getTranslation().getDistance(m_previousPose.getTranslation()) > 1.0) continue;
m_poseEstimator.addVisionMeasurement(
result.estimatedRobotPose.estimatedPose.toPose2d(),
result.estimatedRobotPose.timestampSeconds,
Expand Down Expand Up @@ -559,8 +579,6 @@ private void aimAtPoint(ControlCentricity controlCentricity, double xRequest, do
return;
}

// Adjust point
point = point.plus(AIM_OFFSET);
// Get current pose
Pose2d currentPose = getPose();
// Angle to target point
Expand Down Expand Up @@ -715,26 +733,6 @@ private void stop() {
m_rRearModule.stop();
}

/**
* Call method during initialization of disabled mode to set motors to brake mode
*/
public void disabledInit() {
m_lFrontModule.disabledInit();
m_rFrontModule.disabledInit();
m_lRearModule.disabledInit();
m_rRearModule.disabledInit();
}

/**
* Call method when exiting disabled mode to set motors to coast mode
*/
public void disabledExit() {
m_lFrontModule.disabledExit();
m_rFrontModule.disabledExit();
m_lRearModule.disabledExit();
m_rRearModule.disabledExit();
}

/**
* Toggle traction control
*/
Expand Down Expand Up @@ -803,12 +801,6 @@ private void resetPoseToVision() {
@Override
public void periodic() {
// This method will be called once per scheduler run
m_navx.periodic();
m_lFrontModule.periodic();
m_rFrontModule.periodic();
m_lRearModule.periodic();
m_rRearModule.periodic();

// Filter inertial velocity
m_navx.getInputs().xVelocity = Units.MetersPerSecond.of(
m_xVelocityFilter.calculate(m_navx.getInputs().xVelocity.in(Units.MetersPerSecond))
Expand All @@ -826,12 +818,7 @@ public void periodic() {
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run in simulation
m_lFrontModule.simulationPeriodic();
m_rFrontModule.simulationPeriodic();
m_lRearModule.simulationPeriodic();
m_rRearModule.simulationPeriodic();

double randomNoise = ThreadLocalRandom.current().nextDouble(0.8, 1.0);
double randomNoise = ThreadLocalRandom.current().nextDouble(0.9, 1.0);
m_navx.getInputs().xVelocity = Units.MetersPerSecond.of(m_desiredChassisSpeeds.vxMetersPerSecond * randomNoise);
m_navx.getInputs().yVelocity = Units.MetersPerSecond.of(m_desiredChassisSpeeds.vyMetersPerSecond * randomNoise);
m_navx.getInputs().yawRate = Units.RadiansPerSecond.of(m_desiredChassisSpeeds.omegaRadiansPerSecond * randomNoise);
Expand Down
Loading

0 comments on commit 6f31995

Please sign in to comment.