After restarting the Robot We need to re align the wheels at the start

[varshil1999]

After Switching off the robot and restarting it the wheels don't realign to the zero position we need to use a metal 1X1 pipe again to reset it and start the robot Please Do Help Us.

[dirtbikerxz]

This code is untested since changes were last made. We will test it on a actual robot within the week and make any fixes, if we find issues.

[dirtbikerxz]

Just for reference, what module are you using?

[varshil1999]

Mk4i L2

[dirtbikerxz]

Can you also link me to your code. I looked on your GitHub profile and couldn't find it. Thanks!

[varshil1999]

Can you check with the Frc-2023-testing I have just uploaded the code from my local machine , could you take a look at it and sorry to disturb you but could i ask you a favor as I am a rookie in programming could we arrange a online meet if the problem persists. Thankyou!

[varshil1999]

Any luck?

[dirtbikerxz]

We are currently testing on our MK4's and finding no issues with the code (our MK4i's are not on a robot yet, and therefore we are unable to specifically test on a MK4i swerve base).

However we noticed in your code that you changed the angle motor invert on the MK4i's from true to false. Was there a reason for this? The MK4i's motor is inverted.

[varshil1999]

Lemme check with this and get back to you and another question is how do you provide power to your cancoder's like which power module is it connected to?

[swalha1999]

having the same problem here with MK4i L1 modules it seem that the angle of the Cancoder is not the same angle that reported from the angle from the Falcon encoder mabye that is the problem?

[dirtbikerxz]

@swalha1999 or anyone else, Can you take a video of the behavior and post it when you get a chance. We were unable to replicate the issue with MK4's and our MK4i's are unable to test yet, and won't be for a while yet.

[gsesny]

We are also seeing the same behavior. The only things changed in the code were in the constants for motor id and track width. Worked great first time. Every time we redeploy the angle of the wheel when trying to move forward is different. Regardless of power cycle or just redeploy. I have video as proof that it changes and will upload later

[dirtbikerxz]

I created a new branch: https://github.com/Team364/BaseFalconSwerve/tree/MK4i-Issue-Testing , that took the code as it was on Jan 12, 2022 and simply updated it to 2023 WPILib and fixed any things that were broken due to the update, without any other changes.

The code from Jan 12, 2022 is known good on MK4i's (many teams have used it), so theoretically if this fixes the issue, that means the issue lies somewhere in the code we changed to preset COTS module constants.

I've gone ahead and changed the constants on this branch for the Mk4i_L2 (Drive Ratio: 6.75:1, Angle Ratio: 150/7:1, Angle Motor Invert: True) to make it easier for you guys to test. Please set the remainder constants as they should be, and test this branch and let me know if the issue is fixed. If so I will implement whatever fixes need to happen on Main.

Thanks

[swalha1999]

the same problem is in the new branch every time we restart the robot code every wheel point to random angle only when all the wheels pointing stright ahead the code works correctly

[gsesny]

All,

We did some extensive testing today to verify everything that was going on.

We pulled the branched code and changed the variables. We are running Mk4i_L1 and changed our gearing from 6.75:1 to the proper gearing for L1.

We then used a 1x2 to check our offsets from prior versions. They remained exactly the same. We deployed code and drove the chassis; it responded as I would have expected. We ended this test (wheels were not pointing straight ahead) and did a power cycle on the chases. Once the chassis connected again we enabled the chassis and as before found wheels that were misaligned. Here is a before and after video.

This Video shows the chassis right after getting offsets with wheel aligned before deploying code.

https://user-images.githubusercontent.com/35275204/212404391-4e9153b5-82bb-40db-a48f-78f1139d69d3.mov

This video shows the chassis running after the code is deployed.

https://user-images.githubusercontent.com/35275204/212404530-3ce3fbca-07a5-456b-8608-4cea6b7edae7.mov

After ending the test with wheels misaligned we used the 1x2 again to align wheels and check offsets. Our wheels were aligned front and back. We redeployed the code and the chassis drove as expected.

To help trouble shoot I have attached the images of the firmware and settings of the conceders.

We are using the latest firmware.

