Trajectory Tracking Issue

So we have been playing with path planning for a little while and so far had a good experience but obviously there is a good amount of troubleshooting to do. A few days ago we did some tests with our 2022 robot and went through WPI troubleshooting which fixed our feedforward issues. During troubleshooting the ramsete controller was disabled which we forgot to fix. Trajectory worked for the most part but turning to the right angle did not work. Today we enabled the ramsete controller which fixed turning and made it more accurate but now it shutters during the path.

The PID controllers cause it to get worse but don’t seem to be the main cause. Disabling the ramsete controller does solve it but causes accuracy issues. The pose is very close to where it should be but the jerky motion probably causes a bit of error.

Can you share a link to your code?

Out of curiosity do you happen to be using NEOs?

The neos have this weird quirk where when you set the VelocityConversionFactor you have to divide it by 60, because it measures in revolutions per minute. Hope this helps.

1 Like

had to learn how to use git bash to update the repository someone else made for me.

No we are using Falcon 500s

I’m not seeing a clear cause for this problem in your code, but I do have a couple of notes:

Do you have any graphs of how the wheel speeds are tracking the setpoint?

We did the characterization and the ratios in the code using a 5.6in diameter wheel. If this was off slightly could that cause any of the issues? They are KOP wheels but the we have worn the tread down smooth. I will try to measure the diameter this weekend.

also could you expand on the floating point vs integer thing?

Integer division in Java produces an integer result, discarding any remainder. So the code:

double kGearRatio = (50/14)*(48/16);//10.71428571428571;

will set kGearRatio to 9.0, because 50/14=3 remainder 8. Changing one of your integers to floating point is enough to prevent this. This is a really common error that many people fall into.

Regarding wheel size, yes that can make a difference to trajectory tracking, but I don’t think it would cause shuddering. Ultimately, you’re often best just to determine the appropriate native to metres ratio empirically: Drive for distance and measure it with a tape measure.

Ah. I just had a look in Feeder and it looks like your’re using on-board I2C with a colour sensor. See Onboard I2C Causing System Lockups. Although that talks about lockups, there are a lot of threads here that describe seeing delays instead, which could lead to shuddering.

1 Like

Would this only happen in autonomous? The I2C has not caused us any other problems this season.

I don’t have a good model for predicting exactly when this problem will arise or how severe it will be. Other teams have also reported no problems with this. It is plausible that it would cause shuddering in autonomous, but only mild control lag in teleoperation.

You should check your riolog for messages like “Loop time of 0.02s overrun”. Can you post a DS Log for one of these runs?

Also, as it’s fairly rare (but not unknown) to ingest an unexpected ball colour in autonomous, you could probably get away with not doing colour sensing in that mode. The problem could be provoked just by the SmartDashboard calls in Feed.periodic().

1 Like

The problems are sporadic; is your failure reliably repeatable?

are the system lockups from using the i2c or from every time the code receives data from it? Also is this specific to color sensing or the proximity sensor as well? Also do the default commands run during auto because if so I would like to stop that.

Can’t check right now but that message may have been appearing. Seems familiar.

Not consistent how they happen in auto routines but it does seem to happen consistently in all routines.

It can be provoked by any get call on the colour sensor. As I noted above, just the fact that you’re reporting to SmartDashboard in Feeder.periodic() could be enough.

Default commands generally don’t run during auto, but only because the autonomous routine includes commands that require their subsystem. That’s how the scheduler knows to stop and restart the default command.

Checking that your auto and default commands correctly required the drive subsystem was the second thing I did when I looked at your code.

This is a clue that something in one of your commands (initialize/execute/isFinished/end) or subsystems (periodic) is taking too long. Given that you don’t seem to have implemented any long-running loops, the most likely cause is that they’re waiting for I/O (probably either I2C or CAN bus), but that’s just an educated guess.

The DS log will contain more information, hopefully pointing at the specific method that is taking too long. See, for example, this thread. It’s always better to have hard data about where the delay is, than to speculate from looking at the code.

So one thing I should mention then is this Intake Rotation motor and controller does not exist. It is just preparation for a project this weekend. I don’t think anything is using it but I guess it could be another problem. Hopefully this weekend I can get some more info if not fix the issue.

OK. Let us know what happens. We love to hear the ending.

I’ve got some photos of the rio log but I have never done logging so you might have to give me a bit. In the meantime here are some photos of what is probably part of the issue.


Not this

Thanks. That gives us a little more to go on. I’m going to go over your code again and see how you might be contributing to LiveWindow.updateValues(), but you could try calling LiveWindow.disableAllTelemetry() to cut it out completely.

One thing I just noticed in your code is that you’re calling CommandScheduler.getInstance().run() twice! Once in robotPeriodic and once in autonomousPeriodic. I’d stick with the former only.

Small point: You’re calling DriverStation.getAlliance() two or three times per cycle in periodic methods. I have no reason to think this method is especially slow, but I also don’t expect your alliance colour to change during a match. It’s possible that calling PhotonVision.setPipelineIndex() every cycle has some cost.

There’s an oddity that AngleClimberUpGryo.execute() tests the result of Pigeon2Subsystem.getRobotAngle(), but the latter is hard-coded to return zero.

Update: I’m not sure what you’re doing to make LiveWindow.updateValues() slow. Hopefully a WPILIB developer can chime in and educate us both. @Peter_Johnson ?

So we had the differential drive output not updated often enough issue once before when we accidentally had two drive subsystems fighting each other. Now I think the CommandScheduler thing was an accident. I probably meant to cut and paste something and accidentally copied that as well. I am guessing it could explain why it is only an issue in auto.

1 Like

I’m not really sure what LiveWindow is or where I am using it. Do you recommend any reading for this?

I should probably get rid of this now. We were planning on photon to track balls but don’t have time. This subsystem is not in robot container at the moment so I would think there shouldn’t be any issues.

Another thing we haven’t played with yet. I intended to figure out what angle was best first. I also need to figure out if I needed to get pitch or roll. Don’t remember what orientation it is mounted.

Documentation on LiveWindow is a little thin, but you can find some at:

Basically it automatically generates telemetry for some parts (sensors and actuators) of your robot and exposes them through NetworkTables.

That’s fine, but the general point holds that you should avoid doing things every execute/periodic when they only need to be done once. :slight_smile:

Getting back to your original problem with the shuddering, I was trying to decide whether running the scheduler twice could have caused this. I would have thought that the RamseteCommand would be able to cope with the scheduler being called twice per cycle, but perhaps it has numerical instability if it is executed on too short a time interval. It’s certainly suspicious that this is an autonomous-only issue, and you are only experiencing problems in autonomous.

An experiment I should probably have asked you to do earlier is to trigger your autonomous command in teleop using a button, to see if you still have the shuddering. That would have let us narrow down whether it was an auto-specific problem.