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

Migrate to Phoenix 6 #31

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
Open

Migrate to Phoenix 6 #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
137fa96
Migrate to phoenix6
rkanemoto Aug 23, 2023
9a6c403
Merge branch 'main' into phoenix6
rkanemoto Aug 23, 2023
d9740d9
Format with Spotless
rkanemoto Aug 23, 2023
f556901
Fix vendordeps
rkanemoto Aug 23, 2023
540dbb6
Make adjustments to phoenix v6 changes
rkanemoto Aug 26, 2023
2afac1e
Make further phoenix v6 adjustments
rkanemoto Aug 26, 2023
c99f617
Attempt at tuning swerve
rkanemoto Sep 5, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 15 additions & 15 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,9 @@
package frc.robot;

import com.ctre.phoenix.led.CANdle;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.Pigeon2;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -55,35 +55,35 @@ public class Robot extends LoggedRobot {
private final SwerveModule frontLeft =
new SwerveModule(
Config.SWERVE_FL_CONSTANTS,
new com.ctre.phoenixpro.hardware.TalonFX(
new com.ctre.phoenix6.hardware.TalonFX(
Config.SWERVE_FL_DRIVE_MOTOR_ID, Config.CANIVORE_ID),
new com.ctre.phoenixpro.hardware.TalonFX(
new com.ctre.phoenix6.hardware.TalonFX(
Config.SWERVE_FL_STEER_MOTOR_ID, Config.CANIVORE_ID),
new CANCoder(Config.SWERVE_FL_CANCODER_ID, Config.CANIVORE_ID));
new CANcoder(Config.SWERVE_FL_CANCODER_ID, Config.CANIVORE_ID));
private final SwerveModule frontRight =
new SwerveModule(
Config.SWERVE_FR_CONSTANTS,
new com.ctre.phoenixpro.hardware.TalonFX(
new com.ctre.phoenix6.hardware.TalonFX(
Config.SWERVE_FR_DRIVE_MOTOR_ID, Config.CANIVORE_ID),
new com.ctre.phoenixpro.hardware.TalonFX(
new com.ctre.phoenix6.hardware.TalonFX(
Config.SWERVE_FR_STEER_MOTOR_ID, Config.CANIVORE_ID),
new CANCoder(Config.SWERVE_FR_CANCODER_ID, Config.CANIVORE_ID));
new CANcoder(Config.SWERVE_FR_CANCODER_ID, Config.CANIVORE_ID));
private final SwerveModule backLeft =
new SwerveModule(
Config.SWERVE_BL_CONSTANTS,
new com.ctre.phoenixpro.hardware.TalonFX(
new com.ctre.phoenix6.hardware.TalonFX(
Config.SWERVE_BL_DRIVE_MOTOR_ID, Config.CANIVORE_ID),
new com.ctre.phoenixpro.hardware.TalonFX(
new com.ctre.phoenix6.hardware.TalonFX(
Config.SWERVE_BL_STEER_MOTOR_ID, Config.CANIVORE_ID),
new CANCoder(Config.SWERVE_BL_CANCODER_ID, Config.CANIVORE_ID));
new CANcoder(Config.SWERVE_BL_CANCODER_ID, Config.CANIVORE_ID));
private final SwerveModule backRight =
new SwerveModule(
Config.SWERVE_BR_CONSTANTS,
new com.ctre.phoenixpro.hardware.TalonFX(
new com.ctre.phoenix6.hardware.TalonFX(
Config.SWERVE_BR_DRIVE_MOTOR_ID, Config.CANIVORE_ID),
new com.ctre.phoenixpro.hardware.TalonFX(
new com.ctre.phoenix6.hardware.TalonFX(
Config.SWERVE_BR_STEER_MOTOR_ID, Config.CANIVORE_ID),
new CANCoder(Config.SWERVE_BR_CANCODER_ID, Config.CANIVORE_ID));
new CANcoder(Config.SWERVE_BR_CANCODER_ID, Config.CANIVORE_ID));

private final DriveController driveController = new DriveController(Config.DRIVE_CONTROLLER_PORT);
private final CommandXboxController operatorController =
Expand Down
21 changes: 10 additions & 11 deletions src/main/java/frc/robot/config/Config.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,50 +56,49 @@ public class Config {
public static final SwerveModuleConstants SWERVE_FL_CONSTANTS =
IS_SPIKE
? new SwerveModuleConstants(
Rotation2d.fromDegrees(182.021484375), SwerveCorner.FRONT_LEFT, true, true)
Rotation2d.fromDegrees(5.625), SwerveCorner.FRONT_LEFT, true, true)
: new SwerveModuleConstants(
Rotation2d.fromDegrees(117.19), SwerveCorner.FRONT_LEFT, false, false);
// -62.84
public static final int SWERVE_FR_DRIVE_MOTOR_ID = 6;
public static final int SWERVE_FR_STEER_MOTOR_ID = 7;
public static final int SWERVE_FR_CANCODER_ID = 12;
public static final SwerveModuleConstants SWERVE_FR_CONSTANTS =
IS_SPIKE
? new SwerveModuleConstants(
Rotation2d.fromDegrees(159.521484375), SwerveCorner.FRONT_RIGHT, true, true)
Rotation2d.fromDegrees(2.285 + 90.0), SwerveCorner.FRONT_RIGHT, true, true)
: new SwerveModuleConstants(
Rotation2d.fromDegrees(32.2), SwerveCorner.FRONT_RIGHT, false, false);
// -147.8
public static final int SWERVE_BL_DRIVE_MOTOR_ID = 4;
public static final int SWERVE_BL_STEER_MOTOR_ID = 5;
public static final int SWERVE_BL_CANCODER_ID = 11;
public static final SwerveModuleConstants SWERVE_BL_CONSTANTS =
IS_SPIKE
? new SwerveModuleConstants(
Rotation2d.fromDegrees(2.548828125), SwerveCorner.BACK_LEFT, true, true)
Rotation2d.fromDegrees(9.581), SwerveCorner.BACK_LEFT, true, true)
: new SwerveModuleConstants(
Rotation2d.fromDegrees(-101.25), SwerveCorner.BACK_LEFT, false, false);
// 78.75
public static final int SWERVE_BR_DRIVE_MOTOR_ID = 2;
public static final int SWERVE_BR_STEER_MOTOR_ID = 3;
public static final int SWERVE_BR_CANCODER_ID = 10;
public static final SwerveModuleConstants SWERVE_BR_CONSTANTS =
IS_SPIKE
? new SwerveModuleConstants(
Rotation2d.fromDegrees(110.390625), SwerveCorner.BACK_RIGHT, true, true)
Rotation2d.fromDegrees(-7.20 + 90.0 + 180.0), SwerveCorner.BACK_RIGHT, true, true)
: new SwerveModuleConstants(
Rotation2d.fromDegrees(-75.42), SwerveCorner.BACK_RIGHT, false, false);
// 104.58
public static final int ELEVATOR_MOTOR_ID = 14;

public static final double ELEVATOR_GEARING = IS_SPIKE ? 7.2 : 16.0;
public static final double ELEVATOR_MIN_HEIGHT = IS_SPIKE ? 0 : 0;
public static final double ELEVATOR_MAX_HEIGHT = IS_SPIKE ? 27 : 12;

public static final double ELEVATOR_KV = IS_SPIKE ? 0 : 0;
public static final double ELEVATOR_KF = IS_SPIKE ? 0 : 0;
public static final double ELEVATOR_KP = IS_SPIKE ? 0.7 : 0.8;
public static final double ELEVATOR_KI = IS_SPIKE ? 0 : 0;
public static final double ELEVATOR_KD = IS_SPIKE ? 0.1 : 0;
public static final double ELEVATOR_ARB_F = IS_SPIKE ? 0.08 : 0;
public static final double ELEVATOR_KS = IS_SPIKE ? 0.08 : 0;

public static final int ELEVATOR_CRUISE_VELOCITY = IS_SPIKE ? 25000 : 15000;
public static final int ELEVATOR_ACCELERATION = IS_SPIKE ? 30000 : 27500;
public static final boolean ELEVATOR_INVERTED = IS_SPIKE ? false : true;
Expand All @@ -109,7 +108,7 @@ public class Config {

public static final int WRIST_MOTOR_ID = 16;
public static final double WRIST_GEARING = IS_SPIKE ? 25.0 * 2 : 48.0 * 2;
public static final int WRIST_KF = IS_SPIKE ? 0 : 0;
public static final int WRIST_KV = IS_SPIKE ? 0 : 0;
public static final double WRIST_KP = IS_SPIKE ? 0.5 : 0.1;
public static final int WRIST_KI = IS_SPIKE ? 0 : 0;
public static final int WRIST_KD = IS_SPIKE ? 0 : 0;
Expand All @@ -124,7 +123,7 @@ public class Config {
public static final boolean INVERTED_INTAKE = IS_SPIKE ? false : true;

public static final double SWERVE_STEER_KV = IS_SPIKE ? 0.0 : 0.0;
public static final double SWERVE_STEER_KP = IS_SPIKE ? 5.0 : 3.0;
public static final double SWERVE_STEER_KP = IS_SPIKE ? 30.0 : 3.0;
public static final double SWERVE_STEER_KI = IS_SPIKE ? 0.0 : 0.0;
public static final double SWERVE_STEER_KD = IS_SPIKE ? 0.0 : 0.0;
public static final double SWERVE_STEER_KS = IS_SPIKE ? 0.0 : 0.0;
Expand Down
93 changes: 53 additions & 40 deletions src/main/java/frc/robot/elevator/ElevatorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,10 @@

package frc.robot.elevator;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
Expand All @@ -21,30 +19,47 @@
import org.littletonrobotics.junction.Logger;

public class ElevatorSubsystem extends LifecycleSubsystem {
private static final double TOLERANCE = 0.5;
private static final double HOMING_CURRENT = 5;
private static final double ROTATIONS_PER_ELEVATOR_INCH = 1.0 / (1.75 * Math.PI);

private final TalonFX motor;
private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0);

private double goalPositionInInches = Positions.STOWED.height;
private double sensorUnitsPerElevatorInch = (Config.ELEVATOR_GEARING * 2048) / (1.75 * Math.PI);
private HomingState homingState = HomingState.NOT_HOMED;
private static final double TOLERANCE = 0.5;

public ElevatorSubsystem(TalonFX motor) {
super(SubsystemPriority.ELEVATOR);

this.motor = motor;
this.motor.setInverted(Config.ELEVATOR_INVERTED);

// Set pid for slot 0
this.motor.config_kF(0, Config.ELEVATOR_KF);
this.motor.config_kP(0, Config.ELEVATOR_KP);
this.motor.config_kI(0, Config.ELEVATOR_KI);
this.motor.config_kD(0, Config.ELEVATOR_KD);
// Set motion magic
this.motor.configMotionCruiseVelocity(Config.ELEVATOR_CRUISE_VELOCITY);
this.motor.configMotionAcceleration(Config.ELEVATOR_ACCELERATION);
// Set current limiting
this.motor.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 80, 100, 1));
this.motor.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 20, 30, 0.5));

TalonFXConfiguration motorConfig = new TalonFXConfiguration();
motorConfig.Slot0.kV = Config.ELEVATOR_KV;
motorConfig.Slot0.kP = Config.ELEVATOR_KP;
motorConfig.Slot0.kI = Config.ELEVATOR_KI;
motorConfig.Slot0.kD = Config.ELEVATOR_KD;
motorConfig.Slot0.kS = Config.ELEVATOR_KS;

motorConfig.MotorOutput.Inverted =
Config.ELEVATOR_INVERTED
? InvertedValue.Clockwise_Positive
: InvertedValue.CounterClockwise_Positive;

motorConfig.MotionMagic.MotionMagicCruiseVelocity = Config.ELEVATOR_CRUISE_VELOCITY;
motorConfig.MotionMagic.MotionMagicAcceleration = Config.ELEVATOR_ACCELERATION;

motorConfig.CurrentLimits.StatorCurrentLimit = 80;
motorConfig.CurrentLimits.StatorCurrentLimitEnable = true;

motorConfig.CurrentLimits.SupplyCurrentLimit = 20;
motorConfig.CurrentLimits.SupplyCurrentThreshold = 30;
motorConfig.CurrentLimits.SupplyTimeThreshold = 0.5;
motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;

motorConfig.Feedback.SensorToMechanismRatio = Config.ELEVATOR_GEARING;

motor.getConfigurator().apply(motorConfig);
}