We are happy to provide more information to help solve this. We will be working all day today, Saturday, and Sunday if you need us to do more tests.

(I am curious if the latest firmware of the conceders is causing a problem?)

Greg

[ritchiedc]

I have been watching this with interest as we will be trying to use this code for our base and was wondering if there could be an initialization timing problem. I am curious if using the roborio CAN interface vs the CANivore interface or possibly the old Roborio vs the new version could be causing the observed initialization problems.

[gsesny]

We are going to go back to a prior firmware of the cancoders to test to see if it makes a difference.

[Liquid2112]

Not sure it has anything to do with the CANcoder firmware -- it seems like they're giving the right angle value, the wheels are just centering to a different angle than the offset (+/- 20 off for us most of the time). The CANcoder value is always accurate.

[Dylan-Powers]

@ritchiedc may be onto something. After seeing a post on Chief Delphi talking about a similar issue (source here once I find it again), we tried putting the resetToAbsolute() method inside of SwerveModule one a button binding to test if it was an initialization issue. Here was our testing process:

1. Robot is powered off
2. Modules are rotated in random directions
3. Robot is powered on
4. We connect to robot
5. We enable and the swerve modules are in somewhat random orientations
6. We reset to absolute again using the button (x) that lacy and I made that reruns the 'resetToAbsolute()' method that gets the current angle from the absolute encoders, subtracts the offset and sets the integrated encoder
7. Swerve modules are aligned as expected when driving

It is our opinion that this issue lies in the resetToAbsolute() method being called too early into robot initialization. The exact issue is not known at the moment.

[dirtbikerxz]

Just to document for anyone else reading this, we tested this code with our setup and did not experience the issues observed by others. Our setup: Roborio 1.0, CANivore, TalonFX's, the original CANcoders, SDS MK4.

I do not know what changed from last year to this year that is causing the issues with the code (especially on the testing branch that is a direct import from last year with no additional changes), and I also do not know why we are not experiencing this issue while others are.

However based of @ritchiedc 's thoughts, and @Dylan-Powers 's observations, I can think of two ways to potentially fix this issue, assuming that something changed with how stuff initializes and is therefore causing this issue:

1. Delay setting the internal encoder on the Falcon to match the CANcoder to potentially wait out any initialization issues.
2. Completely remove the internal encoder from the equation, and allow the Angle Motor Falcon to run PID directly off the CANcoder itself.

I opted for option 2 here, because that's actually how we run our code internally. Last year we experienced an issue where the belt on the angle motor became worn out and actually started skipping teeth from time to time in the middle of a regional. The belt wouldn't skip much, maybe 1 or 2 teeth a match max, but even that amount of skipping teeth caused the internal encoder to no longer be accurate, and introduced a lot of wheel drag when the teeth skipped.

To solve this issue mid event we switched from using the internal encoder to using the CANcoder instead by setting up the CANcoder as a Remote Sensor for the Angle Motor. This solved our issue because even if the belt for the Angle Motor skipped, the CANcoder would always be accurate. And even after replacing the belt after the event we continued to run our code this way and it's worked great.

I've copied the changes required to switch from using the internal encoder to using the CANcoder from our internal code. The changes have been made and committed into the testing branch. I haven't had a chance to test these changes on this branch on a real robot yet, since its 3 AM right now lol. But I will test once I get into the shop tomorrow to make sure I didn't mess anything up.

