Hello, I’m trying to figure out how path planner 2024 works, I’ve been able to simulate my robot code and see it following the right path but at the end of the path it just starts spinning around. I’ve now updated my code to try and test it in real life but every single path returns the same thing: It just spins around. Here is my Subsystem and my RobotContainer. Do you guys know why it doesn’t return the right path?
(We use a differential Drive)
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.util.ReplanningConfig;
// Motors
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.DriverStation;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.SPI;
// Encoders
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
// Constants
import frc.robot.Constants;
public class BasePilotable extends SubsystemBase {
private final CANSparkMax m_LeftMotor1 = new CANSparkMax(Constants.BasePilotablePorts.kLeft1Port, MotorType.kBrushless);
private final CANSparkMax m_LeftMotor2 = new CANSparkMax(Constants.BasePilotablePorts.kLeft2Port, MotorType.kBrushless);
private final CANSparkMax m_RightMotor3 = new CANSparkMax(Constants.BasePilotablePorts.kRight1Port, MotorType.kBrushless);
private final CANSparkMax m_RightMotor4 = new CANSparkMax(Constants.BasePilotablePorts.kRight2Port, MotorType.kBrushless);
private RelativeEncoder m_leftEncoder;
private RelativeEncoder m_rightEncoder;
private final DifferentialDrive m_DifferentialDrive = new DifferentialDrive(m_RightMotor3, m_LeftMotor1);
//private final Encoder m_leftEncoder = new Encoder(0, 1);
//private final Encoder m_rightEncoder = new Encoder(2, 3);
private final PIDController m_leftPIDController = new PIDController(0.1, 0, 0);
private final PIDController m_rightPIDController = new PIDController(0.1, 0, 0);
//private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
private final AHRS m_gyroNavX;
private final DifferentialDriveOdometry m_odometry;
private final DifferentialDriveKinematics m_kinematics = new DifferentialDriveKinematics(Constants.kinematics.kTrackwidthMeters);
public DifferentialDrivetrainSim m_drivetrainSimulator;
ChassisSpeeds chassisSpeeds = new ChassisSpeeds(2.0, 0, 1.0);
DifferentialDriveWheelSpeeds wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
double leftVelocity = wheelSpeeds.leftMetersPerSecond;
double rightVelocity = wheelSpeeds.rightMetersPerSecond;
ChassisSpeeds speedsDIFF = new ChassisSpeeds(1.0, 0.0, 1.5);
public BasePilotable() {
SendableRegistry.addChild(m_DifferentialDrive, m_LeftMotor1);
SendableRegistry.addChild(m_DifferentialDrive, m_RightMotor3);
m_LeftMotor1.restoreFactoryDefaults();
m_LeftMotor2.restoreFactoryDefaults();
m_RightMotor3.restoreFactoryDefaults();
m_RightMotor4.restoreFactoryDefaults();
m_RightMotor3.setInverted(true);
m_RightMotor4.setInverted(true);
m_LeftMotor2.follow(m_LeftMotor1);
m_RightMotor4.follow(m_RightMotor3);
m_gyroNavX = new AHRS(SPI.Port.kMXP);
m_DifferentialDrive.setSafetyEnabled(false);
m_leftEncoder = m_LeftMotor1.getEncoder();
m_rightEncoder = m_RightMotor3.getEncoder();
m_leftEncoder.setPositionConversionFactor(Constants.encoders.kEncoderDistancePerPulse);
m_rightEncoder.setPositionConversionFactor(Constants.encoders.kEncoderDistancePerPulse);
m_leftEncoder.setVelocityConversionFactor(Constants.encoders.kEncoderDistancePerPulse/60);
m_rightEncoder.setVelocityConversionFactor(Constants.encoders.kEncoderDistancePerPulse/60);
resetEncoders();
// Odometry
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getPosition(), m_rightEncoder.getPosition());
// AutoBuilder
// Configure AutoBuilder last
AutoBuilder.configureRamsete(
this::getPose, // Robot pose supplier
this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose)
this::getSpeeds, // Current ChassisSpeeds supplier
this::toWheelSpeeds, // Method that will drive the robot given ChassisSpeeds
new ReplanningConfig(), // Default path replanning config. See the API for the options here
() -> {
// Boolean supplier that controls when the path will be mirrored for the red alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this // Reference to this subsystem to set requirements
);
}
public void resetEncoders() {
m_leftEncoder.setPosition(0);
m_rightEncoder.setPosition(0);
}
@Override
public void periodic() {
m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getPosition(), m_rightEncoder.getPosition());
//m_fieldSim.setRobotPose(getPose());
// Values
SmartDashboard.putNumber("EncodeurGaucheMetre", m_leftEncoder.getPosition());
SmartDashboard.putNumber("EncodeurDroiteMetre", m_rightEncoder.getPosition());
SmartDashboard.putNumber("EncodeurGaucheTicks", m_leftEncoder.getCountsPerRevolution());
SmartDashboard.putNumber("EncodeurDroiteTicks", m_rightEncoder.getCountsPerRevolution());
SmartDashboard.putNumber("Degrees: ", getHeading());
SmartDashboard.putNumber("Gyro", getHeading());
}
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
public void resetOdometry(Pose2d pose) {
resetEncoders();
m_odometry.resetPosition(
Rotation2d.fromDegrees(getHeading()),
m_leftEncoder.getPosition(),
m_rightEncoder.getPosition(),
pose);
}
/*
public double getHeading() {
return Math.IEEEremainder(m_gyro.getAngle(), 360) * (Constants.kGyroReversed ? -1.0 : 1.0);
}
*/
public double getHeading() {
return m_gyroNavX.getAngle();
}
public ChassisSpeeds getSpeeds() {
return m_kinematics.toChassisSpeeds(wheelSpeeds);
}
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds(m_rightEncoder.getVelocity(), m_leftEncoder.getVelocity());
}
public void toWheelSpeeds(ChassisSpeeds chassisSpeeds)
{
m_DifferentialDrive.feed();
m_leftPIDController.setSetpoint(chassisSpeeds.vxMetersPerSecond);
m_rightPIDController.setSetpoint(chassisSpeeds.omegaRadiansPerSecond);
// m_DifferentialDrive.arcadeDrive(m_leftPIDController.calculate(m_kinematics.toChassisSpeeds(getWheelSpeeds()).vxMetersPerSecond), m_rightPIDController.calculate(m_kinematics.toChassisSpeeds(getWheelSpeeds()).omegaRadiansPerSecond));
m_DifferentialDrive.arcadeDrive(0.5, 0.5);
}
public void arcadeDrive(double fwd, double rot){
m_DifferentialDrive.arcadeDrive(fwd, rot);
}
public void tankDrive(double leftSpeed, double rightSpeed){
m_DifferentialDrive.tankDrive(leftSpeed, rightSpeed);
}
public void basePilotableStop(){
m_DifferentialDrive.stopMotor();
}
}
package frc.robot;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.commands.stop;
import frc.robot.subsystems.BasePilotable;
public class RobotContainer {
private final BasePilotable m_BasePilotable_SS = new BasePilotable();
SendableChooser<Command> autoChooser = new SendableChooser<>();
public RobotContainer() {
autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
configureBindings();
}
private void configureBindings() {
}
public Command getAutonomousCommand() {
return autoChooser.getSelected();
}
}