RamseteCommand makes my robot go crazy

MrNick42 · February 4, 2023, 1:44pm

I’m trying to understand how to use the RamseteCommand. I’ve copied the example on the WPILib as close as possible.

I am using last years chassis, which has two Neo motors per drive axle and is a simple tank drive system. I can drive the robot just fine with an Xbox controller.

I started out trying to use the SysID program, but it doesn’t seem to do anything. My robot won’t move when I try to characterize it. All of the code is checked in here:

github.com

STEM-Alliance/robot/blob/feature/path_planning/robot_2023/src/main/java/frc/robot/Robot4360.java

// 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.wpilibj.*;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;

import edu.wpi.first.cameraserver.CameraServer;

import edu.wpi.first.math.trajectory.*;
import edu.wpi.first.math.kinematics.*;

This file has been truncated. show original

I have a simple trajectory that drives straight and stops. When I run the autonomous mode, the robot starts to drive forward and then freaks out and the motors oscillate between full speed forward and full speed in reverse. I put the log file here:
https://drive.google.com/file/d/1WXdXXCoU4obPP7ARvxGiXuUJfsYodjZY/view?usp=share_link

The image below shows the start of the autonomous mode. The left/right motor command starts to increase, the left/right encoders show forward movement and then the Ramstete controller decides to command full deflection of the motors in opposite direction. I thought it was something to do with signs, but I’ve inverted the left/right encoder values, pose, everything I could think of, but I still get the same results.

Any help would be greatly appreciated. Thank you.

bovlb · February 4, 2023, 5:23pm

I would agree that it’s likely a problem with the signs. Can you verify (perhaps with the robot on blocks) that a positive voltage (sent to DriveSubsystem.move()) makes all the wheels turn forwards and the encoders increase? And positive on one side with negative on the other turns in the expected direction? Does this track with what the field widget is showing from the odometry?

For example, I notice your arcade drive negates both joystick axes. Negating the Y is expected, but negating the X is not. Unexpected negations in arcade drive are, in my experience, often associated with autos that go crazy.

An unrelated suggestion: I note that you’re using the same code base to configure two different robots, and you have it set up so that you can switch robot with a one-line change. We used to do this, until the day we sent a robot to an away competition with the code for the practice robot and no programmers. Now we create a file on each robot with the robot’s name in it. Load the file, read the name, switch the behaviour. If the file is missing, complain.

MrNick42 · February 5, 2023, 2:33am

Thank you bovlb for the feedback. I like the idea of a file on the unit to customize the robot.

So you know that orbiter NASA sent to Mars where one group was using Metric and the other group was using Imperial units… I should have learned an important lesson from that. My chassis wheel base was set to 26.5 METERS. The chassis is 26.5 inches. Once I converted that everything worked almost ok.

Our robot has a tendency to pull to one side. But at least I can get it to follow a path.

How do I get the robot to end a path facing a certain direction?

I have this curve path:

        Trajectory curve =
            TrajectoryGenerator.generateTrajectory(
                // Start at the origin facing the +X direction
                new Pose2d(0, 0, m_robotDrive.getHeading()),
                // Pass through these two interior waypoints, making an 's' curve path
                List.of(new Translation2d(10, 3), new Translation2d(20, -3)),
                // End 3 meters straight ahead of where we started, facing forward
                new Pose2d(30, 3, Rotation2d.fromDegrees(160)),
                // Pass config
                config);

It DOES follow the curve, but it doesn’t face a certain direction at the end.