The issue:
Our robot turns left during motion profiling. The robot should drive straight forward 3 meters but instead turns to the left, then proceeds to drive in circles, constantly turning to the left. We used the characterization tool to get our values for Motion Profiling.
Data:
I have set up a spreadsheet with a snapshot of the gyro values and the voltage values the motion profile is trying to give to the robot (it gets up to 45 volts somehow!) Here is the link. Note: The x-axis is not in seconds, but rather the number of the set of data we got every time we updated the motors (so just ignore it).
Other resources:
We are using the 2020 WPILIB motion profile. We followed this tutorial. This is our GitHub code. The main files you’ll need to look at are Constants.java (in case our values from the characterization tool seem off), DriveTrain.java (Where we set up our drive train), and RobotContainer.java (Where we set up the motion profile). These three files are where the meat and potatoes of our Motion Profiling is. I’m hungry.
Things to note:
We could not figure out how to switch the lights on our right-hand Falcon500ds to get a green light while driving forward. Instead, the lights are red while driving forward resulting in negative encoder values. To remedy this, wherever we used encoder values from the right side we multiplied it by -1.
We had some unit issues since we had integrated encoders. For example, retrieving the distance traveled results in a distance in encoder ticks, but we needed a distance in meters (hence multiplying by our conversion factor). Same for velocity. We got ticks/100ms, but needed meters/second.
Our gyro gets more positive while turning left. Is this correct?
We are not getting any errors. All Falcons are ID’d correctly. Everything is on the latest firmware (Including VSCode, the RoboRIO, Falcons, etc.)
To those reading this:
Thank you SOOOOO much for your help, sorry if I gave info I didn’t need to or forgot some. Please message me if you need anything else or if you have something you want me to try. Once again,
THANK YOU SO MUCH!
Sounds like your gyro angle is correct if it’s increasing when spinning to the left (CCW). Just wanted to confirm the other two are reading correctly as well.
You mention having to do extra conversions for the integrated encoders, but the 14703.6897 number looks very strange to me… Where did you get this?
From your graph, your left velocity is oscillating quite a bit. This tells me your P constant is likely too high. If this value was provided by the characterization tool, I recommend throwing it out and tuning manually. I’ve been told by a few people that those values are trash (my team had to drop ours from ~3.7 to ~0.04 to get it to not thrash about wildly, so your 7.6 sounds REALLY high, though we’re using dual-NEOs).
I also see some oscillation in the right velocity, though it’s not as obvious since it’s already running at what I’m assuming is max speed. The reasoning for this is still a mystery to me at the moment, though might have something to do with your encoder/motor not being inverted properly as you described.
The navX actually is +CW, as said in https://pdocs.kauailabs.com/navx-mxp/guidance/terminology/ This could cause the increased error to feedback and continue to get worse. As for the 40 something volts, I’d just recalibrate the navX or invert the angle and hope it goes away.
Could that not be the problem? “Gyro getting more positive as turning left” could be reported after the inversion, and could be the problem. Or just un invert the wheel speeds i guess. But idk what the setup is going on.
We are working with just a drivetrain, for now, no mechanisms are currently on it. I added a 25lb weight (secured tightly) in case the lack of weight was messing up the motion profiling somehow. I re-ran the characterization tool to calculate new values.
Changed the motor controllers to TalonFX’s. Why did I have them as SRX’s? I have no clue. Thank you Fletch1373 for noticing that!
I changed it so the program used the front two encoders. I did this because of the way I added the weight. I wanted to ensure the wheels would not slip because of the slightly-forward center of mass.
I noticed I called “navx.reset();” thinking it would zero the gyro. I changed it to “navx.zeroYaw();” After doing all this, the robot still did the same thing.
I un-inverted the gryo. I had it so it got more positive as it turned left but now is more positive as it turns right. Same result.
I then changed the P-value to 0.1, down from the new 11.1 value I got. IT DROVE FORWARD LIKE IT’S SUPPOSED TO! However, it only drives about eight inches.
I was a bit confused about the kTrackWidthMeters value. I got ~1.2442 meters, but my robot is NOT 1 meter wide. Big shrugs there. I had been using 0.5588, our actual width in meters. I changed it to the 1.2442 meters value just for grins. Either way, it drove about eight inches.
I decided to try to drive the curve that WPILIB give you in the tutorial. It turned left like it was supposed to but did not get far. It was giving the left side about 0.65 volts, and the right 0.75 volts.
Other Stuff
As far as that 14703.6897 number goes, we are using integrated mag encoders. It has 2048 ticks per revolution. Our reduction is 280 motor rotations to 39 wheel rotations. I have no clue why we settled on that reduction. Multiply by 280, divide by 39, you get that 14k number.
We invert all four motors so it drives forward. We kinda just picked a direction and called it forward when initially programming it.
I have no clue what holonomic means.
Current Status:
The robot is driving the direction(s) intended. However, it is not driving far enough and it seems like the RamseteController is outputting voltages that are too low.
Once again, THANK YOU FOR ALL YOUR HELP
Edit:
I just pushed the new code to GitHub. The current code on there is exactly what I am running on the robot.
Can you post your characterization results? Particularly the graphs mentioned here here as well as the generated configurations mentioned here. Generally, your descriptions sound like your generated values are off which, without knowing anything else, sounds like you calculated gains based off of position and not velocity or you didn’t generate gains for the correct controllers.
If you have mecanum wheels on your robot, weird things could be happening with your traction. Might be worth trying your code with standard wheels, then going back to the mecanums afterwards.
The other thing is being off by a factor of 15 (8" to 3m if I’m reading code correctly) is a weird amount to be off by, and doesn’t point to a clear culprit in the unit conversions… but could be worth double checking. I don’t know how to read C++ so I don’t know if your constant is set up right.
Is your gearbox near 1:15?
If the mecanum joystick drive already works correctly don’t change anything… but WPI_TalonFX.setInverted does change the lights. You use the command on all four motors during setup (pointed out by another poster), but then send negative values to half your motors in your tank drive function. If you only use setInverted on half your motors, you should get green lights forward and simplify some of your math in tank.
You could setInverted to false during AutoInit and then set them to true at TeleopInit, but that introduces Weird Dependencies. Or basically create a method to call the inversion function outside the constructor in a way that’s convenient, but it’s kinda playing with fire shrug
I don’t think anything is going on with traction because it is only going 8 inches instead of 3 meters like it is supposed to. The wheels move very slowly and there is not any slipping.
Its Java
I think so, but that is likely to be an issue so I can double-check.
Nope! Our robot is 280(motor):39(wheel)
This is what WPILIb did in the example code. Without this, the robot jerks left and right, and does not move forward. Then again, this was before adjusting P so I tried it. It made it so the FX’s are green while driving forward! Thanks!
From talking to your team on Discord, I’ve seen about 4 or 5 different gear ratios that end up somewhere between 1:4 and 1:10, and this is yet another different number. It’s within the ballpark enough to not make a major difference, but you should figure this out properly and use that value in characterization and in your code.
The most reasonable numbers I’ve seen are 14:70, then a 16:24 belt reduction. Since Vex doesn’t sell 16t HTD pulleys with a hex bore, I would assume this is actually an 18t pulley. This gives a number of 6.66… motor rotations per wheel rotation, which is a decent amount away from the 208 to 39 number you have now.
Ha! I clearly don’t know how to read Java either - whoops.
I specifically don’t know how to read the lines your conversion constants are set up in, so no comment on whether they’re accurate.
Yeah…
We had some issues with the gear reduction. I was told about 4-5 different reductions before I confirmed 280:39. We ended up counting the teeth on the gears and pulleys. This also makes sense because of the encoder values we get.
I’m a programming man, so I really have no clue how outrageous that reduction is. All I know about it is it go fast fast.
Remember to use the value for track width given by characterization, rather than a measured one. The PID values kinda depend on it. On another note, track width shouldn’t be that far off. Are you sure you did characterization correctly? Ours was only .05m off of actual when we did it a week ago.
In case you are interested, it reads like something you’d put into a calculator. Ignore the words and these two symbols: /* */ that is just for commenting what each number is. Also, the * symbol means multiplication.
So, we have (1/14703)x(6xPI)x(1/39.3701)
In English: (1/Ticks per rotation)x(circumference in inched)x(1/inches in a meter).
If we did the math right, this should result in some number of meters per tick. Multiply this by the number of ticks at a given time to get meters travelled at that time.
The characterization tool did not state anything about track width that I saw, so I was not too sure what it was about. It rotated to the leg tin place if I’m not mistaken. I just let it run for two complete rotations.
There are no fields in characterization for combined tickrate. It asks for gear ratio and wheel size, and calculates that itself. Tickrate, instead, should be 2048(or whatever falcon is). Side note: I’m not at a computer right now so i could be wrong.
Thank you, I didn’t know that /* */ could be used for inline comments like that
2048 tics/rotation at the motor * 208/39 ~= 11k
Correct, the value labeled “track width” value is derived by the characterization tool and is really a “turning coefficient” in the controller that’s nominally related to the track width of a standard tank drive. Due to the roller geometry I’m not surprised your effective track width is much larger than a typical tank drive - I suspect that the distance between diagonal wheels is a better physical proxy on a mecanum robot.
