Problem with trajectory tracking

My team trying for a month creating trajectory tracking with wpilib tools(TrajecotryGenerator etc…).
for some reason the robot just driving forward, and when we create a trajectory with any turn/curve the robot jumps and not doing as the code says. The encoders and the gyro are working well and we use 3 TalonFX motors in each side of our drivebase, our gyro is ADXRS450 SPI port kOnboardCS0.

image

Did you use the FRC Characterization tool? We plugged the kP from that analysis into the P term of the PID Controllers.

Also, it looks like you might be feeding DifferentialDriveWheelSpeeds with encoder position rather than velocity, depending on what getEncoderAvg{Left|Right}Side is doing.

The usual advice applies about RamseteCommand: make sure your units are all consistent (e.g., for meters, encoder values in meters, velocity in meters/sec, waypoints in meters).

Sure, I used the characterization tool and i think the ka, kv, ks are correct. I used meters as the units in the config and the RamseteCommand.

I’d suggest looking into WPILIB’s troubleshooting page as it is pretty detailed with different failure cases and possible solutions. I will link it below.

https://docs.wpilib.org/en/stable/docs/software/advanced-controls/trajectories/troubleshooting.html

1 Like

I looked at it and I checked everything under: “My robot drives way too far.” and “My robot swings around to drive the trajectory facing the other direction.”. (which is our problems). The encoders are ok, and so does the gyro. I’m giving him straight line to drive but he still driving too far (I’m not sure he stopps we dont have enough space to check it).

Oddly, the solution might be to throw in a few minus signs. For instance, your “setOutput()” routine is actually like to need one of sides flipped (*). That could easily cause jumps and crazy behavior.

The “easiest” and safest way to diagnose this is to get the simulation working and then work it out there.

*) can’t say I completely understand the reason why some calls need to have the values flipped and some don’t. “Consistency is the hobgoblin…”

How are you getting the wheel speeds? Are you using the internal encoders on the Falcons? Are you taking into account that the velocity returns from Falcons is in units per 100ms? If you are using the internal Falcon encoders, did you take the gear ratio between the Falcon and the output wheel into account when converting from encoder units to meters?

Your function says that you are getting the average speed, are you averaging the speed of all 3 Falcons on one side? If this is the case, did you make sure that all 3 Falcons sensors positively increment when moving forward?

1 Like