Looking for Help with IMU Yaw angle turn

[lydiaD]

My team is a 4H team which means that we often have limited time and resources. We feel lucky we even got to work this season! We are in our third year but we are still learning basic Java and even some of the basics of the robot. This season we wanted to use the camera for ring detection and the IMU. Two different programmers were tasked with creating tests based on the sample code and then we combined the code. In our combined code the IMU turn(), turnLeft() and turnRight() methods are not working. We DO get a Yaw angle reading but setting power to the motors does nothing. We do realize that we don't need all three turn methods but our programmer who was playing with the IMU wrote all three. =)

Autonomous with Camera and IMU
The yaw angle is being returned in turn(), turnLeft() and turnRight() but the motors are not moving.
https://gist.github.com/soulreverie/...1473bb6abf748d

Autonomous with IMU
Program is working.
https://gist.github.com/soulreverie/...a2c5d47585581d

What are we missing?? Any insight or help would be appreciated. While we can use our driveBot() method for turns, we were really hoping to use our new IMU based methods!

[mjurisch2017]

Have you upgraded to the 6.2 SDK? Also have you increased the motor powers to see if this fixes the problem. 0.2 power may not be high enough for the wheels to turn the robot depending on friction, weight, etc.

[lydiaD]

We should be up to date but I will double-check this evening - good thought.

The 0.2 motor power is enough - we tested to see what motor power results in the least amount of overshoot. And the turn methods all work in the second program, just not in the first. (We have multiple turn methods as a result of the learning process from one of our programmers. We haven't cleaned up yet!)

One major difference betweent the two programs is that we are not using the driveBot() method or the camera in the IMU program. I'm wondering if we have an issue with the motors somehow being stuck in STOP_AND_RESET_ENCODER? I find the documentation confusing sometimes!
https://ftctechnh.github.io/ftc_app/..._RESET_ENCODER

Further, it should be noted that setting a motor toSTOP_AND_RESET_ENCODER may or may not be a transient state: motors connected to some motor controllers will remain in this mode until explicitly transitioned to a different one, while motors connected to other motor controllers will automatically transition to a different mode after the reset of the encoder is complete.

I also got a suggestion from reddit that we have a sign issue that is resulting in no movement.

Thanks so much for taking a look!

[3805Mentor]

Have you upgraded to the 6.2 SDK? Also have you increased the motor powers to see if this fixes the problem. 0.2 power may not be high enough for the wheels to turn the robot depending on friction, weight, etc.

On lines 243, and 244 you call stop and reset encoders, then the Turn method is called.

right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCOD ER);

There are 4 run modes, and you are always in one of them. Stop and reset, resets the encoders to zero, AND disables the motors. Your Turn method does not change the run mode, so the motors are disabled and do not turn.

Another thing I noticed is your turn methods do not update yawAngle before the while, so your first pass through the while loop is testing based on old data, unless all your turns are based on the original angle, such as the robot is currently at 45 degrees and you want it to turn 90 degrees, you put 135 into your method, or you're assuming that the yawAngle is reset to zero before running the method. I didn't see that the IMU is reset to zero before starting Turn.

It is probably a good idea to read the angle before the loop, or use a DO WHILE loop so the angle is read before the test.

[4634 Programmer]

The 0.2 motor power is enough - we tested to see what motor power results in the least amount of overshoot.

I realize this isn't directly related to the problem at hand, however, I would highly suggest that you use a P-controller with RUE mode enabled on the drivetrain motors rather than using a fixed-power turn.

The basic idea is that you slow down the rate of rotation as you approach the target heading. Each time around the loop, you calculate the current heading error (target-current) and multiply it by an empirically tuned constant (usually around 0.012 is a good starting point) and that result then becomes the power that you apply to the motors. RUE mode will work to make sure the motor velocity actually is something close to what was commanded. Note that you may need to have a "floor" value for the power applied, because although the integral will eventually get the robot moving for very low powers, it can sometimes take a verrry long time for it to ramp up enough to make that happen.