While attempting to get my mecanum drive XRP to run “Field Oriented” I have run into an issue and an anomaly with the XRPGyro. When I use the getRotation2d, in the following line, the getRotation2d appears to be inverted:
m_robotDrive.driveCartesian(-strafe, -forward, rotate, m_gyro.getRotation2d());
While trouble shooting the issue I have an anomaly that popped up. The getRotation2d call returns numbers from 0 to 250 and then “flips” to -119 to 0. I expected the numbers to flip at 180 or just continue to 360 and either wrap to 361 or reset (as if modulus 360) to0 again after 359. I’m using integers as an example to simplify the description.
The observed results while driving the robot via teleop/xbox controller are as follows:
The robot runs inverted (front is back and back is front) when just using the X axis.
Y seems normal until the robot is rotated (Z axis) then is inverted.
The robot does not drive “Field Oriented”. If the robot is rotated the controls do not follow the rotation. The controls continue as they were. Just as if they are set for robot oriented driving.
On the Simulation Driver’s Station, the XRPGyro is displayed via the “hardware” tab shows as 180 degrees off from the original calibration at all times.
I am NOT a programmer. I am trying to understand the XRP functions in an attempt to help our new programmers get started in the programming journey. My code is a smashup of WPILib samples and Google searches. I have added print statements in an effort figure out what is going on. I first though the reset gyro method was not working because the dashboard does not change when it is executed. But using the printf and println methods, I do see it working.
The code is not command based so it pretty much all resides in the robot.java file. I’ll now attemt to add it here:
// 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;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.xrp.XRPMotor;
import edu.wpi.first.wpilibj.xrp.XRPGyro;
/** This is a demo program showing how to use Mecanum control with the MecanumDrive class. */
public class Robot extends TimedRobot {
private static final int kFrontLeftChannel = 3;
private static final int kRearLeftChannel = 1;
private static final int kFrontRightChannel = 2;
private static final int kRearRightChannel = 0;
private XRPGyro m_gyro; // Declare your gyro object
// A flag to ensure the gyro only resets once per button press
private boolean m_wasButtonPressed = false;
@Override
public void robotInit() {
m_gyro = new XRPGyro(); // Initialize the gyro
//Wait for calibration (optional but recommended)
Timer.delay(2); // Wait a couple of seconds for IMU calibration
m_gyro.reset();
System.out.println("Gyro reset called via line 31 in robotInit. -- " + m_gyro.getRotation2d());
}
private MecanumDrive m_robotDrive;
private XboxController m_driverController;
/** Called once at the beginning of the robot program. */
public Robot() {
XRPMotor frontLeft = new XRPMotor(kFrontLeftChannel);
XRPMotor rearLeft = new XRPMotor(kRearLeftChannel);
XRPMotor frontRight = new XRPMotor(kFrontRightChannel);
XRPMotor rearRight = new XRPMotor(kRearRightChannel);
// Invert the right side motors.
// You may need to change or remove this to match your robot.
frontRight.setInverted(true);
rearRight.setInverted(true);
// frontLeft.setInverted(true);
// rearLeft.setInverted(true);
m_driverController = new XboxController(0); // Adjust port if needed
m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
SendableRegistry.addChild(m_robotDrive, frontLeft);
SendableRegistry.addChild(m_robotDrive, rearLeft);
SendableRegistry.addChild(m_robotDrive, frontRight);
SendableRegistry.addChild(m_robotDrive, rearRight);
}
@Override
public void teleopPeriodic() {
// Get gamepad inputs
double strafe = m_driverController.getLeftY(); // Strafe (left/right) (nob-inverted)
double forward = m_driverController.getLeftX(); // Forward/backward (inverted Y)
double rotate = m_driverController.getRightX(); // Rotation
// Check if button 1 (A) is currently pressed and Reset the once Gyro if it is
boolean isButtonPressed = m_driverController.getRawButton(1); // Replace 1 with your button number
// Check for the rising edge of the button press (only runs once)
if (isButtonPressed && !m_wasButtonPressed) {
resetGyro();
System.out.println("Rest Gyro called by line 74 on Button A Pressed in teleopPeriodic. -- " + m_gyro.getRotation2d());
}
// Update the button state for the next loop iteration
m_wasButtonPressed = isButtonPressed;
SmartDashboard.putNumber(“Gyro Angle”, m_gyro.getAngle());
SmartDashboard.putNumber(“Gyro Rotation2d”, m_gyro.getRotation2d().getDegrees());
// Call driveCartesian
// For field-oriented control, pass the gyro angle as the fourth argument.
// Adjust the angle based on your robot’s front direction relative to the gyro’s 0.
// Use the following to display drive parameters in the VS Code window for debugging
System.out.printf(“| strafe : %.4f”, strafe);
System.out.printf(" | forward : %.4f", -forward);
System.out.printf(" | rotate : %.4f", rotate);
System.out.printf(" | rotation2d : %.4f", m_gyro.getRotation2d().getDegrees());
System.out.print(“|”+“\r”);
m_robotDrive.driveCartesian(-strafe, -forward, rotate, m_gyro.getRotation2d());
// If not using field-oriented, use: m_robotDrive.driveCartesian(strafe, forward, rotate);
//m_robotDrive.driveCartesian(-strafe, forward, rotate); // Forward is forward and left is left
//m_robotDrive.driveCartesian(forward, strafe, rotate);
}
// Your resetGyro method remains the same
public void resetGyro() {
//if (m_gyro != null) {
m_gyro.reset();
System.out.println("Gyro reset to 0 degrees via line 108 in resetGyro. -- " + m_gyro.getRotation2d());
//}
}
}