Need help with inertial sensor Get_heading() implementation

This is my code:

void ImuTurnleft(double degree, double kp){
  double power;
  double error;
  error = -degree + inertial_sensor.get_heading();
  while(inertial_sensor.get_heading() >= degree){
    error = -degree + inertial_sensor.get_heading();
    power = error*kp;
    leftFrontMotor = -power;
    leftBackMotor = -power;
    rightFrontMotor = -power;
    rightBackMotor = -power;

I am trying to turn left at the beginning of the auto. This code should make the robot slow down while it is going to reach the target. It worked well with get_rotation(), however, it is not corroborating with get_heading(). When I use this code, the robot is not moving. Can anyone help me with it?

edit: code tags added by mods

Try continuously printing error, inertial_sensor.get_heading(), and degree to the brain. You could also just try continuously printing inertial_sensor.get_heading() to the brain and turning your robot by hand (literally just hold it and turn it back and forth) and seeing what the results are.

1 Like

The result: it starts at 0 in the beginning. When I turn right, it is increasing gradually from 0. But, when I restart and turn left, it the number starts from 0 and become 359 then the number is decreasing from 359 as I turn left more.

So your ImuTurnLeft (and yes you should capitalize the l) is a function you defined. The code you posted is the code that defines ImuTurnLeft. Can you show your code that calls the function?

You mean how it is presented in auto?
It is:
ImuTurnleft(270, 1);
I am intended to make the robot turn 90 degrees left.

All I can see in your while loop as far as setting motor velocities is

Setting variable values is all good and well, but your motors are not going to move unless you actually set their velocities. Do you have some other task running somewhere that does motor.setVelocity(leftFrontMotor); or whatever function? Otherwise all your loop does is change variable values. I would suggest you remove leftFrontMotor and the others all together and replace those lines of code with suchAndSuchMotor.setVelocity(-power, rpm); or whatever the V5 set motor velocity command is.

Another possible issue is that you never seem to spin your motors forward. You could put motor.spin in your while loop or before it. It may or may not matter. If one doesn’t work, try the other.

Keep in mind I don’t do V5-specific coding, so I don’t know the particular V5 functions. This is all essentially psuedocode.


Thank you, but I think it is not the problem of setting velocity because the code for turning right works perfectly fine with similar code.

Alright, I think I found your issue. I think it’s best explained as what the computer evaluates at every line.

1-3. ImuTurnleft, power, and error are declared.
4. inertial_sensor.get_heading() (or only get_rotation()?) is set to = 0.
5. error defined
6. The while loop condition, get_heading() >= degree, evaluates this way:
inertial_sensor.get_heading() >= degree
(evaluates to) 0 >= 270
(evaluates to) false
7. The while loop is skipped since its condition is false.
8. Basically end of function.

But what I want to know is if

  1. Why were messing with the code after it worked?
  2. Why did you come to the forum if you already had a working solution?

P. S. I’m not upset or anything, I just don’t like to change what works because

  1. I don’t want to spend valuable time troubleshooting a redundant solution.
  2. I don’t want to break something that works.
1 Like

We are trying to figure get_heading out because we found out that it is much more accurate in auto than get_rotation in auto. For get_heading if you get one turn wrong, it wouldn’t mess up the rest of the turns because it is recording the robot’s degree from the beginning of the auto. Our right turn for heading is working very well and has consistency in auto that is why we attempt to figure out the turn left function with it. We have tried to use get_rotation for left and get_heading for right but the inertial sensor seems to be really unhappy with it. Thank you for your time to help us out.