Robot turning and then following path backwards

bobbysq · April 1, 2021, 4:22am

While running path following, we have been running into an issue where the robot has been turning in a direction and then following the path while driving backwards. A video of this issue can be seen here:

So far, we have tried:

Ensuring robot odometry is reset in AutonomousInit
Attempting paths of different shapes and directions, all of them lead to this behavior
Re-characterizing the robot’s drivetrain

None of these have made any difference.

Any help with this would be appreciated!

Relevant files:

AutonomousInit:

github.com

frc4646/2021-competition-code/blob/master/src/main/java/frc/robot/Robot.java#L109


}
@Override
public void disabledPeriodic() {
}
/**
 * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
 */
@Override
public void autonomousInit() {
  Robot.m_drivetrain.resetEncoders();
  Robot.m_drivetrain.zeroHeading();
  Robot.m_drivetrain.resetOdometry(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
  m_autonomousCommand = m_robotContainer.getAutonomousCommand();
  // schedule the autonomous command (example)
  if (m_autonomousCommand != null) {
    m_autonomousCommand.schedule();
  }

RamseteCommand setup:

github.com

frc4646/2021-competition-code/blob/master/src/main/java/frc/robot/RobotContainer.java#L169


    //left stick
    leftTrigger.whileHeld(new LaunchBalls());
    leftButton4.whenPressed(new AlignToTarget2());
}
/**
 * Use this to pass the autonomous command to the main {@link Robot} class.
 *
 * @return the command to run in autonomous
 */
public Command getAutonomousCommand() {
    // An ExampleCommand will run in autonomous
    //Pathweaver stuff
    trajectoryJSON = "paths/test3.wpilib.json";
    trajectoryPath =  Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
    try {
    test1Trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    }
    catch (IOException ex) {
    DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, ex.getStackTrace());

Drivetrain Subsystem:

github.com

frc4646/2021-competition-code/blob/master/src/main/java/frc/robot/subsystems/Drivetrain.java

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.*;
import com.ctre.phoenix.motorcontrol.can.*;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
//import frc.robot.commands.DriveTeleOp;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

This file has been truncated. show original

Attempted Path:

github.com

frc4646/2021-competition-code/blob/master/PathWeaver/Paths/test3.path

X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
0.0,0.0,0.0,0.0,true,false,
4.0,0.0,0.0,0.0,true,false,

jdaming · April 1, 2021, 4:29am

This seems like a sensor/drivetrain inversion issue. It seems like the code thinks what you are calling the back of your robot is the front.

Going through these troubleshooting steps is the comprehensive way to solve these issues.

If you face you robot “backwards” what does it do?

bobbysq · April 1, 2021, 10:41pm

Are the start and end headings of your trajectory wrong?
- Potentially? I was under the impression that the robot set its initial heading to the first heading defined by the trajectory and our test path should have an initial heading of zero, but it could definitely be an issue if it’s not.
Is your robot’s gyro getting reset to the wrong heading?
- I’ve checked, it resets to zero unless we’re putting the wrong value on SmartDashboard or somehow sending the compass-based heading to our path following.
Do you have the reverse flag set incorrectly?
- Fairly certain it’s not, unless this is separate from the PathWeaver reverse flag.
Are your gyro angles clockwise positive? If so, you should negate them.
- Potentially. We will check this tonight.
- Edit: I can confirm gyro angles are going the right way.

Thank you for passing on those troubleshooting steps. Despite the issues we’ve been having, we’re grateful to the WPILib team’s work over the past few years to introduce more advanced controls into FRC over the past few years.

Edit: We got it working, at least at the beginning. We had to flip the left side of the drivetrain in the DriveByVolts function. Still having a strange turning thing happen at the end of the path, but this might be an issue with the path.

Edit 2: it was an issue with the path

jdaming · April 2, 2021, 1:40am

Awesome to hear! Getting all the sensor/motor directions and units correct can really be a pain but it feels really good IMO when it just all works!

system · April 2, 2022, 1:40am

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.