Swerve Modules Rotate Upon First Enable

Technical Programming
1

We are having an issue with our swerve drive where some of the modules will rotate to a seemingly random position then rotate back to where it started the first time the robot is enabled after power on or code is deployed. Usually only one or two modules will exhibit this behavior, and which ones do changes every so often. We are using SDS MK4i modules with Falcons and CANcoders, and our code is based of of team 364’s swerve code.

Link to our code: GitHub - hackbots-3414/2023-Energize at milton

Thanks in advance for any help in solving this.

2

Take a look at this issue: After restarting the Robot We need to re align the wheels at the start · Issue #8 · Team364/BaseFalconSwerve · GitHub

Tldr if you don’t wait after initializing your motors before initializing the CANCoders, they’ll seed the wrong angle.

1 Like
3

Thanks, but we have already implemented the fix for that issue. The modules work fine once they are done moving.

1 Like
4

Our solution for now is going to be to bind a button for resetToAbsolute and add a reset routine at the start of auto and tele. This seems to happen randomly for us also on a seemingly random module once in a while. That initial issue was solved early January, this popped up recently and we can’t seem to diagnose root cause. No magnet slipping, no offsets changing, just unsure why at the start it isn’t going to the same position.

Someone mentioned sticky faults in that same git issue, I’ll have to check that tonight, but our CAN bus is not long or seeing faults.

1 Like
5

We’re binding a button, and reseeding every second when the robot is disabled. We can reproduce the problem pretty consistently if we redeploy/restart robot code and hit Enable as fast as possible when the driverstation connects. If we wait just a moment before enabling, we can avoid the issue.

At events, the robot always sits in the disabled state for a bit before the match starts, so we’re hoping it will be OK. The unknown is if the robot reboots during a match. In this case, the wheels will probably need a manual reset since the robot becomes enabled immediately.

3 Likes
6

@Akash_Rastogi and @gdefender Thanks for the suggestions, but I think we are having different problems. Here is a video of what our modules are doing:

The modules rotate two full revolutions when the robot is enabled. Once they stop, they behave normally and the robot can drive without issue.

1 Like
7

We implemented BaseFalconSwerve from 364 and are experiencing the same thing. It only happens occasionally though, maybe half of enables it jumps more than one revolution. I think it has to do with the zeroing from the absolute encoder.

8

I just took a quick look at the code, but is there anyplace you’re handling the fact that the Falcon’s encoder does not wrap? One rotation is 2048, two rotations is 4096. Link to our code for this.

I’m also not able to find where the conversion from degrees to talon native units happens. This looks like the code is setting the falcon’s encoder with degrees, but I can’t find a coefficient being configured. I’m probably just missing it.

public void resetToAbsolute() {
    double absolutePosition = Conversions.degreesToFalcon(makePositiveDegrees(getCanCoder().getDegrees() - angleOffset.getDegrees()), Constants.Swerve.angleGearRatio);
    mAngleMotor.setSelectedSensorPosition(absolutePosition);
}
1 Like
9

FYI This isn’t our code, I am from another team than the OP.

10

I believe the Falcon encoder wrapping is handled by the SwerveModuleState optimize function:

public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
    double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
    double targetSpeed = desiredState.speedMetersPerSecond;
    double delta = targetAngle - currentAngle.getDegrees();
    if (Math.abs(delta) > 90){
        targetSpeed = -targetSpeed;
        targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180);
    }        
    return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
  }

  /**
     * @param scopeReference Current Angle
     * @param newAngle Target Angle
     * @return Closest angle within scope
     */
    private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
      double lowerBound;
      double upperBound;
      double lowerOffset = scopeReference % 360;
      if (lowerOffset >= 0) {
          lowerBound = scopeReference - lowerOffset;
          upperBound = scopeReference + (360 - lowerOffset);
      } else {
          upperBound = scopeReference - lowerOffset;
          lowerBound = scopeReference - (360 + lowerOffset);
      }
      while (newAngle < lowerBound) {
          newAngle += 360;
      }
      while (newAngle > upperBound) {
          newAngle -= 360;
      }
      if (newAngle - scopeReference > 180) {
          newAngle -= 360;
      } else if (newAngle - scopeReference < -180) {
          newAngle += 360;
      }
      return newAngle;
  }

It seems to work , as the rotation issue only happens immediately after the robot is enabled. It doesn’t happen when driving.

Conversions from CTRE units to degrees is in the Conversions library.

Both of these are directly from 364’s BaseFalconSwerve code.

1 Like
11

Yeah I agree with this statement. We have no issue once they have done their spin when it enables, then it works like a charm.

12

Did you find any resolution to this? All of a sudden we’re also experiencing this randomly upon teleop enable now.

1 Like
13

No, we haven’t found a solution, although we haven’t investigated this much further as we have been more focused on other priorities. I was actually just about to bump this thread.

1 Like
14

FYI it appears this isn’t just a handful of teams. I’m assuming multiple teams are experiencing this behavior.

2 Likes
15

Yeah it seems like an inherent problem with the 364 base falcon swerve codebase

1 Like
16

Our Open Alliance update couple of weeks ago describes how we addressed this

3 Likes
17

Thanks Brad! We’ll try this out this week.

Would you mind also sharing this in the 364 git issue I linked?

18

We implemented your makePostiveDegrees code, but we are still having the issue.

1 Like
19

Appears a bit different than their solution. You’re not referencing the old angle or checking the difference it seems?

20

I can’t check now because I don’t have access to a bot. I would recommend running with the debugger attached and putting a break point in the setAngle method and see where the odd value is coming from. What method is passing it down. There may be something odd with initialization order.