Hello, my team is having an issue where our swerve drive has a couple second delay whenever we put in an input. When the file has only our swerve code, it works fine, but once we add our other subsystems we get a delay. For our swerve drive we are using the generated swerve from Phoenix X.
We’ve tested the times every subsystem taking System.currentTimeMillis() and we didn’t get anything absurd. Our robotPeriodic was outputting fairly high at 0.5 sec. The non swerve subsystem files we are adding are fairly barebones with code only getting motor speed and button presses.
We also made sure our radio had no bandwidth limit enabled.
Our code can be found at: https://github.com/FRC-8709/2024MainTestChassis/tree/main
Currently in the GitHub the subsystems are commented out inside RobotContainer() in the RobotContainer.java file, along with some of the trobleshooting we were doing.
We’ve often seen similar behavior when the code is trying to send CAN messages to a device that is not on the pus, due to an ID issue, wiring issue or firmware issue.
Can you share the output of your riolog? There is timing information printed there that would be helpful to see. Also there might be messages showing CAN device issues.
Is just timing how long it takes to create the command, not actually run the command.
Suggest you edit your commands to add the timing info similar to this:
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Launcher;
/** An example command that uses an example subsystem. */
public class TeleopLauncher extends Command {
private final Launcher subsystem;
private final Joystick soloStick;
private boolean finished = false;
public boolean toggle = false;
public TeleopLauncher(Launcher subsystem, Joystick soloStick) {
this.subsystem = subsystem;
//solo stick is the joystick on the far right; all functions not for driving go on this joystick
this.soloStick = soloStick;
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
long temp = System.currentTimeMillis();
//voltage goes up to 12, not linear
// 6 volts != 50% speed
//set button number 1 - 12 on joystick : all labled ex; button 1 is trigger
if (soloStick.getRawButton(1)) {
//toggle is so its only a button press, not a button hold
toggle = true;
}else if (soloStick.getRawButton(2)) {
toggle = false;
}
if(toggle){
// - NEGATIVE VALUES ONLY
subsystem.setMotors(-6);
}
else {
subsystem.setMotors(0);
}
finished = true;
long temp2 = System.currentTimeMillis();
DriverStation.reportError("TeleOpLauncher Execute time" + (temp2 - temp), false);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
subsystem.setMotors(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return finished;
}
}
That makes a lot of sense, I’ll try relocating the output times to each command file. Once I do that I’ll screenshot the riolog and place it here. My team doesn’t meet until Monday so unfortunately I can’t do too much right now. I do remember getting CAN device errors which I’ll also place here once I can see them again.
We did flash all the CANCoders to make sure they are on the right module. We also made sure the firmware was all up to date. Wiring I’m not sure of as we haven’t checked that yet.
Thank you for the advice! I’ll apply it and upload the results Monday.
It may be useful if you used Cache’s. I have implemented them in YAGSL with the next release. The main file you need to look at it this. If implemented correctly this will allow you to grab the data at any point and when the validity period is expired it will call the supplier function and update the data.
I implemented the new statements in the subsystem files to get the time for each. No subsystem looks too too high, although we are getting an error with CAN. We also get a CommandSchedular loop overrun. The only thing that seems very high right now is robotPeriodic at 0.509123s. Below I pasted screenshots of our riolog.
I took a look at your code and I noticed some things that could be slowing down your execute loops. Specifically in the setMotors() method of AngleSubsystem.java.
// motors going opposite directions, change to false if they need to go the same direction for any reason
followerMotor.setControl(new Follower(masterMotor.getDeviceID(), true));
set the mode to brake. make motor hard stop
masterMotor.setNeutralMode(NeutralModeValue.Brake);
followerMotor.setNeutralMode(NeutralModeValue.Brake);
All three of these lines seem like configuration statements that should be done once upon robot initialization. I have not used TalonFX.setControl(Follower) before but I THINK that you don’t need to repeatedly set that. Or at least there is a way to specify one motor as a follower of another once upon initialization. I do know that the .setNeutralMode() can be done upon initialization once and it will still work as intended.
Many configuration statements are not meant to be called periodically as they send a message over the CAN bus, then block until they get a response they can return. That being said, it shouldn’t cause multiple seconds of input delay. If you are still having a problem tonight, post that here with any further data you can gather and I can try to take another look.
I think that will fix it, I’ll try that out. One thing that we did was move the radio into a place in which it’s not trapped, which helped a lot. Now it’s just some optimization that we need to do. The issue is, I have no idea where else to put the master and follower motor stuff. I know it’s the placement of the followerMotor.setControl(new Follower(masterMotor.getDeviceID(), true)); line in the angleSubsytem.java and launcherSubsystems.java files that’s causing the delay. They need to be initialized once in the start of the program. There is no delay when we add the IndexerSubsystem.java file as there is no master or follower motors, it’s just one motor.
You should be able to put initialization stuff in the constructor of your subsystem.
public AngleSubsystem(TalonFX masterMotor, TalonFX followerMotor) {
this.masterMotor = masterMotor;
this.followerMotor = followerMotor;
// motors going opposite directions, change to false if they need to go the same direction for any reason
followerMotor.setControl(new Follower(masterMotor.getDeviceID(), true));
// set the mode to brake. make motor hard stop
masterMotor.setNeutralMode(NeutralModeValue.Brake);
followerMotor.setNeutralMode(NeutralModeValue.Brake);
}
In 99% of cases it will work as intended. If there is weird stuff happening you might need to put some delay in the constructor cause EVERYTHING, including the roboRIO and various vender libraries are starting upon the robot getting power, but it should work just fine for basic stuff like that.
I tried that out but it completely broke our subsystems. What’s happening now is that the motors stutter and move periodically. I tried putting only the NeutralModeValue in the constructor, only putting in followerMotor.setControl, and putting in both. All of them give the weird stuttering issue. I’ve tried taking out NeutralMode completely and just using the base state of the motor, but then the motors don’t spin at all.
Well, that is certainly strange. I haven’t worked much with Pheonix 6 or their Swerve builder so I don’t 100% know where to go from here.
Last year, we ran into an issue with our swerve not resetting the angle motors properly. It came down to the fact that the roboRIO booted faster and started the robot code before the Phoenix Server could fully start. This meant that the statement that reset the angle motor to the proper angle was sent before the Phoenix Server was ready to receive it. We discovered this by putting the module angle reset on a button and it worked after that. The official solution that WPILib and CTRE devs came up with was a delay before resetting the motors in order to let the Pheonix Server startup.
/* By pausing init for a second before setting module offsets, we avoid a bug with inverting motors.
* See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info.
*/
Timer.delay(1.0);
resetModulesToAbsolute();
You could try a delay or putting your configuration statements in a method called by teleopInit() or a button press to try this.
Couple things here. First of all, 0.5s is extremely slow, >25x slower than it should be running. If your software runs slow, it won’t be calling the motor setControl function enough, and motor safety will disable the motors until the next loop, which will cause your stuttering issue. Not sure how configuring the motors every loop is avoiding that, but it is going to slow things down even more, so I’d keep them in the constructor. The issue in both cases is execution speed. Why it’s running so slow could be a lot of factors, I don’t see anything obviously wrong aside from configuring things every loop, so once that’s fixed I’d make sure you don’t have a physical CAN issue - look into CAN usage and whether your dropping any packets. Also, I’d go into phoenix tuner and update
your devices + reset configs to factory to rule out the possibility of having something weird in terms of firmware/configuration.
So I fixed the issue. Inside each command file we had a variable called “finished” that would return false when the command is finished. We just took that out and just returned false and everything worked. We added the variable to fix another issue, but it seemed to cause more issues :(.
Looking at your changes, it appears that finished was being set to true in the execute() loop which meant that the command only got one loop before being canceled. However, you were setting those commands as default commands which meant that on the next loop, they were getting rescheduled. My guess is there there is significant overhead from CommandScheduler in cancellling and scheduling commands. Doing that three times per main robot loop was probably causing your delays. Not sure how we didn’t catch that earlier but glad you got it solved.