Hopefully, this will fix the zeroing issues everyone is experiencing (assuming I didn't mess up copying the changes over).

[gsesny]

We tested new code and wheels spun wildly so something is off with the update (probably because you did it a 3am :). I agree with sentiment about reset absolute encoder during initialization happening too soon. Our two main coding mentors are not here today to help but we can test anything that comes up.

https://user-images.githubusercontent.com/35275204/212480956-afc5457c-8e46-4d68-a7ed-a56b45628a77.MOV

[gsesny]

@ritchiedc may be onto something. After seeing a post on Chief Delphi talking about a similar issue (source here once I find it again), we tried putting the resetToAbsolute() method inside of SwerveModule one a button binding to test if it was an initialization issue. Here was our testing process:

1. Robot is powered off
2. Modules are rotated in random directions
3. Robot is powered on
4. We connect to robot
5. We enable and the swerve modules are in somewhat random orientations
6. We reset to absolute again using the button (x) that lacy and I made that reruns the 'resetToAbsolute()' method that gets the current angle from the absolute encoders, subtracts the offset and sets the integrated encoder
7. Swerve modules are aligned as expected when driving

It is our opinion that this issue lies in the resetToAbsolute() method being called too early into robot initialization. The exact issue is not known at the moment.

We implemented this into our code and resetToAbsolute on a button binding and confirm we drive like we should when we resettoAbsolute.

[swalha1999]

I moved the resetToAbsolute() method to TeleopInit() and AutonInit() and the code worked perfectly thank a lot guys for the help the problem was ( I thank form my testing ) in calling resetToAbsolute() before the Phonix Libary server was online

[yutyra]

I moved the resetToAbsolute() method to TeleopInit() and AutonInit() and the code worked perfectly thank a lot guys for the help the problem was ( I thank form my testing ) in calling resetToAbsolute() before the Phonix Libary server was online

Same here and it’s all good! Robot is now driving well 😁

[Lethalsandwich]

I moved the resetToAbsolute() method to TeleopInit() and AutonInit() and the code worked perfectly thank a lot guys for the help the problem was ( I thank form my testing ) in calling resetToAbsolute() before the Phonix Libary server was online

Can you show a snippet of your code for how you moved resetToAbsolute() . We've been running into the same problems.

[yutyra]

I moved the resetToAbsolute() method to TeleopInit() and AutonInit() and the code worked perfectly thank a lot guys for the help the problem was ( I thank form my testing ) in calling resetToAbsolute() before the Phonix Libary server was online

Can you show a snippet of your code for how you moved resetToAbsolute() . We've been running into the same problems.

Here are the additions to our code, I hope this helps.

[danielml2]

Hey all,

We had similair issues with it, while not using the necessairly 2023 version of BaseFalconSwerve, but the old version just imported to 2023 with some of our changes.

We also scrambed our heads with what could be the reason for these things, and we believe we somewhat were able to fix it as of recently.

After printing the error codes for each setConfig call on the SwerveModule class config methods, we saw that sometimes the config wasn't being actually applied to the motors. After a lot of trial and error in other stuff before this, we just repeated the config ___ motor/encoder calls in the constructor and it seemed to start working fine from there. The last calls for the config always returned OK.

We also believe it might be because we kept the robot turned on from the moment we finished flashing all the motors and cancoders and never really restarted the whole robot, but from what this thread looks like, it probably wasn't that / or just that.

Our 2023 code: https://github.com/primo4586/SwerveDemo2022/tree/wpilib-2023-rewrite

This is essentially the only thing different we did: https://github.com/primo4586/SwerveDemo2022/blob/4d8d660f07b738448acf27d0732a9a66490e90fc/src/main/java/frc/robot/SwerveModule.java#L37-L52

Hope this helps!

[hollisms]

The CAN calls are non blocking. A hunch I have is that the set to abs was sent but the inquiry is happening prior it taking effect using a cached value in the API. I believe there is a way to wait for success on send for a timeout by checking for status.

[sshar1]

Hello, My team has been facing the same issue: sometimes (about 40 percent of the time) one of the modules is angled incorrectly. It's usually perpendicular to how it should be oriented. We've tried the solutions in this thread (configuring the motors/cancoders multiple times and moving restToAbsolute to teleopinit/autoinit). When running absoluteToZero with a button, all the wheels angle themselves in completely random ways and the wheels don't zero correctly. We've also updated the firmware of all our devices. Does anyone have any ideas?

[supermario6409]

@dirtbikerxz Our team had worked with MK4i's last year with your swerve code and when working with it we had not inverted the angle motors but instead the CANcoder, this year when we updated to the new template format the issue that others have mentioned prior showed up. We have tried inverting the angle motors and the CANcoders seperately and the issue seems to still be present.

In case you were wondering the problem was that the CANcoder would see the direction that you had defined as forward and tell the angle motors to turn a certain amount to align to forward. The good news was that when your wheel was straight there was no issue regarding the wheels as the error was negligible, however if the error was say, 90 degrees, your wheel on next startup would now be in reverse, or if you were at 45 degrees your next startup would be at 90 degrees. I will say that we did not spend nearly as long with inverting the variables as we should have but this is my input from experience with last year's version.

[gsesny]

@dirtbikerxz

Thanks for looking into the code. One of my students responded today (yutyra) that we followed the advice to reset absolute in teleop and auto init and it worked for us so far. We will stress test over the next weeks. We are done for the day and will be back in on Monday morning. I won't have access to test until then but it seems like @supermario6409 was able to complete the tests you asked for. I agree that it seems like an initialization issue. FYI we are using Roborio2. Here is the link to our code.

https://github.com/5804/ChargedUp.git

One of my kids made a mistake and created a master branch where the latest code it. So don't use main but use master.

[dsawdai]

Our team had similar issues. We used much of the code from this project in our swerve testing (very helpful, thanks!), but our code has diverged a bit since then. We had similar issues, and it took several hours of debugging. We found that some of the CTRE methods have non-blocking delays in affecting the readback angle, even with a large timeout like motor.configFactoryDefault(100); We read back the error code, and no error was returned, even though the defaults were not yet applied when the next line of code executed.

We have 2 approaches together to solve this problem.

First is that we never reset any encoders in hardware. Instead, when our code needs to zero an encoder, we just save the current raw encoder value to a zeroPosition variable (or similar math to set it to a desired position instead of zero). We then have a method that subtracts the zeroPosition from the raw encoder value before returning the "calibrated" position. Since this is all done in Java robot code and we only run a single thread, there is never an intermittent CANbus timing issue if we have back-to-back calls to zero the encoder and read back the encoder value.

Second, we found that configuring the Falcon or CANcoder may cause a single jump in the encoder value, and that jump may occur up to 100 milliseconds after the config method returns. This would periodically happen to one or two motors on our swerve drive, even though we used the same class for each of the 4 swerve modules. We ended up added a 200ms dumb wait loop (yikes!) in our initialization code, and that has solved the problem. Obviously, we can't use a dumb wait loop in any real-time part of the code, but it does not cause any issues in a constructor that is only called when the robot code initializes. It's not elegant, but one week later and we have not had a single issue with this anymore with lots of testing and driving. Here is a code snippet from our swerve module constructor.

  public SwerveModule(int driveMotorAddress, int turningMotorAddress, int cancoderAddress,
      boolean driveEncoderReversed, boolean turningEncoderReversed, boolean cancoderReveresed,
      double turningOffsetDegrees) {

    // Create motor and encoder objects
    driveMotor = new WPI_TalonFX(driveMotorAddress);
    turningMotor = new WPI_TalonFX(turningMotorAddress);
    turningCanCoder = new WPI_CANCoder(cancoderAddress);

    // configure drive motor
    driveMotor.configFactoryDefault(100);
    driveMotor.configAllSettings(CTREConfigs.swerveDriveFXConfig, 100);
    driveMotor.selectProfileSlot(0, 0);
    driveMotor.setInverted(false);
    driveMotor.enableVoltageCompensation(true);

    // configure turning motor
    turningMotor.configFactoryDefault(100);
    turningMotor.configAllSettings(CTREConfigs.swerveAngleFXConfig, 100);
    turningMotor.selectProfileSlot(0, 0);
    turningMotor.setInverted(true);
    turningMotor.enableVoltageCompensation(true);

    // other configs for drive and turning motors
    setMotorModeCoast(true);        // true on boot up, so robot is easy to push.  Change to false in autoinit or teleopinit

    // configure turning CanCoder
    turningCanCoder.configFactoryDefault(100);
    turningCanCoder.configAllSettings(CTREConfigs.swerveCanCoderConfig, 100);
    turningCanCoder.configSensorDirection(cancoderReveresed, 100);

    // configure drive encoder
    driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 100);
    driveMotor.setSensorPhase(driveEncoderReversed);

    // configure turning TalonFX encoder
    turningMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 100);
    turningMotor.setSensorPhase(turningEncoderReversed);

    // NOTE!!! When the Cancoder or TalonFX encoder settings are changed above, then the next call to 
    // getCanCoderDegrees() getTurningEncoderDegrees() may contain an old value, not the value based on 
    // the updated configuration settings above!!!!  The CANBus runs asynchronously from this code, so 
    // sending the updated configuration to the CanCoder/Falcon and then receiving an updated position measurement back
    // may take longer than this code.
    // The timeouts in the configuration code above (100ms) should take care of this, but it does not always wait long enough.
    // So, add a wait time here:
    double finishTime = System.currentTimeMillis() + 200;
    while (System.currentTimeMillis() < finishTime) {}

    // Call methods to zero the encoders in Java software (save the offset) instead of zeroing in hardware.
    zeroDriveEncoder();
    calibrateCanCoderDegrees(turningOffsetDegrees);
    calibrateTurningEncoderDegrees(getCanCoderDegrees());

  }