public void startHoming() {
Expand All @@ -53,17 +68,16 @@ public void startHoming() {

public void resetHoming() {
homingState = HomingState.NOT_HOMED;
motor.set(TalonFXControlMode.PercentOutput, 0);
motor.disable();
}

public HomingState getHomingState() {
return homingState;
}

public double getHeight() {
// Read talon sensor, convert to inches
double sensorUnits = motor.getSelectedSensorPosition();
double position = sensorUnits / sensorUnitsPerElevatorInch;
double rotations = motor.getRotorPosition().getValue();
double position = rotations / ROTATIONS_PER_ELEVATOR_INCH;
return position;
}

Expand All @@ -85,36 +99,35 @@ public void setGoalPosition(double goal) {
@Override
public void robotPeriodic() {
Logger.getInstance().recordOutput("Elevator/Position", getHeight());
Logger.getInstance().recordOutput("Elevator/StatorCurrent", motor.getStatorCurrent());
Logger.getInstance().recordOutput("Elevator/SupplyCurrent", motor.getSupplyCurrent());
Logger.getInstance()
.recordOutput("Elevator/StatorCurrent", motor.getStatorCurrent().getValue());
Logger.getInstance()
.recordOutput("Elevator/SupplyCurrent", motor.getSupplyCurrent().getValue());
Logger.getInstance().recordOutput("Elevator/GoalPosition", goalPositionInInches);
Logger.getInstance().recordOutput("Elevator/Homing", homingState.toString());
Logger.getInstance().recordOutput("Elevator/AppliedVoltage", motor.getMotorOutputVoltage());
Logger.getInstance().recordOutput("Elevator/SensorVelocity", motor.getSelectedSensorVelocity());
Logger.getInstance().recordOutput("Elevator/TemperatureCelsius", motor.getTemperature());
Logger.getInstance().recordOutput("Elevator/AppliedDutyCycle", motor.getDutyCycle().getValue());
Logger.getInstance().recordOutput("Elevator/SensorVelocity", motor.getVelocity().getValue());
Logger.getInstance()
.recordOutput("Elevator/TemperatureCelsius", motor.getDeviceTemp().getValue());
}

@Override
public void enabledPeriodic() {
// Add homing sequence
// Convert goal in inches to sensor units, and set motor
if (homingState == HomingState.HOMING) {
motor.set(ControlMode.PercentOutput, -0.15);
double current = motor.getSupplyCurrent();
motor.set(-0.15);
double current = motor.getSupplyCurrent().getValue();
if (current > HOMING_CURRENT) {
motor.setSelectedSensorPosition(0);
motor.setRotorPosition(0);
homingState = HomingState.HOMED;
goalPositionInInches = Positions.STOWED.height;
}
} else if (homingState == HomingState.HOMED) {
double goalPositionInSensorUnits = goalPositionInInches * sensorUnitsPerElevatorInch;
motor.set(
ControlMode.MotionMagic,
goalPositionInSensorUnits,
DemandType.ArbitraryFeedForward,
Config.ELEVATOR_ARB_F);
double goalPosition = goalPositionInInches * ROTATIONS_PER_ELEVATOR_INCH;
motor.setControl(motionMagic.withPosition(goalPosition));
} else {
motor.set(ControlMode.PercentOutput, 0);
motor.disable();
}
}

Expand Down
20 changes: 15 additions & 5 deletions src/main/java/frc/robot/imu/ImuSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,9 @@

package frc.robot.imu;

import com.ctre.phoenix.sensors.Pigeon2;
import com.ctre.phoenix6.configs.MountPoseConfigs;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
Expand All @@ -19,6 +21,10 @@ public ImuSubsystem(Pigeon2 imu) {
super(SubsystemPriority.IMU);

this.imu = imu;

Pigeon2Configuration imuConfig = new Pigeon2Configuration();

imu.getConfigurator().apply(imuConfig);
}

public void robotPeriodic() {
Expand All @@ -28,17 +34,21 @@ public void robotPeriodic() {
}

public Rotation2d getRobotHeading() {
return Rotation2d.fromDegrees(imu.getYaw());
return Rotation2d.fromDegrees(imu.getYaw().getValue());
}

public Rotation2d getRoll() {
return Rotation2d.fromDegrees(imu.getRoll());
return Rotation2d.fromDegrees(imu.getRoll().getValue());
}

public void zero() {

setAngle(new Rotation2d());
this.imu.configMountPoseRoll(this.imu.getRoll());

MountPoseConfigs mountPoseConfig = new MountPoseConfigs();

mountPoseConfig.MountPoseRoll = getRoll().getDegrees();

imu.getConfigurator().apply(mountPoseConfig);
}

public void setAngle(Rotation2d zeroAngle) {
Expand Down
Loading