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.

IMG_9950.mov

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

IMG_9951.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.

IMG_5788.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.

[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.

[supermario6409]

@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.

We had just tested this suggestion on our MK4is and this solution does in fact work, we haven't tested with field oriented but the bot works as intended.

[bhall-ctre]

@MrXpira None of the other changes we made today (including the delay in the Swerve constructor) will be necessary, only what is currently on the wait-for-cancoder-before-zero branch in this repo and the additional Timer.delay() call in the SwerveModule.

[dirtbikerxz]

Like Ben said, thanks to @MrXpira's testing, we now believe the issue to be that motor inverts were not getting set fast enough before the offset position was being set to the angle motor. Specifically if you look at @MrXpira's testing you can see that from the 1st reset to the 2nd reset a second later, module 0 and module 3 negate their positions (5539 and 1328 to -5539 and -1317).

This also explains why only MK4i (and WCP Swerve X Flipped) users have been experiencing this issue, because only those modules require the angle motor to be inverted.

The solution to this is to add a delay between setting the motor invert and calling resetToAbsolute(). This fix has been implemented on the main branch, specifically here. The main branch with the fix has been tested on a real robot with MK4i's thanks to @Bankst's team, and is verified working as expected.

Theoretically this issue is resolved, but if anyone has any further problems please report them.
Thanks again to everyone for the testing and feedback, would have been much harder to find the issue without it.

[Dylan-Powers]

I am finally coming back to this thread after seemingly reviving it. You can thank our lead design mentor for putting the resetToAbsolute() on a button binding, something I stubbornly did not do at his first request.

I have followed the conversation here and love many of the suggestions. Utilizing the waitForCanCoder() functionality looks pretty robust, especially if we also have the resetToAbsolute() on a button binding still (which we will be keeping since that is a good in-match troubleshooting tool). However, when I sat down today to clean up our Git mess and merge the new changes discussed here, I was confused as changes like this and this are not in BaseFalconSwerve/main. Instead, we seem to have gone with a simple delay on initialization. I am not necessarily opposed to that (it solves the mid-match brownout issue kind of), but I wonder why this decision was made when we had things like waitForCanCoder() working. Can someone shed some light on this? Perhaps @dirtbikerxz or @bhall-ctre?

[dirtbikerxz]

The waitForCancoder() method did not actually solve the issue since the issue had to do with setting inverts and not CANcoder data. The timer delay is required to give sufficient time for the inverts to be properly set before resetting the motors to absolute position. As you can see in @MrXpira's testing.

However, like Ben suggested the waitForCancoder() method should still be included in main, because theoretically that could be an issue (even though it is not the issue we were experiencing at this time). I just forgot to include it in main lol, will fix when I'm in front of a computer.

[Dylan-Powers]

@dirtbikerxz Thanks for the context! I also just realized that for those of us using the Thrifty Absolute Encoder, checking for errors from a CANCoder wouldn't exactly work.

Speaking of which, we have a few changes we are sitting on to make the Thrifty Absolute Encoders a drop-in replacement for the CANCoders. Right now we are still very much in the validation stage but eventually I can work on adjusting our code so that you guys could merge it with BaseFalconSwerve so that it can use the Thrifty Encoder as an option. However, if you would not merge that in any way, I would not bother with putting that all together, let me know what you think.

To be more specific, we essentially created a new class to mimic the base-level functionality of the CANCoder with just a simple analog absolute encoder like this one.

[dirtbikerxz]

Sure, the whole point of abstracting it into a separate getCanCoder() method was so that the angle encoder could be changed out in a single location, and everything else would just work. The only thing that matters is that the output of the getCanCoder() method is in degrees and CCW+. (I should really rename it from getCanCoder() to getAngleEncoder() or something). Same applies for the gyro.

[bhall-ctre]

However, when I sat down today to clean up our Git mess and merge the new changes discussed here, I was confused as changes like this and this are not in BaseFalconSwerve/main. Instead, we seem to have gone with a simple delay on initialization. I am not necessarily opposed to that (it solves the mid-match brownout issue kind of), but I wonder why this decision was made when we had things like waitForCanCoder() working. Can someone shed some light on this?

As @dirtbikerxz said, the issue was also caused by a race condition between setting inverts and setting the sensor position. Since the solution that ended up being implemented on main is a very long delay (1 second) followed by re-running the resetToAbsolute() calls, the waitForCanCoder() function is not really necessary, as the delay is excessive enough to cover both the invert issue and the bad CANcoder signal issue. If the implementation had instead done a short (20-100ms) delay immediately after setting the invert (such as here), then waitForCanCoder() would still be necessary.

Also keep in mind that waitForCanCoder() was only necessary because of the asynchronous nature of the CAN bus; non-CAN encoders do not have the same problem.

[gsesny]

@dirtbikerxz I am not sure if it related to what is happening here but it could be. We are running MK4i on Canivore with a pigeon2. The code works well except on module (module2) consistently does not get resetToAbsolute on initialization. (or that is what we think). Sometimes it gets seeded correctly but more often it does not. We get the sticky fault, "Remote Sensor is missing on Bus" when I run a selftest of that module 2 angle motor. We were however able to bind a button to resetToAbsolute and it fixes the issue every time. We are running the 1 second delay as prescribed in the swerve subsystem. Some other notes, I know we have no updated the Pigeon2 to the Current firmware and we may or may not have to look at the Canivore. The problem seems to becoming more frequent but it is only that module. The physical length of our can is about 30 feet. Anythoughts? We will do more testing on Monday, adding more resets, shortening Can and checking all firmware. We will also change out canCoder as the final step.

[bhall-ctre]

@gsesny A couple of things to test tomorrow:

• If you change the order the modules are initialized in, is it always the second module initialized that has the issue, or is it one specific module that has it?
• If you clear the sticky faults in Tuner, reproduce the issue, and run a self test again, does the "Remote Sensor is missing on Bus" sticky fault return?
  • Are you configuring the CANcoder as a remote sensor? Most teams instead use the TalonFX integrated sensor and initialize it to the CANcoder absolute position on boot.
• Are all the LEDs of the devices on the CAN bus good (i.e. no red blips)?

[gsesny]

@bhall-ctre

Thanks for the help. I have a couple of things to report (I will admit I had a personal family illness with my kids and wasn't in the lab much the past two days).

-We are using the CANcoder as intended in the code. The CanCoder initializes the Talon FX as you stated and is how most teams are using it.

-We cleared all sticky faults. I have not seen the error "Remote Sensor is missing on Bus".
-We have shortened the length of the CAN Bus and separated the Swerve modules to be on the Canivore by themselves. ----The Canivore has been updated to latest firmware.
-We are calling reset to absolute in robot.java during all disables and teleop and auto init.
-We have not changed the order of initialization yet (that is my next step)

We have seen less issues with module but still there from time to time. Once we change the order of module boots and get data I will check back in.

We also noticed there is a can wire that has been almost cut all the way through. We will fix here shortly.

We may also change out the cancoder if necessary.

[Gary-Delancy]

I'm a little late to this game here, but we are having what i think is the same issue. Problem is, we are using MK2 hardware, and I'm not sure how/if to implement the same solution. I can't find any other promising ideas and we've wasted days messing around with this.
Any suggestions on how to troubleshoot/solve this problem with MK2 hardware?

[Wing57]

Like Ben said, thanks to @MrXpira's testing, we now believe the issue to be that motor inverts were not getting set fast enough before the offset position was being set to the angle motor. Specifically if you look at @MrXpira's testing you can see that from the 1st reset to the 2nd reset a second later, module 0 and module 3 negate their positions (5539 and 1328 to -5539 and -1317).

This also explains why only MK4i (and WCP Swerve X Flipped) users have been experiencing this issue, because only those modules require the angle motor to be inverted.

The solution to this is to add a delay between setting the motor invert and calling resetToAbsolute(). This fix has been implemented on the main branch, specifically here. The main branch with the fix has been tested on a real robot with MK4i's thanks to @Bankst's team, and is verified working as expected.

Theoretically this issue is resolved, but if anyone has any further problems please report them. Thanks again to everyone for the testing and feedback, would have been much harder to find the issue without it.

Ive been mulling over this issue by myself for a week to make sure that I wasn't making any stupid mistakes but I geniuenly cant find whats going wrong. I have the same issue demonstrated in the @gsesny 's video with the modules spinning wildly out of control. I've of course tried the solution you've provided but I've also tried the other things people suggested like moving resetToAbsolute() into Robot.java and still no luck. I recognize that this isnt a physical/hardware issue because when I copy this repository and modify the CAN ID's etc it works perfectly fine. To the best of my knowledge the code for the swerve between this and my own is identical (set aside variable names) so I'm lost as to why this is happening. I would geniunely appreciate any help/suggestions for this issue. This is my code: https://github.com/rambots/Viina/tree/Arm (Arm is the most up to date branch)

[ericjanofski]

I'm assisting a team with the same issues. We've put in the Timer.delay and it seemed to allow us to drive straight and strafe upon initial init, Turning didn't work. Then after disabling teleop, the pods spun a few times, then when we enabled teleop, the spun around a few more times. Very strange behavior!

https://github.com/steppj/7250-2023-Charged-Up-Code/blob/main/src/main/java/frc/robot

[RedBread7576]

we also have encountered the same issue, with regular mk4 modules however. Assigning resettoabsolute() to a button and pressing it whenever the bug occurs is what we have done to overcome the issue

[ericjanofski]

Our solution with the mk4i was to set the angle motor invert to true. we also had to invert the drive motors. the config that was shipped with this code was incorrect.

[ritchiedc]

We have had a problem with a single wheel (module 3) coming up pointed wrong. It was intermittent and we made some changes (updating firmware, shortening can wires, etc) and the problem went away until our first competition. Our code has the 1 sec delay before the extra reset call which we left but we added a 1sec delay before the module creations as well giving more time for encoders and controllers to come up. All I can be sure of is that we did not have any more issues with wheel alignment for the remainder of the competition.

PS: almost forgot. I also put in a call to reset module 3 a second time in the reset function. Not knowing how the lower level stuff works I wanted to make sure there wasn't a problem with a buffer not being flushed or the like. When you're at competition you throw everything you can think of at the problem.

[DoctorFogarty]

I have an issue that I think is related to this where modules will lock 90 degrees off (like they ignored the offset) and we’re using the latest version of this code that has the 1 second delay in the swerve constructor. Something else has to be going on here.

I’m going to bind the ResetToAbsolute to a button tomorrow but so far this happens to us at every single meeting without fail so far on our MK4is.

https://github.com/MAikenMagic1102/Sheriff-ChargedUp-2023/blob/main/Sheriff-ChargedUp-2023/src/main/java/frc/robot/subsystems/Drive/Swerve.java

Every time the robot is enabled you can hear one or more modules spin around in place as well.

[ritchiedc]

@DoctorFogarty We had a similar problem where right/left became forward/backward, etc, but only for field oriented. For us it would happen mid match. Our driver was able to run the robot into the front wall and reset yaw a couple of times (bond to a button) and it would usually start working again. I suspect it may be pigeon2 related since it happened to all 4 modules and only in field oriented. If this sounds like your problem and you find a solution let me know and I'll do the same. I doubt it is related to this issue though.

[hollisms]

@ritchiedc do you use vision to update your odometry?

[ritchiedc]

@hollisms No we don't use vision but I see what your thinking. I would love to get vision working. Maybe next year (seems I say that every year). It would happen randomly. Now if it will only happen before the next competition so we can troubleshoot.

[DoctorFogarty]

The issue I’m describing only happens on startup I’ve never seen it happen where once we have the swerve working correctly it randomly locks up. We can run for extended periods of time without issue it’s always something that happens right after deploying new code that doesn’t effect the drive or on first robot startup to run. Our random tests in the shop don’t involve resetting our wheels to face the same direction at the start of a run, but I wonder if this problem ever happens when we do that.

[github3658]

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

We have used your code base , thank you!, for our swerve. When we use the left thumbstick all operations are as they should be. Using the right thumbstick for rotation does not work at all. Instead of the modules going to a 45 degree allowing the chassis to turn in a O they are pointed to the center of the chassis forming an X.

[github3658]

Our solution with the mk4i was to set the angle motor invert to true. we also had to invert the drive motors. the config that was shipped with this code was incorrect.

How to you set the invert if you are using Mk3 modules instead of the Mk4?