YAGSL Swerve Drive Infinitely Rotates Help Needed

Omer-Tuna.Cavdar · January 9, 2025, 8:56pm

Hello,

We are trying to rebuild our swerve code using YAGSL. Our hardware configuration is as follows:

Neos on both drive and steer motors, all connected to Spark MAX controllers
CanCoders as encoders
Pigeon 2.0 as the IMU
Mk4i L2 as the module

I have generated a config using the config generator and performed all the tests in the “When to Invert” section of the YAGSL documentation. The situation is as follows:

When I turn each module counterclockwise, both the absolute encoder and the relative encoder of the steer motor increase.
When I turn the drive motor counterclockwise (with the bevel gear facing towards the left of the robot), the relative encoder of the drive motor increases.
When I lift the robot and turn it counterclockwise, the IMU reading increases.

I have double-checked all CAN IDs, and they match with their respective modules and do not overlap with anything else.

When I levitate the robot so its wheels are not touching the ground, it behaves as expected. This means that it correctly drives in both rotation and translation without any issues. However, when I place it on the ground, the robot starts spinning around itself whenever I try to move it in any direction or even stop.

The IMU is securely mounted, and I have performed both the translational offset and mounting calibration of the Pigeon in the Phoenix software. All firmware is up to date, and I am using the 2025 release version of everything.

If this is unclear, I can share a video and the code tomorrow.

nstrike · January 9, 2025, 11:42pm

From the sounds of it you did nothing wrong! This is a simple issue that can be solved with 8 steps.
However if the 8 steps don’t solve this please contact me on the YAGSL discord.

Omer-Tuna.Cavdar · January 10, 2025, 1:50pm

Yeah definitly will try that and get back to you. Should I inverse the current config or set everything to false when it says drive invert to false in module JSONs. I will get back to you with the results.

nstrike · January 10, 2025, 1:57pm

Yes you should, it is guiding you through all possibilities of your inversion states, if you divert from it at all you may miss something

Omer-Tuna.Cavdar · January 10, 2025, 6:10pm

When we did the test for this was the config that worked best for us. Here are the results WhatsApp Video 2025-01-10 at 21.03.23_dbf1c1cf.mp4 - Google Drive. I’m not sure what causes the back and forth osilation in the z axis. I double checked the wiring and can IDs. Here is the RIO log :

Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	teleopPeriodic(): 0.000260s 
 	SmartDashboard.updateValues(): 0.000464s 
 	robotPeriodic(): 0.048527s 
 	LiveWindow.updateValues(): 0.000037s 
 	Shuffleboard.update(): 0.000000s 
  
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
Warning  1  Loop time of 0.02s overrun
  edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436) 
 Warning at edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436): Loop time of 0.02s overrun 
  
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
Warning  1  	SwerveSubsystem.periodic(): 0.000037s
	RunCommand.execute(): 0.042943s
	buttons.run(): 0.001466s
  edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) 
 Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	SwerveSubsystem.periodic(): 0.000037s 
 	RunCommand.execute(): 0.042943s 
 	buttons.run(): 0.001466s 
  
Warning  1  	teleopPeriodic(): 0.001206s
	SmartDashboard.updateValues(): 0.000612s
	robotPeriodic(): 0.053387s
	LiveWindow.updateValues(): 0.000000s
	Shuffleboard.update(): 0.000000s
  edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) 
 Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	teleopPeriodic(): 0.001206s 
 	SmartDashboard.updateValues(): 0.000612s 
 	robotPeriodic(): 0.053387s 
 	LiveWindow.updateValues(): 0.000000s 
 	Shuffleboard.update(): 0.000000s 
  
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
ERROR  -1003  CAN frame not received/too-stale. Check the CAN bus wiring, CAN bus utilization, and power to the device.  pigeon 2 2 ("") Status Signal AngularVelocityZWorld 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
Warning  1  Loop time of 0.02s overrun
  edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436) 
 Warning at edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436): Loop time of 0.02s overrun 
  
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
Warning  1  	SwerveSubsystem.periodic(): 0.000037s
	RunCommand.execute(): 0.041404s
	buttons.run(): 0.000000s
  edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) 
 Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	SwerveSubsystem.periodic(): 0.000037s 
 	RunCommand.execute(): 0.041404s 
 	buttons.run(): 0.000000s 
  
