Swerve Modules Rotate Upon First Enable

Technical Programming
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.

21

Same thing here as well. We implemented and our wheels spin when first enabled sometimes several times.

22

My students integrated Brad’s solution and got it working as intended. I’ll post code when I am at a desktop but here’s the gist of it written up by one of our lead programming kids, Arnav:

  1. We passed in the overloaded and original makePositiveDegrees method, the optimizeTurn method, and using make positive degrees in the resetToAbsolute as the example code provided indicated.

  2. We then used optimizeTurn in setAngle in SwerveModule.java and passed in the old angle parameter as getAngle() in SwerveModule.java and did angle = optimizeTurn(getAngle(), angle) before y’all angle in the setAngle function in SwerveModule.java

So far so good. If anything changes we’ll report back here.

Thanks a ton @Bmongar this was a massive headache we were dealing with!

3 Likes
24

I am curious what was different between @Chris_Ely 's implementation and yours. @Akash_Rastogi did you have to modify the code to get it to work?

25

@Bmongar @Chris_Ely I’m the student who wrote up the solution in Akash’s post, Arnav.

Here’s our code for this season, all of the changes I made were in SwerveModule.java in main: 2023-ChargedUp-177/SwerveModule.java at main · BobcatRobotics/2023-ChargedUp-177 · GitHub

I added in all the functions provided exactly as given in SwerveModule.java. The only change I made was in setAngle():

private void setAngle(SwerveModuleState desiredState){
        Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01)) ? lastAngle : desiredState.angle; //Prevent rotating module if speed is less then 1%. Prevents Jittering.
        Rotation2d oldAngle = getAngle();
        angle = optimizeTurn(oldAngle, angle);  
        mAngleMotor.set(ControlMode.Position, Conversions.degreesToFalcon(angle.getDegrees(), Constants.Swerve.angleGearRatio));
        lastAngle = angle;
    }

As you can see, I just find the old angle using the getAngle() function and then pass it into optimizeTurn and use the resulting angle as the angle variable in setAngle(). I didn’t see an implementation for optimizeTurn anyways in the solution code provided so I just presumed this was how it was meant to be used. No other changes were made in any other file than SwerveModule.java

Again, every other change we made to SwerveModule.java was exactly as documented in the solution code. You can view it all in the repository and file linked above.

Thanks for the solution again, really appreciate it!

4 Likes
26

I think i may have lost that in a merge lol, thanks. yes that was the intended use.

4 Likes
27

We didn’t add the optimizeTurn function since our understanding is that the CTREModuleState.optimize function from 364’s base code takes care of the angle optimization. We’ll try @Bmongar’s optimizeTurn and your setAngle modifications. Unfortunately our meeting for tonight has been canceled due to weather, so it’ll have to wait until Thursday.

Thanks @Bmongar, @arnavthak, and @Akash_Rastogi for all of your help!

5 Likes
28

@Bmongar’s and @arnavthak’s code worked!

2 Likes