[dirtbikerxz]

I really appreciate everyone's testing and ideas. After talking to WPILib and CTRE devs, we believe the issue is being caused by the Phoenix Server not initializing before the resetToAbsolute() method is called. We believe the reason this issue appeared this year and not in previous years is due to changes in WPILib from 22 -> 23 that is allowing it to boot faster then before, and therefore booting faster then the Phoenix Server and causing the resetToAbsolute() method to be called before the CANcoder has sent its first signal. A guess as to why some are experiencing this issue while others aren't is that its simply possible its due to every Rio having a slightly different boot time.

Like some of you found, calling the resetToAbsolute() method on Auto/Teleop Init, or continuously calling it on disabled (this is something teams actually did even before this issue was a problem) fixes this issue because the Phoenix Server will have successfully booted by then, and the CANcoders will have already been sending their signals.

We added the "calling the resetToAbsolute() method continuously on disabled" fix to the main branch with the upgrades that allows for the COTS Swerve Module presets. And have verified that the main branch is fully functional thanks to @BanksT's team testing it on their robot with MK4i's.

However, some of the devs have theorized that there could potentially be an issue by using either of those methods if the robot browned out/lost power and the roboRIO rebooted in the middle of a match. In that situation, Teleop Init could be called instantly on code startup and the same issue where Phoenix Server doesn't boot fast enough could potentially exist in that situation.

