Hi, I have been having issues with setting the angle offsets in our cancoders for a swerve bot. The robot drives completely as expected but with incorrectly positioned wheels, and changing the magnetoffset and angleoffset in the phoenix tuner and code respectively does not change the position of the wheels. One notable log error is “CTRE: CAN frame not received/too-stale,” which I have not been able to find any solutions for that are related to our issue.
The SwerveModule subsystem code:
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import io.github.oblarg.oblog.annotations.Log;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.ctre.phoenix.sensors.CANCoder;
public class SwerveModule extends SubsystemBase{
private PIDController anglePID = new PIDController(0.005, 0, 0);
@Log
private double pidOut;
private CANSparkMax drive;
private CANSparkMax rotate;
private RelativeEncoder encoder;
private CANCoder canCoder;
private double angleOffset;
public SwerveModule(int driveMotorID, int turnMotorID, int canCoderID, double angleOffset) {
drive = new CANSparkMax(driveMotorID, MotorType.kBrushless);
rotate = new CANSparkMax(turnMotorID, MotorType.kBrushless);
this.canCoder = new CANCoder(canCoderID);
encoder = rotate.getEncoder();
encoder.setPosition(canCoder.getAbsolutePosition());
anglePID.enableContinuousInput(0, 360);
this.angleOffset = angleOffset;
}
public void setState(SwerveModuleState state){
state = SwerveModuleState.optimize(state, new Rotation2d(canCoder.getAbsolutePosition()*Math.PI/180)); //try *Math.PI/180
drive.set(state.speedMetersPerSecond/2.5);
pidOut = anglePID.calculate(canCoder.getAbsolutePosition()+this.angleOffset, state.angle.getDegrees());
//System.out.println(String.format(canCoder.getAbsolutePosition() + "-" + state.angle.getDegrees() + "-" + pidOut));
rotate.set(pidOut/8);
}
public double getTurningPosition() {
return encoder.getPosition();
//return getAbsoluteEncoderRad();
}
public double getTurningVelocity() {
return encoder.getVelocity();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
The Swerve subsystem code:
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import com.ctre.phoenix.sensors.PigeonIMU;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class Swerve extends SubsystemBase {
/** Creates a new Swerve. */
Translation2d m0 = new Translation2d(Constants.trackWidth/2, Constants.wheelBase/2);
Translation2d m1 = new Translation2d(Constants.trackWidth/2, -Constants.wheelBase/2);
Translation2d m2 = new Translation2d(-Constants.trackWidth/2, Constants.wheelBase/2);
Translation2d m3 = new Translation2d(-Constants.trackWidth/2, -Constants.wheelBase/2);
//test!
SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m0, m1, m2, m3);
//im literally making changes right now hello?
SwerveModule Mod_0 = new SwerveModule(Constants.Mod0.driveMotor, Constants.Mod0.turnMotor, Constants.Mod0.canCoder, Constants.Mod0.angleOffset);
SwerveModule Mod_1 = new SwerveModule(Constants.Mod1.driveMotor, Constants.Mod1.turnMotor, Constants.Mod1.canCoder, Constants.Mod1.angleOffset);
SwerveModule Mod_2 = new SwerveModule(Constants.Mod2.driveMotor, Constants.Mod2.turnMotor, Constants.Mod2.canCoder, Constants.Mod2.angleOffset);
SwerveModule Mod_3 = new SwerveModule(Constants.Mod3.driveMotor, Constants.Mod3.turnMotor, Constants.Mod3.canCoder, Constants.Mod3.angleOffset);
public Swerve() {
}
public void drive(double y, double x, double w, PigeonIMU gyro){
Rotation2d gyroAngle = new Rotation2d((Math.abs(gyro.getYaw())%360)/57.2958); //gyro abs angle in rads
System.out.println(gyroAngle);
ChassisSpeeds speeds = new ChassisSpeeds(y, x, w);
//ChassisSpeeds frspeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, gyroAngle);
SwerveModuleState states = kinematics.toSwerveModuleStates(speeds);
Mod_0.setState(states[0]);
Mod_1.setState(states[1]);
Mod_2.setState(states[2]);
Mod_3.setState(states[3]);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
Any help is appreciated, thank you for your time.