Warning  1  	teleopPeriodic(): 0.000111s
	SmartDashboard.updateValues(): 0.001855s
	robotPeriodic(): 0.067411s
	LiveWindow.updateValues(): 0.000018s
	Shuffleboard.update(): 0.000000s
  edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) 
 Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	teleopPeriodic(): 0.000111s 
 	SmartDashboard.updateValues(): 0.001855s 
 	robotPeriodic(): 0.067411s 
 	LiveWindow.updateValues(): 0.000018s 
 	Shuffleboard.update(): 0.000000s 
  
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
Warning  1  Loop time of 0.02s overrun
  edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436) 
 Warning at edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436): Loop time of 0.02s overrun 
  
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
Warning  1  	SwerveSubsystem.periodic(): 0.000019s
	RunCommand.execute(): 0.044445s
	buttons.run(): 0.000000s
  edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) 
 Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	SwerveSubsystem.periodic(): 0.000019s 
 	RunCommand.execute(): 0.044445s 
 	buttons.run(): 0.000000s 
  
 CommandScheduler loop overrun 
Warning  1  	teleopPeriodic(): 0.001057s
	SmartDashboard.updateValues(): 0.002078s
	robotPeriodic(): 0.038844s
	LiveWindow.updateValues(): 0.000018s
	Shuffleboard.update(): 0.000000s
  edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) 
 Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	teleopPeriodic(): 0.001057s 
 	SmartDashboard.updateValues(): 0.002078s 
 	robotPeriodic(): 0.038844s 
 	LiveWindow.updateValues(): 0.000018s 
 	Shuffleboard.update(): 0.000000s 
  
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
Warning  1  Loop time of 0.02s overrun
  edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436) 
 Warning at edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436): Loop time of 0.02s overrun 
  
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
 CommandScheduler loop overrun 
Warning  1  	SwerveSubsystem.periodic(): 0.000055s
	RunCommand.execute(): 0.041534s
	buttons.run(): 0.000000s
  edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) 
 Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	SwerveSubsystem.periodic(): 0.000055s 
 	RunCommand.execute(): 0.041534s 
 	buttons.run(): 0.000000s 
  
 CommandScheduler loop overrun 
Warning  1  	teleopPeriodic(): 0.000111s
	SmartDashboard.updateValues(): 0.000501s
	robotPeriodic(): 0.041571s
	LiveWindow.updateValues(): 0.000000s
	Shuffleboard.update(): 0.000000s
  edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) 
 Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): 	teleopPeriodic(): 0.000111s 
 	SmartDashboard.updateValues(): 0.000501s 
 	robotPeriodic(): 0.041571s 
 	LiveWindow.updateValues(): 0.000000s 
 	Shuffleboard.update(): 0.000000s 
  
 CommandScheduler loop overrun 
Warning  44004  FRC: The Driver Station has lost communication with the robot.  Driver Station

The IMU yaw readings seem to be correct as well

Omer-Tuna.Cavdar · January 10, 2025, 6:13pm

Here is the code on github. I’m not realy sure I can hop on discord since it is banned in Turkey. I would love to do a google meet or a zoom if you have the time though. GitHub - Omer-Tuna-Cavdar/YAGSL-Example-main

nstrike · January 10, 2025, 6:37pm

@Kully if I don’t have time to help them, can you help them with exactly what we did last night in AdvantageScope?

What we did was we mapped SmartDashboard/swerve/modules/…/Raw Absolute Encoder and SmartDashboard/swerve/modules/…Raw Angle Encoder to an AdvantageScope graph and spun a wheel 360. If it did not come back to the same place your conversion factor is off, if it goes down instead of up it needs to be inverted.

If this test succeeds you need to tune your angle motor PID on blocks. Then you can test on the ground, but only with YAGSL version 2025.1.3-1

Omer-Tuna.Cavdar · January 10, 2025, 6:54pm

Ok I will test this tomorrow when I’m back at the workshop. Thank you so much for your help!