To fix this completely, it was suggested to change the resetToAbsolute() method to actually check if the CANcoder has started sending signals before zeroing the Falcon. Thanks to Ben from CTRE for writing this fix. It has been implemented in this branch, specifically here.

This fix has been tested in Sim and does work. However it has yet to be tested on a real robot. We and others plan on testing this on Monday and will report back. If anyone has a chance to test before we do, please report back.

Additionally (assuming it works) this should output the time each resetToAbsolute() method had to wait for the CANcoder. Theoretically, only the first module should have a "long" wait time on init. If you have a chance to test this, please also take a note of these numbers that are outputted onto the DS console.

Thanks

[gsesny]

@dirtbikerxz We tested the new branch this morning and we did not get favorable results.

This is a screenshot of the rio log involving the readiness of the cancoders on the modules. As you see they take 0.0 ms to set.

Running the code results in the same original error condition. If the wheels are aligned it works. On any subsequent deploy or power cycle with wheels not aligned the resetabsolute is not being received.

[ritchiedc]

@dirtbikerxz When setting a configuration parameter with a timeout is the phoenix server used? If so could a config command be used with timeout to wait for the server to be up?

[bhall-ctre]

This is a screenshot of the rio log involving the readiness of the cancoders on the modules. As you see they take 0.0 ms to set.

Running the code results in the same original error condition. If the wheels are aligned it works. On any subsequent deploy or power cycle with wheels not aligned the resetabsolute is not being received.

@gsesny As a test, can you also log the value of absolutePosition and the error code returned by the subsequent call to setSelectedSensorPosition() in the resetToAbsolute() method? I'm wondering if the CANcoder position is actually good, but the setter is failing. (Note: you will have to make the timeout on the setter 50-100 ms to get a useful error code, the default is 0)

