diff --git a/simgui-ds.json b/simgui-ds.json index e3207aa..07beb58 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -9,11 +9,6 @@ "visible": true } }, - "System Joysticks": { - "window": { - "visible": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ @@ -106,9 +101,15 @@ } ], "robotJoysticks": [ - {}, + { + "guid": "Keyboard1" + }, { "guid": "Keyboard0" + }, + {}, + { + "guid": "Keyboard2" } ] } diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index a4baf31..0228ef3 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -9,6 +9,7 @@ import static org.sciborgs1155.robot.Constants.PERIOD; import static org.sciborgs1155.robot.drive.DriveConstants.*; +import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -60,6 +61,12 @@ public Robot() { super(PERIOD.in(Seconds)); configureGameBehavior(); configureBindings(); + configureLog(); + } + + private void configureLog() { + SignalLogger.setPath("./logs/"); + SignalLogger.enableAutoLogging(true); } /** Configures basic behavior for different periods during the game. */ @@ -138,6 +145,10 @@ private void configureBindings() { .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER)); // TODO: Add any additional bindings. + operator + .a() + .onTrue(Commands.runOnce(SignalLogger::start)) + .onFalse(Commands.runOnce(SignalLogger::stop)); } /** diff --git a/src/main/java/org/sciborgs1155/robot/drive/Drive.java b/src/main/java/org/sciborgs1155/robot/drive/Drive.java index a0357e2..426b6e2 100644 --- a/src/main/java/org/sciborgs1155/robot/drive/Drive.java +++ b/src/main/java/org/sciborgs1155/robot/drive/Drive.java @@ -11,6 +11,7 @@ import static org.sciborgs1155.robot.Ports.Drive.*; import static org.sciborgs1155.robot.drive.DriveConstants.*; +import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.controller.PIDController; @@ -149,7 +150,11 @@ public Drive( translationCharacterization = new SysIdRoutine( - new SysIdRoutine.Config(), + new SysIdRoutine.Config( + null, + Volts.of(4), + null, + (state) -> SignalLogger.writeString("translation state", state.toString())), new SysIdRoutine.Mechanism( volts -> modules.forEach( @@ -159,7 +164,11 @@ public Drive( "translation")); rotationalCharacterization = new SysIdRoutine( - new SysIdRoutine.Config(), + new SysIdRoutine.Config( + null, + Volts.of(4), + null, + (state) -> SignalLogger.writeString("rotation state", state.toString())), new SysIdRoutine.Mechanism( volts -> { this.frontLeft.updateInputs( diff --git a/src/main/java/org/sciborgs1155/robot/drive/TalonModule.java b/src/main/java/org/sciborgs1155/robot/drive/TalonModule.java index e211354..6023927 100644 --- a/src/main/java/org/sciborgs1155/robot/drive/TalonModule.java +++ b/src/main/java/org/sciborgs1155/robot/drive/TalonModule.java @@ -6,24 +6,15 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; +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 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; @@ -31,16 +22,18 @@ public class TalonModule implements ModuleIO { private final TalonFX driveMotor; // Kraken X60 - private final CANSparkMax turnMotor; // NEO 550 + private final TalonFX turnMotor; // Kraken X60 -- NEW private final StatusSignal drivePos; private final StatusSignal driveVelocity; - private final SparkAbsoluteEncoder turningEncoder; + // private final SparkAbsoluteEncoder turningEncoder; private final VelocityVoltage velocityOut = new VelocityVoltage(0); + private final PositionVoltage radiansOut = new PositionVoltage(0); - private final SparkPIDController turnPID; + // private final SparkPIDController turnPID; private final SimpleMotorFeedforward driveFF; + private final SimpleMotorFeedforward turnFF; private final Rotation2d angularOffset; @@ -52,73 +45,47 @@ public class TalonModule implements ModuleIO { public TalonModule(int drivePort, int turnPort, Rotation2d angularOffset, String name) { driveMotor = new TalonFX(drivePort); + turnMotor = new TalonFX(turnPort); + drivePos = driveMotor.getPosition(); 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); drivePos.setUpdateFrequency(1 / SENSOR_PERIOD.in(Seconds)); driveVelocity.setUpdateFrequency(1 / SENSOR_PERIOD.in(Seconds)); - TalonFXConfiguration talonConfig = new TalonFXConfiguration(); + TalonFXConfiguration talonDriveConfig = new TalonFXConfiguration(); + TalonFXConfiguration talonTurnConfig = new TalonFXConfiguration(); // reset config - driveMotor.getConfigurator().apply(talonConfig); + driveMotor.getConfigurator().apply(talonDriveConfig); + turnMotor.getConfigurator().apply(talonTurnConfig); - talonConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - talonConfig.Feedback.SensorToMechanismRatio = Driving.POSITION_FACTOR.in(Meters); - talonConfig.CurrentLimits.SupplyCurrentLimit = Driving.CURRENT_LIMIT.in(Amps); + talonDriveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + talonDriveConfig.Feedback.SensorToMechanismRatio = Driving.POSITION_FACTOR.in(Meters); + talonDriveConfig.CurrentLimits.SupplyCurrentLimit = Driving.CURRENT_LIMIT.in(Amps); - talonConfig.Slot0.kP = Driving.PID.TALON.P; - talonConfig.Slot0.kI = Driving.PID.TALON.I; - talonConfig.Slot0.kD = Driving.PID.TALON.D; + talonTurnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + talonTurnConfig.Feedback.SensorToMechanismRatio = Turning.POSITION_FACTOR.in(Radians); + talonTurnConfig.CurrentLimits.SupplyCurrentLimit = Turning.CURRENT_LIMIT.in(Amps); - driveMotor.getConfigurator().apply(talonConfig); + talonDriveConfig.Slot0.kP = Driving.PID.TALON.P; + talonDriveConfig.Slot0.kI = Driving.PID.TALON.I; + talonDriveConfig.Slot0.kD = Driving.PID.TALON.D; - TalonUtils.addMotor(driveMotor); + talonTurnConfig.Slot0.kP = Turning.PID.P; + talonTurnConfig.Slot0.kI = Turning.PID.I; + talonTurnConfig.Slot0.kD = Turning.PID.D; - turnMotor = new CANSparkMax(turnPort, MotorType.kBrushless); - turningEncoder = turnMotor.getAbsoluteEncoder(); - turnPID = turnMotor.getPIDController(); - - check(turnMotor, turnMotor.restoreFactoryDefaults()); - - check(turnMotor, turnPID.setP(Turning.PID.P)); - check(turnMotor, turnPID.setI(Turning.PID.I)); - check(turnMotor, turnPID.setD(Turning.PID.D)); - check(turnMotor, turnPID.setPositionPIDWrappingEnabled(true)); - check(turnMotor, turnPID.setPositionPIDWrappingMinInput(-Math.PI)); - check(turnMotor, turnPID.setPositionPIDWrappingMaxInput(Math.PI)); - check(turnMotor, turnPID.setFeedbackDevice(turningEncoder)); - - check(turnMotor, turnMotor.setIdleMode(IdleMode.kBrake)); - check(turnMotor, turnMotor.setSmartCurrentLimit((int) Turning.CURRENT_LIMIT.in(Amps))); - turningEncoder.setInverted(Turning.ENCODER_INVERTED); - check(turnMotor); - check( - turnMotor, turningEncoder.setPositionConversionFactor(Turning.POSITION_FACTOR.in(Radians))); - check( - turnMotor, - turningEncoder.setVelocityConversionFactor(Turning.VELOCITY_FACTOR.in(RadiansPerSecond))); - check(turnMotor, turningEncoder.setAverageDepth(2)); - check( - turnMotor, - SparkUtils.configureFrameStrategy( - turnMotor, - Set.of(Data.POSITION, Data.VELOCITY, Data.APPLIED_OUTPUT), - Set.of(Sensor.ABSOLUTE), - false)); - SparkUtils.addChecker( - () -> - check( - turnMotor, - SparkUtils.configureFrameStrategy( - turnMotor, - Set.of(Data.POSITION, Data.VELOCITY, Data.APPLIED_OUTPUT), - Set.of(Sensor.ABSOLUTE), - false))); - check(turnMotor, turnMotor.burnFlash()); + driveMotor.getConfigurator().apply(talonDriveConfig); + turnMotor.getConfigurator().apply(talonTurnConfig); register(turnMotor); + register(driveMotor); + + TalonUtils.addMotor(driveMotor); + TalonUtils.addMotor(turnMotor); resetEncoders(); @@ -154,10 +121,8 @@ public double driveVelocity() { @Override public Rotation2d rotation() { lastRotation = - SparkUtils.wrapCall( - turnMotor, - Rotation2d.fromRadians(turningEncoder.getPosition()).minus(angularOffset)) - .orElse(lastRotation); + Rotation2d.fromRadians(turnMotor.getRotorPosition().getValueAsDouble()) + .minus(angularOffset); return lastRotation; } @@ -189,7 +154,7 @@ public void setDriveSetpoint(double velocity) { @Override public void setTurnSetpoint(double angle) { - turnPID.setReference(angle, ControlType.kPosition); + turnMotor.setControl(radiansOut.withPosition(angle).withFeedForward(turnFF.calculate(angle))); } @Override