Long story short is I, a Mechanical Engineer with some basic level programming knowledge, have become our local programming “expert” due to mentor and student turnover. For our first competition, after receiving some help (mainly @Clayton_Yocom helping us get the old SDS library functions into our current project), we ran what was basically our 2020 code with updated CAN IDs and some small tweaks. This made for pretty ugly code (commands and subsystems were a mess, and we did not use RobotContainer or define much at all in Robot), but it worked well enough to have the robot driving and functioning for week 1 in Detroit. In the meantime, I’ve tried to dive in and pick up as much java as I can, and try to untangle and simplify what was previously written so it’d be easier to understand for me or anyone I might be able to teach.
We are running SDS mk2 modules, as we ordered mk4s late and had a hard enough time getting our old code running, so we decided to shelve those until next year.
Now for what we’re trying to accomplish that I could use guidance on:
We want to integrate a Pigeon2.0 into our drivetrain subsystem, as we had a lot of gyro drift issues with our NavX at Detroit that would make field-centric control useless after half a match, I’ve started to lay that out in code, but am open to any feedback on how we’re doing it
After some updates Saturday morning at Detroit (only changed encoder offsets afaik), we seemed to lose field centric control entirely, so if anyone sees any red flags in our code in regards to that, I’d appreciate the feedback
I did notice that our drivetrain subsystem differs from the SDS mk2 examples in a couple ways that I don’t really understand, namely that we’re taking in angular rate in our yaw command for positioning, where the examples from SDS only use angle as far as I can tell.
If you haven’t caught on by now, I don’t have that firm a grasp on what I’m doing. Beyond the above, any general feedback is welcome, but I definitely want to prioritize functionality changes over convention changes (i.e. moving things to robot container which I think I understand, but don’t really have the time or additional support to do).
One thing you might need to do for the navX sensor is adjust the Gyro Scale Factor Ratio. The factory default scale factor ratio might be non-optimal for the sensor, leading to errors in angle calculation.
You can adjust the “Gyro Scale Factor Ratio” using these instructions. The Gyro Scale Factor Ratio is applied automatically when set, so the impact of changing the ratio should be immediately evident in the yaw angle.
In general, if the amount of yaw angle error is positive, the Gyro Scale Factor Ratio should be decreased, and vice-versa. This calibration can occur with the sensor mounted directly on the robot, if the USB cable to reach to the PC running the calibration software.
The Gyro Scale Factor Ratio will stay in the sensor’s flash memory, even if factory calibration is re-run.
Hi! Taking a look at your code I have a few suggestions that might help solve your problem
First, I haven’t used the Pigeon, but your existing code looks good so I would suggest keeping the style the same and using these docs to reference the necessary code changes Bring Up: Pigeon IMU — Phoenix documentation
Next, in regards to your existing code, there are a few things I would do to go about troubleshooting your problem.
Make sure “robot-relative” driving is working properly before attempting to debug field-relative driving, this is important because it may show small problems in the drivetrain subsystem that would otherwise go unnoticed.
I see you are outputting the gyro angle to the SmartDashboard in your drivetrain subsystem periodic, so I would recommend watching that value to make sure it is in the correct units, zero is pointing in the correct direction, and it is not randomly resetting itself.
If the gyro angle is correct and you are able to drive in robot-relative mode, I would suggest changing line 35 of DriveCommand.java from fieldOrientedFlag = !Robot.oi.driveController.getRawButton(6);
to fieldOrientedFlag = true;
If your button is not working properly, then that could explain why it is switching out of field-oriented mode. This will help you with temporarily debugging the issue and not having to worry about any problems with the controllers.
I hope this is helpful in solving the problem! I would be happy to answer any questions if that didn’t make complete sense or see what else I could do to help if it doesn’t resolve the issue.