Skip to content

Commit

Permalink
spotless...
Browse files Browse the repository at this point in the history
  • Loading branch information
michaelx0281 committed Nov 18, 2024
1 parent d6b52ed commit 897db30
Showing 1 changed file with 6 additions and 21 deletions.
27 changes: 6 additions & 21 deletions src/main/java/org/sciborgs1155/robot/drive/TalonModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,28 +6,15 @@

import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkAbsoluteEncoder;
import com.revrobotics.SparkPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.motorcontrol.Talon;

import java.util.Set;
import monologue.Annotations.Log;
import org.sciborgs1155.lib.SparkUtils;
import org.sciborgs1155.lib.SparkUtils.Data;
import org.sciborgs1155.lib.SparkUtils.Sensor;
import org.sciborgs1155.lib.TalonUtils;
import org.sciborgs1155.robot.drive.DriveConstants.ControlMode;
import org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.Driving;
Expand Down Expand Up @@ -64,8 +51,7 @@ public TalonModule(int drivePort, int turnPort, Rotation2d angularOffset, String
driveVelocity = driveMotor.getVelocity();
driveFF =
new SimpleMotorFeedforward(Driving.FF.TALON.S, Driving.FF.TALON.V, Driving.FF.TALON.A);
turnFF =
new SimpleMotorFeedforward(Turning.FF.S, Turning.FF.V, Turning.FF.A);
turnFF = new SimpleMotorFeedforward(Turning.FF.S, Turning.FF.V, Turning.FF.A);

drivePos.setUpdateFrequency(1 / SENSOR_PERIOD.in(Seconds));
driveVelocity.setUpdateFrequency(1 / SENSOR_PERIOD.in(Seconds));
Expand All @@ -76,7 +62,6 @@ public TalonModule(int drivePort, int turnPort, Rotation2d angularOffset, String
driveMotor.getConfigurator().apply(talonDriveConfig);
turnMotor.getConfigurator().apply(talonTurnConfig);


talonDriveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
talonDriveConfig.Feedback.SensorToMechanismRatio = Driving.POSITION_FACTOR.in(Meters);
talonDriveConfig.CurrentLimits.SupplyCurrentLimit = Driving.CURRENT_LIMIT.in(Amps);
Expand All @@ -88,7 +73,7 @@ public TalonModule(int drivePort, int turnPort, Rotation2d angularOffset, String
talonDriveConfig.Slot0.kP = Driving.PID.TALON.P;
talonDriveConfig.Slot0.kI = Driving.PID.TALON.I;
talonDriveConfig.Slot0.kD = Driving.PID.TALON.D;

talonTurnConfig.Slot0.kP = Turning.PID.P;
talonTurnConfig.Slot0.kI = Turning.PID.I;
talonTurnConfig.Slot0.kD = Turning.PID.D;
Expand Down Expand Up @@ -132,7 +117,9 @@ public double driveVelocity() {

@Override
public Rotation2d rotation() {
lastRotation = Rotation2d.fromRadians(turnMotor.getRotorPosition().getValueAsDouble()).minus(angularOffset);
lastRotation =
Rotation2d.fromRadians(turnMotor.getRotorPosition().getValueAsDouble())
.minus(angularOffset);
return lastRotation;
}

Expand Down Expand Up @@ -164,9 +151,7 @@ public void setDriveSetpoint(double velocity) {

@Override
public void setTurnSetpoint(double angle) {
turnMotor.setControl(
radiansOut.withPosition(angle).withFeedForward(turnFF.calculate(angle));
)
turnMotor.setControl(radiansOut.withPosition(angle).withFeedForward(turnFF.calculate(angle)));
}

@Override
Expand Down

0 comments on commit 897db30

Please sign in to comment.