Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
package frc.robot.subsystems.swervev3.io;

import com.ctre.phoenix.sensors.WPI_CANCoder;
import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.CANSparkMax;
import frc.robot.constants.Constants;
import frc.robot.swervev2.KinematicsConversionConfig;
Expand All @@ -11,13 +11,13 @@
public class SparkMaxModuleIO implements ModuleIO {
private final CANSparkMax driveMotor;
private final CANSparkMax steerMotor;
private final WPI_CANCoder absEncoder;
private final CANcoder absEncoder;
private final AtomicLong steerOffset = new AtomicLong(0);

public SparkMaxModuleIO(SwerveIdConfig motorConfig, KinematicsConversionConfig conversionConfig, boolean driveInverted, boolean steerInverted) {
driveMotor = new CANSparkMax(motorConfig.getDriveMotorId(), CANSparkMax.MotorType.kBrushless);
steerMotor = new CANSparkMax(motorConfig.getTurnMotorId(), CANSparkMax.MotorType.kBrushless);
absEncoder = new WPI_CANCoder(motorConfig.getCanCoderId());
absEncoder = new CANcoder(motorConfig.getCanCoderId());
setMotorConfig(driveInverted, steerInverted);
setConversionFactors(conversionConfig);
resetEncoder();
Expand Down Expand Up @@ -58,7 +58,7 @@ public void setSteerVoltage(double volts) {
@Override
public void setSteerOffset(double zeroAbs) {
steerMotor.getEncoder().setPosition(0);
steerOffset.set(Double.doubleToLongBits(normalizeAngle(Math.toRadians(zeroAbs - absEncoder.getAbsolutePosition()))));
steerOffset.set(Double.doubleToLongBits(normalizeAngle(Math.toRadians(zeroAbs - absEncoder.getAbsolutePosition().getValueAsDouble()))));
}

private double normalizeAngle(double angleInRad) {
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.robot.swervev2;

import com.ctre.phoenix.sensors.WPI_CANCoder;

import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.CANSparkMax;
import frc.robot.constants.Constants;
import frc.robot.swervev2.components.EncodedSwerveSparkMax;
Expand All @@ -22,7 +23,7 @@ public EncodedSwerveMotorBuilder(SwerveIdConfig motorConfig, KinematicsConversio
public EncodedSwerveSparkMax build(){
CANSparkMax driveMotor = new CANSparkMax(motorConfig.getDriveMotorId(), CANSparkMax.MotorType.kBrushless);
CANSparkMax turnMotor = new CANSparkMax(motorConfig.getTurnMotorId(), CANSparkMax.MotorType.kBrushless);
WPI_CANCoder canCoder = new WPI_CANCoder(motorConfig.getCanCoderId());
CANcoder canCoder = new CANcoder(motorConfig.getCanCoderId());
double driveVelConvFactor = (2 * conversionConfig.getWheelRadius() * Math.PI) / (conversionConfig.getDriveGearRatio() * 60);
double drivePosConvFactor = (2 * conversionConfig.getWheelRadius() * Math.PI) / (conversionConfig.getDriveGearRatio());
double steerPosConvFactor = 2 * Math.PI / Constants.SWERVE_MODULE_PROFILE.getSteerRatio();
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
package frc.robot.swervev2.components;

import com.ctre.phoenix.sensors.WPI_CANCoder;
import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.CANSparkMax;
import frc.robot.constants.Constants;

public class EncodedSwerveSparkMax extends GenericEncodedSwerve {
public EncodedSwerveSparkMax(CANSparkMax driveMotor, CANSparkMax steerMotor, WPI_CANCoder absEncoder, double driveVelFactor, double drivePosFactor, double steerPosFactor) {
public EncodedSwerveSparkMax(CANSparkMax driveMotor, CANSparkMax steerMotor, CANcoder absEncoder, double driveVelFactor, double drivePosFactor, double steerPosFactor) {
super(driveMotor, steerMotor, absEncoder, driveMotor.getEncoder(), steerMotor.getEncoder(), driveVelFactor, drivePosFactor, steerPosFactor);
configureEncoders(driveVelFactor,drivePosFactor, steerPosFactor);
}
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
package frc.robot.swervev2.components;

import com.ctre.phoenix.sensors.CANCoderStatusFrame;
import com.ctre.phoenix.sensors.WPI_CANCoder;

import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
Expand All @@ -17,9 +17,9 @@ public class GenericEncodedSwerve implements SwerveMotor, SwerveMotorEncoder {
private final RelativeEncoder driveEncoder;
private final RelativeEncoder steerEncoder;
private double steerOffset = 0;
private final WPI_CANCoder absEncoder;
private final CANcoder absEncoder;

public GenericEncodedSwerve(MotorController driveMotor, MotorController steerMotor, WPI_CANCoder absEncoder, RelativeEncoder driveEncoder, RelativeEncoder steerEncoder,
public GenericEncodedSwerve(MotorController driveMotor, MotorController steerMotor, CANcoder absEncoder, RelativeEncoder driveEncoder, RelativeEncoder steerEncoder,
double driveVelFactor, double drivePosFactor, double steerPosFactor) {
this.driveMotor = driveMotor;
this.steerMotor = steerMotor;
Expand All @@ -41,7 +41,7 @@ public void configureEncoders(double driveVelFactor, double drivePosFactor, doub
driveEncoder.setPositionConversionFactor(drivePosFactor);
steerEncoder.setPositionConversionFactor(steerPosFactor);
steerEncoder.setVelocityConversionFactor(steerPosFactor/60);
absEncoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData,50);
absEncoder.getPosition().setUpdateFrequency(50); //We think its 50
}

@Override
Expand Down Expand Up @@ -89,7 +89,7 @@ public double getSteerOffset() {
@Override
public void setSteerOffset(double zeroAbs) {
steerEncoder.setPosition(0);
steerOffset = Math.toRadians(zeroAbs - absEncoder.getAbsolutePosition());
steerOffset = Math.toRadians(zeroAbs - absEncoder.getAbsolutePosition().getValueAsDouble());
steerOffset = normalizeAngle(steerOffset);
}
public SwerveModulePosition getPosition(){
Expand All @@ -108,7 +108,7 @@ private double normalizeAngle(double angleInRad){
return angleInRad;
}

public WPI_CANCoder getAbsEnc() {
public CANcoder getAbsEnc() {
return absEncoder;
}

Expand Down
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
package frc.robot.utils.diag;

import com.ctre.phoenix.sensors.WPI_CANCoder;
import com.ctre.phoenix6.hardware.CANcoder;

/**
* A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has traveled at least a given
* distance from its initial position (measured at initialization or after a reset)
*/
public class DiagSparkMaxAbsEncoder extends DiagDistanceTraveled {

private WPI_CANCoder canCoder;
private CANcoder canCoder;

/**
* Constructor
Expand All @@ -17,14 +17,14 @@ public class DiagSparkMaxAbsEncoder extends DiagDistanceTraveled {
* @param requiredTravel - the required difference between the initial position to qualify for success
* @param canSparkMax - the encoder instance to test
*/
public DiagSparkMaxAbsEncoder(String title, String name, double requiredTravel, WPI_CANCoder canCoder) {
public DiagSparkMaxAbsEncoder(String title, String name, double requiredTravel, CANcoder canCoder) {
super(title, name, requiredTravel);
this.canCoder = canCoder;
reset();
}

@Override
protected double getCurrentValue() {
return canCoder.getAbsolutePosition();
return canCoder.getAbsolutePosition().getValueAsDouble();
}
}
42 changes: 0 additions & 42 deletions vendordeps/photonlib.json

This file was deleted.