Skip to content

Commit

Permalink
Sync with GRRBase
Browse files Browse the repository at this point in the history
  • Loading branch information
@bryceroethel
bryceroethel committed Feb 17, 2024
1 parent 2db1337 commit bc72e8f
Show file tree
Hide file tree
Showing 24 changed files with 394 additions and 431 deletions.
11 changes: 5 additions & 6 deletions src/main/java/org/team340/lib/GRRDashboard.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.Supplier;
import org.team340.lib.controller.Controller2;
import org.team340.lib.util.Math2;

/**
* Utility class for interfacing with the dashboard.
Expand Down Expand Up @@ -402,13 +401,13 @@ private static class RobotSendable implements Sendable {
@Override
public void initSendable(SendableBuilder builder) {
builder.addBooleanProperty("blueAlliance", () -> DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue), null);
builder.addDoubleProperty("cpuTemperature", () -> Math2.toFixed(RobotController.getCPUTemp()), null);
builder.addDoubleProperty("cpuTemperature", RobotController::getCPUTemp, null);
builder.addBooleanProperty("enabled", DriverStation::isEnabled, null);
builder.addIntegerProperty("matchTime", () -> (int) DriverStation.getMatchTime(), null);
builder.addDoubleProperty("ntUpdateTime", () -> Math2.toFixed(lastWatchdog, 1e-6), null);
builder.addDoubleProperty("powerUsage", () -> Math2.toFixed(lastPowerUsage), null);
builder.addDoubleProperty("matchTime", DriverStation::getMatchTime, null);
builder.addDoubleProperty("ntUpdateTime", () -> lastWatchdog, null);
builder.addDoubleProperty("powerUsage", () -> lastPowerUsage, null);
builder.addIntegerProperty("timestamp", RobotController::getFPGATime, null);
builder.addDoubleProperty("voltage", () -> Math2.toFixed(RobotController.getBatteryVoltage()), null);
builder.addDoubleProperty("voltage", RobotController::getBatteryVoltage, null);
}
}
}
29 changes: 14 additions & 15 deletions src/main/java/org/team340/lib/HardwareSendables.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
import java.util.List;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.Supplier;
import org.team340.lib.util.Math2;

// TODO Faults

Expand Down Expand Up @@ -172,13 +171,13 @@ public void initSendable(SendableBuilder builder) {
usageMutex.unlock();
}

return Math2.toFixed(p);
return p;
},
null
);
builder.addDoubleProperty("powerUsage", () -> Math2.toFixed(usage), null);
builder.addDoubleProperty("voltage", () -> Math2.toFixed(voltage.get()), null);
builder.addDoubleProperty("current", () -> Math2.toFixed(current.get()), null);
builder.addDoubleProperty("powerUsage", () -> usage, null);
builder.addDoubleProperty("voltage", voltage::get, null);
builder.addDoubleProperty("current", current::get, null);
}
}

Expand Down Expand Up @@ -224,10 +223,10 @@ public Motor(
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.addDoubleProperty("output", () -> Math2.toFixed(output.get()), null);
builder.addDoubleProperty("temperature", () -> Math2.toFixed(temperature.get()), null);
builder.addDoubleProperty("velocity", () -> Math2.toFixed(velocity.get()), null);
builder.addDoubleProperty("position", () -> Math2.toFixed(position.get()), null);
builder.addDoubleProperty("output", output::get, null);
builder.addDoubleProperty("temperature", temperature::get, null);
builder.addDoubleProperty("velocity", velocity::get, null);
builder.addDoubleProperty("position", position::get, null);
}
}

Expand Down Expand Up @@ -255,8 +254,8 @@ public Encoder(String key, String label, Object api, Supplier<Double> velocity,
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.addDoubleProperty("velocity", () -> Math2.toFixed(velocity.get()), null);
builder.addDoubleProperty("position", () -> Math2.toFixed(position.get()), null);
builder.addDoubleProperty("velocity", velocity::get, null);
builder.addDoubleProperty("position", position::get, null);
}
}

Expand Down Expand Up @@ -287,9 +286,9 @@ public IMU(String key, String label, Object api, Supplier<Double> yaw, Supplier<
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.addDoubleProperty("yaw", () -> Math2.toFixed(yaw.get()), null);
builder.addDoubleProperty("pitch", () -> Math2.toFixed(pitch.get()), null);
builder.addDoubleProperty("roll", () -> Math2.toFixed(roll.get()), null);
builder.addDoubleProperty("yaw", yaw::get, null);
builder.addDoubleProperty("pitch", pitch::get, null);
builder.addDoubleProperty("roll", roll::get, null);
}
}

Expand Down Expand Up @@ -663,7 +662,7 @@ public PneumaticHub(String label, edu.wpi.first.wpilibj.PneumaticHub pneumaticHu
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.addDoubleProperty("pressure", () -> Math2.toFixed(pressure.get()), null);
builder.addDoubleProperty("pressure", pressure::get, null);
builder.addBooleanProperty("pressureSwitchOn", pressureSwitchOn::get, null);
builder.addBooleanProperty("compressorOn", compressorOn::get, null);
}
Expand Down
17 changes: 8 additions & 9 deletions src/main/java/org/team340/lib/controller/Controller2.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import org.team340.lib.GRRDashboard;
import org.team340.lib.util.Math2;

/**
* A modified {@link CommandXboxController}.
Expand Down Expand Up @@ -453,19 +452,19 @@ public void initSendable(SendableBuilder builder) {
builder.addIntegerProperty("pov", hid::getPOV, null);

builder.addBooleanProperty("ls", hid::getLeftStickButton, null);
builder.addDoubleProperty("lx", () -> Math2.toFixed(getLeftX()), null);
builder.addDoubleProperty("ly", () -> Math2.toFixed(getLeftY()), null);
builder.addDoubleProperty("ln", () -> Math2.toFixed(Math.hypot(getLeftX(), getLeftY())), null);
builder.addDoubleProperty("lns", () -> Math2.toFixed(Math.hypot(super.getLeftX(), super.getLeftY())), null);
builder.addDoubleProperty("lx", this::getLeftX, null);
builder.addDoubleProperty("ly", this::getLeftY, null);
builder.addDoubleProperty("ln", () -> Math.hypot(getLeftX(), getLeftY()), null);
builder.addDoubleProperty("lns", () -> Math.hypot(super.getLeftX(), super.getLeftY()), null);

builder.addBooleanProperty("rs", hid::getRightStickButton, null);
builder.addDoubleProperty("rx", () -> Math2.toFixed(getRightX()), null);
builder.addDoubleProperty("ry", () -> Math2.toFixed(getRightY()), null);
builder.addDoubleProperty("rx", this::getRightX, null);
builder.addDoubleProperty("ry", this::getRightY, null);

builder.addBooleanProperty("lb", hid::getLeftBumper, null);
builder.addBooleanProperty("rb", hid::getRightBumper, null);

builder.addDoubleProperty("lt", () -> Math2.toFixed(getLeftTriggerAxis()), null);
builder.addDoubleProperty("rt", () -> Math2.toFixed(getRightTriggerAxis()), null);
builder.addDoubleProperty("lt", this::getLeftTriggerAxis, null);
builder.addDoubleProperty("rt", this::getRightTriggerAxis, null);
}
}
45 changes: 29 additions & 16 deletions src/main/java/org/team340/lib/swerve/SwerveBase.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,31 +180,31 @@ public SwerveBase(String label, SwerveConfig config) {
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);

builder.addDoubleProperty("velocityX", () -> Math2.toFixed(getVelocity(true).vxMetersPerSecond), null);
builder.addDoubleProperty("velocityY", () -> Math2.toFixed(getVelocity(true).vyMetersPerSecond), null);
builder.addDoubleProperty("velocityRot", () -> Math2.toFixed(getVelocity(true).omegaRadiansPerSecond), null);
builder.addDoubleProperty("velocityNorm", () -> SwerveSerializer.chassisSpeedsNorm(getVelocity(true), true), null);
builder.addDoubleProperty("velocityX", () -> getVelocity(true).vxMetersPerSecond, null);
builder.addDoubleProperty("velocityY", () -> getVelocity(true).vyMetersPerSecond, null);
builder.addDoubleProperty("velocityRot", () -> getVelocity(true).omegaRadiansPerSecond, null);
builder.addDoubleProperty("velocityNorm", () -> SwerveSerializer.chassisSpeedsNorm(getVelocity(true)), null);

builder.addDoubleProperty("odometryX", () -> Math2.toFixed(getPosition().getX()), null);
builder.addDoubleProperty("odometryY", () -> Math2.toFixed(getPosition().getY()), null);
builder.addDoubleProperty("odometryRot", () -> Math2.toFixed(getPosition().getRotation().getRadians()), null);
builder.addDoubleProperty("odometryX", () -> getPosition().getX(), null);
builder.addDoubleProperty("odometryY", () -> getPosition().getY(), null);
builder.addDoubleProperty("odometryRot", () -> getPosition().getRotation().getRadians(), null);

builder.addDoubleArrayProperty(
"desiredModuleStates",
() -> SwerveSerializer.moduleStatesDoubleArray(getDesiredModuleStates(), true),
() -> SwerveSerializer.moduleStatesDoubleArray(getDesiredModuleStates()),
null
);
builder.addDoubleArrayProperty("moduleStates", () -> SwerveSerializer.moduleStatesDoubleArray(getModuleStates(), true), null);
builder.addDoubleArrayProperty("moduleStates", () -> SwerveSerializer.moduleStatesDoubleArray(getModuleStates()), null);

for (SwerveModule module : modules) {
GRRDashboard.addSubsystemSendable(
"Modules/" + StringUtil.toPascalCase(module.getLabel()),
this,
SendableFactory.create(moduleBuilder -> {
moduleBuilder.publishConstString(".label", module.getLabel());
moduleBuilder.addDoubleProperty("velocity", () -> Math2.toFixed(module.getVelocity()), null);
moduleBuilder.addDoubleProperty("distance", () -> Math2.toFixed(module.getDistance()), null);
moduleBuilder.addDoubleProperty("angle", () -> Math2.toFixed(module.getHeading()), null);
moduleBuilder.addDoubleProperty("velocity", module::getVelocity, null);
moduleBuilder.addDoubleProperty("distance", module::getDistance, null);
moduleBuilder.addDoubleProperty("angle", module::getHeading, null);
})
);
}
Expand Down Expand Up @@ -336,23 +336,31 @@ protected void driveVelocity(double xV, double yV, double rotV, boolean fieldRel
* Drives the robot using percents of its calculated max velocity while locked pointing at a position on the field.
* @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}.
* @param y The desired {@code y} speed from {@code -1.0} to {@code 1.0}.
* @param angleOffset The offset to use when determining which side of the robot should face the point. (value in radians)
* @param point The desired field relative position to point at (axis values in meters).
* @param controller A profiled PID controller to use for translating to and maintaining the angle to the desired point.
*/
protected void driveAroundPoint(double x, double y, Translation2d point, ProfiledPIDController controller) {
driveAroundPointVelocity(x * config.getVelocity(), y * config.getVelocity(), point, controller);
protected void driveAroundPoint(double x, double y, double angleOffset, Translation2d point, ProfiledPIDController controller) {
driveAroundPointVelocity(x * config.getVelocity(), y * config.getVelocity(), angleOffset, point, controller);
}

/**
* Drives the robot using velocity while locked pointing at a position on the field.
* @param xV The desired {@code x} velocity in meters/second.
* @param yV The desired {@code y} velocity in meters/second.
* @param angleOffset The offset to use when determining which side of the robot should face the point. (value in radians)
* @param point The desired field relative position to point at (axis values in meters).
* @param controller A profiled PID controller to use for translating to and maintaining the angle to the desired point.
*/
protected void driveAroundPointVelocity(double xV, double yV, Translation2d point, ProfiledPIDController controller) {
protected void driveAroundPointVelocity(
double xV,
double yV,
double angleOffset,
Translation2d point,
ProfiledPIDController controller
) {
Translation2d robotPoint = getPosition().getTranslation();
double angle = MathUtil.angleModulus(point.minus(robotPoint).getAngle().getRadians());
double angle = MathUtil.angleModulus(point.minus(robotPoint).getAngle().getRadians() + angleOffset);
driveAngleVelocity(xV, yV, angle, controller);
}

Expand Down Expand Up @@ -462,6 +470,11 @@ protected void driveVoltage(double voltage, Rotation2d heading) {
for (int i = 0; i < modules.length; i++) {
modules[i].setVoltage(voltage, heading);
}
SwerveModuleState[] states = new SwerveModuleState[modules.length];
for (int i = 0; i < states.length; i++) {
states[i] = new SwerveModuleState(0, heading);
}
ratelimiter.setLastState(new SwerveRatelimiter.SwerveState(new ChassisSpeeds(), states));
}

/**
Expand Down
9 changes: 4 additions & 5 deletions src/main/java/org/team340/lib/swerve/SwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,15 +134,14 @@ public SwerveModulePosition getModulePosition() {
public void setDesiredState(SwerveModuleState state) {
double moveSpeed = state.speedMetersPerSecond;
double angleDiff = state.angle.rotateBy(Rotation2d.fromRadians(getHeading()).times(-1.0)).getRadians();
boolean flipped = false;
if (Math.abs(angleDiff) > (Math2.HALF_PI)) {
if (angleDiff > 0) angleDiff -= Math.PI; else angleDiff += Math.PI;
moveSpeed *= -1.0;
flipped = true;
}

double moveFF = moveFFController.calculate(
moveSpeed,
controlTimer.get() == 0 ? 0.0 : (moveSpeed - lastMoveSpeed) / controlTimer.get()
);
double moveFF = moveFFController.calculate(moveSpeed);
double turnTarget = turnMotor.getPosition() + angleDiff;

moveMotor.setReference(moveSpeed, moveFF);
Expand All @@ -157,7 +156,7 @@ public void setDesiredState(SwerveModuleState state) {
simVelocity = moveSpeed;
}

desiredState = state;
desiredState = flipped ? new SwerveModuleState(-state.speedMetersPerSecond, state.angle.rotateBy(Math2.ROTATION2D_PI)) : state;
lastMoveSpeed = moveSpeed;
controlTimer.restart();
}
Expand Down
7 changes: 3 additions & 4 deletions src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@
import org.team340.lib.swerve.util.SwerveConversions;
import org.team340.lib.util.Math2;
import org.team340.lib.util.config.PIDConfig;
import org.team340.lib.util.config.rev.AbsoluteEncoderConfig;
import org.team340.lib.util.config.rev.RelativeEncoderConfig;
import org.team340.lib.util.config.rev.SparkAbsoluteEncoderConfig;
import org.team340.lib.util.config.rev.SparkFlexConfig;
import org.team340.lib.util.config.rev.SparkFlexConfig.Frame;
import org.team340.lib.util.config.rev.SparkPIDControllerConfig;
Expand Down Expand Up @@ -56,13 +56,12 @@ public SwerveSparkFlex(

int periodMs = (int) (config.getPeriod() * 1000.0);
int periodOdometryMs = (int) (config.getOdometryPeriod() * 1000.0);
boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType());
boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType()) && !isMoveMotor;
double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian());
PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID();

new SparkFlexConfig()
.clearFaults()
.restoreFactoryDefaults()
.enableVoltageCompensation(config.getOptimalVoltage())
.setSmartCurrentLimit((int) (isMoveMotor ? config.getMoveCurrentLimit() : config.getTurnCurrentLimit()))
.setIdleMode(
Expand Down Expand Up @@ -91,7 +90,7 @@ public SwerveSparkFlex(
.apply(sparkFlex, relativeEncoder);

if (usingAttachedEncoder) {
new AbsoluteEncoderConfig()
new SparkAbsoluteEncoderConfig()
.setPositionConversionFactor(Math2.TWO_PI)
.setVelocityConversionFactor(Math2.TWO_PI / 60.0)
.setInverted(moduleConfig.getEncoderInverted())
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@
import org.team340.lib.swerve.util.SwerveConversions;
import org.team340.lib.util.Math2;
import org.team340.lib.util.config.PIDConfig;
import org.team340.lib.util.config.rev.AbsoluteEncoderConfig;
import org.team340.lib.util.config.rev.RelativeEncoderConfig;
import org.team340.lib.util.config.rev.SparkAbsoluteEncoderConfig;
import org.team340.lib.util.config.rev.SparkMaxConfig;
import org.team340.lib.util.config.rev.SparkMaxConfig.Frame;
import org.team340.lib.util.config.rev.SparkPIDControllerConfig;
Expand Down Expand Up @@ -56,7 +56,7 @@ public SwerveSparkMax(

int periodMs = (int) (config.getPeriod() * 1000.0);
int periodOdometryMs = (int) (config.getOdometryPeriod() * 1000.0);
boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType());
boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType()) && !isMoveMotor;
double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian());
PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID();

Expand Down Expand Up @@ -91,7 +91,7 @@ public SwerveSparkMax(
.apply(sparkMax, relativeEncoder);

if (usingAttachedEncoder) {
new AbsoluteEncoderConfig()
new SparkAbsoluteEncoderConfig()
.setPositionConversionFactor(Math2.TWO_PI)
.setVelocityConversionFactor(Math2.TWO_PI / 60.0)
.setInverted(moduleConfig.getEncoderInverted())
Expand Down
9 changes: 1 addition & 8 deletions src/main/java/org/team340/lib/swerve/util/SwerveField2d.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package org.team340.lib.swerve.util;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
Expand Down Expand Up @@ -38,12 +37,6 @@ public void update(Pose2d newPose) {
);
}

setRobotPose(
new Pose2d(
Math2.toFixed(newPose.getX()),
Math2.toFixed(newPose.getY()),
Rotation2d.fromDegrees(Math2.toFixed(newPose.getRotation().getDegrees()))
)
);
setRobotPose(newPose);
}
}
Loading

0 comments on commit bc72e8f

Please sign in to comment.