Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
chore: Format.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Jan 20, 2024
1 parent d73e53a commit e282705
Show file tree
Hide file tree
Showing 5 changed files with 13 additions and 12 deletions.
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,9 @@ private void configureBindings() {

driverController.a().whileTrue(swerve.forwards());
driverController.b().whileTrue(swerve.sideways());
driverController.y().onTrue(Commands.runOnce(() -> odometry.setRotation(Rotation2d.fromDegrees(0))));
driverController
.y()
.onTrue(Commands.runOnce(() -> odometry.setRotation(Rotation2d.fromDegrees(0))));

operatorController.leftBumper().whileTrue(shooter.intake());
operatorController.leftTrigger().whileTrue(shooter.smartIntake());
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/odometry/GyroscopeIOSim.java
Original file line number Diff line number Diff line change
@@ -1,10 +1,9 @@
package frc.robot.odometry;

import edu.wpi.first.math.util.Units;
import frc.robot.RobotConstants;
import java.util.function.DoubleSupplier;

import edu.wpi.first.math.util.Units;

/** Simulated gyroscope. */
public class GyroscopeIOSim implements GyroscopeIO {

Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/odometry/Odometry.java
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,7 @@ public void setPosition(Pose2d position) {

/**
* Sets the rotation of the robot on the field.
*
*
* @param rotation the rotation of the robot on the field.
*/
public void setRotation(Rotation2d rotation) {
Expand Down Expand Up @@ -148,6 +148,7 @@ public Twist2d getVelocity() {
chassisSpeeds.vxMetersPerSecond * rotation.getSin()
+ chassisSpeeds.vyMetersPerSecond * rotation.getCos();

return new Twist2d(xVelocityMetersPerSecond, yVelocityMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond);
return new Twist2d(
xVelocityMetersPerSecond, yVelocityMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond);
}
}
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/swerve/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,8 @@ public void execute() {
double maxOmegaRadiansPerSecond = SwerveConstants.MAXIMUM_ROTATION_SPEED.getRadians();

if (Math.abs(chassisSpeeds.omegaRadiansPerSecond) > maxOmegaRadiansPerSecond) {
chassisSpeeds.omegaRadiansPerSecond = Math.signum(chassisSpeeds.omegaRadiansPerSecond) * maxOmegaRadiansPerSecond;
chassisSpeeds.omegaRadiansPerSecond =
Math.signum(chassisSpeeds.omegaRadiansPerSecond) * maxOmegaRadiansPerSecond;
}

swerve.setChassisSpeeds(chassisSpeeds);
Expand Down
10 changes: 4 additions & 6 deletions src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.CoastOut;
import com.ctre.phoenix6.controls.VoltageOut;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
Expand Down Expand Up @@ -56,17 +55,16 @@ public void configure() {
public void setSetpoint(double positionRotations) {
if (pidf.atGoal()) {
// TODO Doesn't work for some reason...
//talonFX.setControl(new CoastOut());
//return;
}
// talonFX.setControl(new CoastOut());
// return;
}

Rotation2d measuredPosition =
Rotation2d.fromRotations(
BaseStatusSignal.getLatencyCompensatedValue(
this.positionRotations, this.velocityRotationsPerSecond));

double voltage =
pidf.calculate(measuredPosition, Rotation2d.fromRotations(positionRotations));
double voltage = pidf.calculate(measuredPosition, Rotation2d.fromRotations(positionRotations));

talonFX.setControl(new VoltageOut(voltage));
}
Expand Down

0 comments on commit e282705

Please sign in to comment.