Pathing not working

Hello everyone! I have been trying to get Ramsete path following working on our robot all season, but have failed each time. Currently the robot is not following the generated trajectory despite being given all the constraints and the derived velocities. Can anyone help me on that?

The command is how the drivetrain follows the generated trajectory, the controller sets the gen erates and sets the desired trajectory.

Sorry I don’t have a more direct answer for you …. But when debugging our issues we found that using the odometry and the Field2d widget to watch what the robot thinks it is doing super helpful.

https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.html

1 Like

Could you give more detail about this? What do you expect the robot to do? What does it do instead? Does the odometry agree with the actual or expected behaviour?

Hmm. This code is very different from most. Have you considered using the WPILIB command framework? They provide a RamseteCommand class that packages up a lot of this, and then you wouldn’t have to maintain your own scheduler.

I’m concerned about the conversions between feet and metres everywhere. Better to pick one and stick to it. I haven’t found any errors related to this, but it’s an error waiting to happen.

I’m trying to following what’s going on with the pose and odometry.

Could the problem be just that your robot thinks it’s nowhere near the desired position at the very start of the path?

The robot just doesn’t move in the right direction at all during the path. I also think that something is up with the pose and the odometry. Could you explain why you think the pose is being reset to (0, 0, 0) and not the starting pose of the trajectory? It should be ROBOT_START.

Because it’s getting the start pose here: https://github.com/iliterobotics/FRC-Robot-2022/blob/af4645fbc279379de0078e60a8abb2af439def79/src/main/java/us/ilite/robot/controller/BaseAutonController.java#L159

All the auto controllers (i.e. ThreeBallTrajectoryController) extend BaseAutonController, which the drive team selects through shuffleboard (mAutonSelection.getSelectedAutonController()). The selected controller methods override the parents methods. Shouldn’t the ThreeBallTrajectoryController getStartPose() override BaseAutonController getStartPose()?

I think you’re right. :slight_smile: You should mark the overriding method with @Override, and consider making the parent method abstract.

I see you have a lot of logging. Can you post any of it?

I’m looking in BaseAutonController and I see a field mMotorPidController which is used in calculating the output values for the drive train, but you’re using the same PID controller for both left and right, which seems wrong, and also it seems to be unrelated to any of the six PID controllers you have in NeoDriveModule

The pid controllers and feed forward that are being setup in BaseAutonController are not being used for pathing. Only the LeftCtrl and RightCtrl smart pid controllers are being used (specifically in the PATH_FOLLOWING_RAMSETE state in the NeoDriveModule class). Unfortunately, I do not have any logging of the auto odometry and path following)

OK. I’m obviously struggling to work out which bits of code are being invoked here. I’ll continue to point out things that strike me as odd, and hopefully some of this will be helpful to you.

Looking at your trajectory, the start co-ordinate is (28.415, 17.214), which feels like it’s on the opposing side of the hub, according to the conventional co-ordinate system.

In ThreeBallTrajectoryController.updateImpl, you’re creating a new trajectory on every update for 100ms. If this is called every 20ms, you’ll be doing this 5 times. Maybe use a state machine or boolean instead.

You should probably log m2ndBallLegTime and mReturnShootLeg; if they don’t get reasonable values, then everything will be off. You could also log to SmartDashboard when you change trajectory.

You call fireCargo and intakeCargo in updateImpl. Are they happening as expected? Do they ever stop?

In NeoDriveModuke.setOutputs, in the PATH_FOLLOWING_RAMSETE case, why is the gear ratio not used here?

Are these not the coordinates that we need to follow to create the trajectories? I thought they were since I was generating a reasonable trajectory. As for the gear ratio, I completely missed that and will immediately include that.

Well, it’s probably not the cause of any problem here, but the convention is that the origin is on your alliance wall, with the two alliances using different co-ordinate schemes. You’re using a blue alliance co-ordinate system, to start from the red tarmac.

Did that improve things?

I haven’t gotten a chance to test it yet, I’ll keep you updated.

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