SwerveDrive not working diagonally

My team’s swerve drive appears to work fine but they are very sensitive when going in a diagonal direction. They work fine when you are trying to go in one direction like sideways forward or backward but when you move the joystick to a diagonal position it will continue moving in the direction it was already going until it you bring it to a cardinal position and it goes in the direction. For example I put the joystick straight to the left, the bot goes left, but when I bring the joystick to the top left it continues going left, and then when I bring it to the top the bot goes forward. I am able to get it to go in a diagonal direction if I am slow with the control but it also moves slower. This is my teams codebase GitHub - FRCTeam5199/Commandbase-2023-Offseason. I am using BroncRobotz swervedrive example with the YAGSL library. Any help is greatly appreciated.

This may be a symptom of a poor quality joystick, especially if you are using the thumb stick of a 3rd-party Xbox controller to control your direction. Some of them do a really poor job of smoothly measuring the X and Y coordinates of the thumb stick, particularly in the corners. An easy way to test for this is to pull up the joystick control panel in Windows. It has a diagnostic screen where it shows you the data being output by the joystick. Check to see if the cross hairs jump around when moving to the corners.

You can see similar information of the USB page of the driver station, but it’s a little bit less obvious there.

I followed your suggestion and the input is not jumping around in the diagnostic page so I do not think it is due to the joystick. I also tried to calibrate my controller and the issue is still apparent. Thank you for your suggestion though!

I tried simulating your code and from what I saw it seems to be a weird and mysterious deadband somewhere in your code, that makes so when one joystick value is big enough it overides the other joystick, make the robot only move in one direction.
The symptoms that I found were that the robot doesn’t react at all to a below 0.2 value from the joysticks, but strangely when both joysticks were at 0.2 the robot “half reacted”, and the wheels turned, but didn’t drive.
I hope that what I found is helpful, and good luck solving the problem!

Thank you! I suspected it was a deadband issue and I tried to adjust the values of the deadbands but there was no change so I gave up on the idea. I’ll look more into it.

There are also a few places where controller input gets modified (including a few in YAGSL itself, in the SwerveController class however those are documented in Javadocs). I strongly recommend that you try to trace through all of the control smoothing to find out how your input is being transformed. YAGSL does not modify controller input as i recall in SwerveDrive.drive

In controllerproperties you set the deadband to 0.3 for angleJoystickRadius.
In Constants you use 0.001 for the deadband. I would start there to see if the angleJoystickRadius is causing your issue. There’s a lot of code to work through to determine if an incorrect value is being generated.

The deadband was originally at .01 and it was still behaving how it is now.I changed it to the current value believing that might help but it didn’t. The angle joystick deadband should not matter since it applies to the right joystick which controls the in place rotation of the bot.

I didn’t look at your code and have no special insight into your problem but this sounds like the “anti-pattern” numerical truncation. For example, 3/2 is vastly different and causes large step jumps than 3./2. which is smooth. Always look for missing decimal points in constants or mixing int variables with floats/doubles where the up-casting didn’t work in the order assumed.

Thank you for the advice! I’ll check through the code and make sure there isn’t any issue like that.

I figured it out! In the drivebase folder in the commands folder of the code the files TeleopDrive, AbsoluteDrive, and AbsoluteFieldDrive, modify the Vy and Vx which are the x inputs and y inputs of the controller using Math.pow(). It was originally Math.pow(Vx, 3) but I changed it to Math.pow(Vx, 1) and the problem appears to be fixed. Thanks to everyone who responded to my post! It made it a lot easier to narrow down my issue.

1 Like

I try not to argue with success but I don’t understand your solution so if you were me you’d keep driving for the root cause of your problem. I say this because normally raising the joystick input to the third power is your friend and gives you fine control at lower speeds.

(I’m always suspicious of int that should be double so I’d properly use pow(vx, 3.) but I’m also suspicious of 3 or 3. being a 3 so I’d do vx*vx*vx. I know, I’m being silly because the pow documentation says it handles 3 or 3. as a perfect int.)

Your code comment says you invert a joystick axis; I don’t see that in the code.

You are using a variable with sim in the name so are you simulating or is it real? Does it even matter?

A tiny dead band is unusual ( for my team) so you must have the best joystick in the world (at least a lot better than ours).

You display joystick numbers. What does a plot on the SmartDashboard (or ShuffleBoard) show? A nice smooth response with numbers between -1 and +1?

I see others’ comments above about suspicious activities. I’d heed their hints. As my first choice of trouble I suspect your SwerveMath.antiJitter code is fighting you. Next I’d check what all those Timers are doing to you.

I hope I’m not leading you astray but I’d spend more time understanding where the problem lies and WHY it’s a problem. Maybe the pow is a problem but WHY?

When you tested the joystick in the control panel, did it go all the way to the corner or did it go in a circle? If it went in a circle then X and Y in the corner would be about 0.7. When you cube it, it would be about 0.35.

The deadband was small due to other testing and it was never fixed, and after looking at the other uses of Math.pow I found the controller inputs were being cubed twice so it was being powered to the ninth which is what was causing the issue, the controller inputs are now only cubed which is what fixed the issue. Thank you for your input!