Problems following a Pathweaver trajectory via Ramsete controller

[cowabunga]

Hello,

I am attempting to get my robot to follow a trajectory generated in Pathweaver, but when tested, the robot just kind of spins in a circle really fast.

I followed the WPILib docs info on Pathweaver, characterization, and trajectory following as best as I could, but we’re using Iterative Robot, not Command, so I had to get creative when trying to find a way to actually follow the path.

We are using 4 Falcons (2 per side) through a drivetrain gear ratio of 18.75:1 and 6 inch wheels.

.zip with code and paths:

code.zip

//the 38400 number in the code is the Falcon CPR times our gear ratio…2048 times 18.75.
Characterization tool screenshot:
Data analyzer -

//More in the comments. New users can only put so many files into a topic I guess.

Through a little troubleshooting (cout statements after every line in the followPath function), it seems that the differential drive wheel speeds, when converted from chassis speeds using differential drive kinematics, are way too high, often exceeding the max acceleration and velocity limits set in the Pathweaver config by a large amount. I am not sure why this is happening and can’t find a fix.

I tried running with with a kP value of 0, and it essentially does the same thing. The motors are spinning at the speed they’re given, it’s the speed they’re given which is wrong if that makes sense.

I’m not really sure where to go from here.

[cowabunga]

Time-Domain plots -

Voltage-Domain plots -

[keco185]

#1 thing would be to make sure you’re output to the right side of the drivetrain is set to the negative of the voltage since the motors on the right side of the drivetrain need to go in the opposite direction to move the robot forward. Also make sure that your gyro angle function returns an increasing value when your robot is rotated counter clockwise and a decreasing value when rotated clockwise. Also make sure that the encoder values given to the ramsete controller are positive for both sides when the robot travels forward