I'd also be interested to know if the same value for absolutePosition is reported if resetToAbsolute() is called a second or so later in the robot program (without actually moving the robot).

[MrXpira]

@bhall-ctre Im not sure if this was what you were looking for, but I also tried logging the absolute value.

[bhall-ctre]

@MrXpira Yep, that tells me a CANcoder position frame is being received, so that wasn't exactly the problem. It's still good to check the error code on the signal and wait for success, it's just not the only issue at play here.

The only questions I have now are:

• Is that position correct, or is it maybe stale?
  • Get and print it out again after a second or so, without moving the bot. This is probably easiest by making resetToAbsolute() public and calling it again a second or so after construction.
• Is the setSelectedSensorPosition() call failing?
  • Add a timeout of ~100ms to the call and log the error code returned by the function (i.e. ErrorCode err = mAngleMotor.setSelectedSensorPosition(absolutePosition, 0, 100);).

[MrXpira]

@bhall-ctre that makes since. I have reported the setSelectedSensorPosition() and it only got logged once. Which makes me think it only got called once?

I made resetToAbsolute() public. Where should I call them again?

[bhall-ctre]

I'm surprised if the error code is only printed out once. I'd expect the setSelectedSensorPosition(absolutePosition, 0, 100) in resetToAbsolute() to be printed out once per module, just like the absolute position, as it should be called once for every module right now.

At the end of the Swerve subsystem constructor, you can add a Timer.delay(1.0) and re-call resetToAbsolute() on all the modules (for loop over them). That should be sufficient for the test.

[MrXpira]

I couldnt find a "Task.delay(1.0)" so i ended up using Timer.delay(). Looking at the logs, it didnt seem to change the encoder angles.

[MrXpira]

The Set Selected Position seems to still only be logged once during initial call and once for the second call.

[bhall-ctre]

Yep, sorry, it's Timer.delay(), that was my bad (I've worked with too much C#).

Still surprised by the position call only being printed once. Is your most recent test code uploaded to a publicly visible GitHub I can check out?

[MrXpira]

https://github.com/MrXpira/BaseFalconSwerve-1

[MrXpira]

Not sure what I did besides changing formatting, but now position is being reported OK for every module.

[bhall-ctre]

Okay, a couple more questions:

• If you run the code as it is on your branch now (with the 1-second delay and re-running resetToAbsolute()), does the issue go away?
• Can you log the Falcon 500 getSelectedSensorPosition() and the CANcoder getAbsolutePosition() value at the start of each resetToAbsolute() call?

Also, you can remove the logging of the error code to clean up the console output, since it's all OK I don't think that's the problem.

[MrXpira]

[MrXpira]

I couldn't get the joystick to work with the robot. But I think it might of worked. I had it ontop of a box and running the auto, the wheels seemed to be aligned. Unfortunately, I have to leave because the practice has ended but I will be able to work on it more tomorrow.

edit: Im not sure why teleop didn't let the joysticks work.

[hollisms]

https://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues.html#driver-station-does-not-detect-joysticks-at-startup

could be the above known issue...?

[bhall-ctre]

@MrXpira Thank you for all the testing today, this was actually very helpful.

I think I've identified the issue to be a race condition between setting the invert and setting the sensor position, as in Phoenix 5, it's essentially the same issue as between setting the sensor position and reading it back. Adding a short (anywhere from 0.02-0.1 seconds) Timer.delay() directly before this line on the wait-for-cancoder-before-zero branch should fix the issue.

The waitForCanCoder() function should probably be left in, just because it is still theoretically possible if status periods are modified, and it is triggered in simulation, but on hardware it should just about always succeed on the first or second loop, and the timeout for the loop can probably be reduced.

Side note: this invert issue is not present in Phoenix Pro, as invert is simply another config in Pro.

[MrXpira]

Sounds good. Glad I could be of use.

Will the Timer.Delay() still be needed in the Swerve constructor?

I will test these changes tomorrow.