Ever since we started using swerve, we’ve experienced one enduring problem. Whenever our robot accelerates, there’s a small (roughly one in every five instances) chance that the rotation of the robot changes slightly.
We’ve tried almost everything to fix it: We’ve redone module offsets, changed treads, swapped to a custom swerve library, etc. Still, the issue persists.
To elaborate more on the issue: Each time we go from a standing to moving position, whether that happens slowly or quickly, the robot sometimes rotates/shifts slightly. It still remains on-course since we use field-oriented drive, but the orientation of the chassis changes.
We have also the same issue the route is still direct but the orientation of the chassis changes but this orientation is always changing like sometimes it is 30 degrees or less but we are not changing the distance. We also know this is normal for Swerve drive but in 8 meters 30 degree change is not acceptable for every team.
We’ve recently implemented a fix that seems to work for some cases (very slow acceleration).
Basically, we get the angle of odometry and the angle of the pidgey and find their difference. We use this difference in what’s basically a P controller to calculate a rotational offset in our ChassisSpeeds which we get from the drive controller. We’ve found this to work for keeping the robot straight & predictable when our driver is scoring, but not when we’re quickly accelerating back to the human player station.
Still, the issue persists at high speeds. How’s your robot’s weight distribution? We’ve been theorizing that that may have something to do with it…
Also idk if it’s normal for swerve drive. Our sister team which we share a lab with seems to accelerate & stay pointed in the right direction just fine. We tried switching to their swerve control library and that didn’t fix it, so it might be something more mechanical causing the issue.
Our robot’s weight distribution is so good maybe 2 pounds or less then. We think same this is more mechanical issues like tread, lubrication or any mechanical thing.
First the obvious: Are you seeing any drift in your gyro? Are you feeding the gyro heading into your swerve code (you pretty much have to in order to run field orient)? If your gyro is not drifting and you are running a closed loop control on gyro heading, then the code should correct this angle. So, if the control loop is not achieving the desired heading, then you may need to tune this loop a bit.
You may want to look at your actual commands coming from the controller. It could be that there is some coupling going on in the controller that is actually commanding this rotation. I suspect that you are driving with your thumbs off the rotation control, but there still could be some level of electrical coupling going on inside the controller. Can you print out the commanded rotation angle as well as the gyro measured heading on the driver station and make sure that the commanded angle is not changing during these accelerations and that the gyro heading is matching the commanded angle?
No, this is not normal. You should be able to solve this with some good troubleshooting. Good Luck.
I would actually say that’s pretty much working as expected. I’m assuming you are not closing the loop on your heading. That is one type of field centric.
There is field centric where the robot uses the right stick to rotate (hold right, it rotates right, hold left, it rotates left).
For this type of field centric, you are relying on essentially open loop to maintain heading. You are expecting your velocity loops to all be exactly correct, and the modules to all rotate instantaneously. Since those are not true, every robot will experience some level of rotation using this method of field centric with no closed loop heading. Any amount of friction or delay will cause heading deviation.
In fact, that is why some people were delving into 2nd order kinematics to account for the module rotation delay.
There is also field centric where the robot uses the right stick to tell the robot which way to face. By pushing the right stick 90 degrees to the right, that’s the way the robot faces. By pushing it back towards you, that’s the way the robot faces. And when you aren’t changing orientation, the robot maintains it’s last commanded heading with a closed loop. This will maintain rotation when accelerating (within reason).
We do something similar with heading control, and found it worked very well on our bot this year. Essentially, we store a desired heading variable, that is set whenever the driver rotates the robot. Then, whenever the driver isn’t rotating the robot, we use a PID controller to drive the distance between the desired heading and the actual heading (from a gyro) to 0. You can trace the logic starting in rotateOrKeepHeading() on line 367 of DriveSubsystem.javahere. (For context, this function is run on line 391 of RobotContainer.javahere.) Just trace through the logic about what this function is doing, and you should be able to work something similar into your own code. Of course, this might not address a deeper hardware issue in a way that works best for you, and obviously won’t work if your gyro itself is not working properly.
We encountered a similar issue with our swerve drive this season. Our solution was pretty simple, we ignored the heading of the robot most of the time. When the driver (was me) was driving with simple field centric drive, we let the heading of the robot drift slightly, and if the driver needed to they could correct manually. For scoring and collecting, the driver can then use a face button (ABXY) to snap the robots heading to a multiple of 90°. The robot would hold this heading until either another face button was pressed, or until a normal steering input was applied. This worked pretty well for us, and when I drove it felt pretty good to me.
I may be in a unique spot because I programmed and drove my team’s first swerve, but I prefer having more lower level control of the robot by default, with higher level control available when I asked for it.
If you were using SDS Mk4 with NEOs, I would suspect pinion slip as primary cause. The 32DP pinions can and do slip under load. 4i’s should not have this issue because the deleted the press on pinions. We create an error checking function that we could check every steer motor for pinion slip using the ABS encoder.
Does this happen with autonomous moves as well? If not, you may have some controller (joystick) or operator issues. If this is the case, you should try logging the controller positions to see if that explains it.
The issue persists when we run robot oriented. So I have a hunch that it’s probably something mechanical causing this (and we just have to fix it programatically ).
I’llLimelight, an integrated vision coprocessor look at this, but I don’t think it’s because of the controller. We have some pretty nice newish razer ultra-rgb-gaming super controllers which I don’t think would have this issue.
This is the most likely solution in our case I think. When I get back to the lab I’llLimelight, an integrated vision coprocessor tune our swerve motor angle loop (because maybe the gains are just a little off and we don’t have enough power to reach the setpoint right away?) I’llLimelight, an integrated vision coprocessor also take a look at the course correcting loop I mentioned earlier and see if I can get that working a bit better.
This is the unfortunate reality of swerve. Without some software-based control to maintain your orientation, you’re going to see some inadvertent rotation. As a few people on this thread have said, there are a lot of variables that need to be perfectly in sync to avoid unintentional rotation: each module needs to rotate at the exact same rate, accelerate at the exact same rate, and have the exact same traction on the ground. That’s not going to happen in reality, so software to control the orientation is quite useful.
That being said, there are some things you should check independent of the software. In particular, you should confirm that each module accelerates at very close to the same rate and achieves very close to the same speed at various voltages. You should also confirm that they rotate at about the same rate at various voltages. If not, you probably have a mechanical issue somewhere. You should resolve that first and get the mechanics in good shape, and then add the software control on top to polish it off.
My team implements the software control of orientation by keeping track of the last-desired orientation - i.e., the angle the robot is facing at the last moment that the driver is directing a rotation. Then we do a PID where the input is the current orientation, the set point is the last-desired orientation, and the output is a rotational velocity term for your swerve kinematics. This works quite well when it is tuned appropriately.
In vehicle control systems, there is a concept of “inner loops” and “outer loops”. For swerve, the outer loop is the vehicle state (global x-y translation and body rotation in some frame of reference). This outer loop can be run open-loop (with no control feedback other than the driver inputs). Or you could implement closed loop control over one, or all of these DOFs. In this specific case, you could have a closed loop control on body rotation angle with the desired rotation angle being provided by the drivers in terms of joystick angle providing the desired direction and rate of rotation and the desired rotation angle at any given time is achieved by integrating the rate command over time. Or you could allow body rotation to run open loop when the drivers are commanding the rotation with the joystick and only run closed loop when the joystick input was zero by implementing a “hold heading” mode as well as “rotate to heading 0/90/180/270” mode as was suggested by #IanSzalai above.
The outer loop control provides specific module wheel speeds and angles that are sent to the individual modules. The module rotation angle and wheel speed can then be run independently as “inner loop” control systems. The inner loop control system would try to achieve the desired module rotation and speed provided by the outer loop control scheme. It sounds like you have this part of the system working well enough.
In theory, tuning those inner loops can improve the stability of open loop behavior of the outer loop. But, in practice, there are too many variables to do this successfully. So, I would not mess around with these inner loops to try to fix an outer loop problem. Instead, you either live with the rather docile outer loop behavior and let your drivers apply corrections to any drift, or you implement some closed loop control schemes into your outer loop. Letting your drivers correct it is fine for teleop, but you may not be happy with the result for auton (however, many of the path following programs will do a pretty good job of providing rotation corrections if you put the robot heading into those path following schemes in addition to the X-Y path). But you may be happier with the results if you apply a “hold mode”.
@tails618 suggested this type of system earlier. I modified our swerve subsystem to drive like this (with an inner & outer loop) and I’m pretty sure it’llLimelight, an integrated vision coprocessor work now. Although I haven’t tested it yet but I may get a chance to later today. As long as our gyro doesn’t drift I can’t see why this system wouldn’t work to be honest.
I’ve been doing a system like this for ages, except in my case I’m always in closed loop. The driver’s rotation input gets multipled by a scalar and is added to the setpoint. The robot then turns because it’s trying to get back to the setpoint.
When the driver lets go of the stick, I remember that angle, and flip a boolean to say that we’re no longer turning.
).