Help coding the v5 inertia sensor

/*
to = timeout 
deg = degres you want to turn + for right - for left
spe = speed
*/


//resets all sensors / integers
int deg2 = 0;
Brain.Timer.reset();
Inertial.calibrate();
Inertial.setHeading(0, degrees); 





if(deg < 0){


//for going left you need to find the distence from 360
deg2 = deg + 360;

  FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
  BackL.spin(directionType::fwd, spe,velocityUnits::pct);
  FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
  BackR.spin(directionType::fwd, spe,velocityUnits::pct);

  wait(90,msec);

//loop tillthe gyro states that is withen the deadzone
while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 4) || Inertial.heading(degrees) < (deg2 - 4))){

Controller1.Screen.clearScreen(); 
Controller1.Screen.print(Inertial.heading(degrees)); 
wait(60,msec);

// telling the robot to ether turn left or right to get to where it needs to go 
if(Inertial.heading(degrees) < (deg2 - 1)){

  //turn right
  FrontL.spin(directionType::rev, spe,velocityUnits::pct);
  BackL.spin(directionType::rev, spe,velocityUnits::pct);
  FrontR.spin(directionType::rev, spe,velocityUnits::pct);
  BackR.spin(directionType::rev, spe,velocityUnits::pct);
  

}else if(Inertial.heading(degrees) > (deg2 + 1)){

  //turn left    
  FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
  BackL.spin(directionType::fwd, spe,velocityUnits::pct);
  FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
  BackR.spin(directionType::fwd, spe,velocityUnits::pct);

}

wait(50,msec);



}
}else if(deg > 0){
while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 2) || Inertial.heading(degrees) < (deg2 - 2))){




if(Inertial.heading(degrees) < (deg - 1)){

  //turn right
  FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
  BackL.spin(directionType::fwd, spe,velocityUnits::pct);
  FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
  BackR.spin(directionType::fwd, spe,velocityUnits::pct);

}else if(Inertial.heading(degrees) > (deg + 1)){

  //turn right    
  FrontL.spin(directionType::rev, spe,velocityUnits::pct);
  BackL.spin(directionType::rev, spe,velocityUnits::pct);
  FrontR.spin(directionType::rev, spe,velocityUnits::pct);
  BackR.spin(directionType::rev, spe,velocityUnits::pct);

}

So we just recently got the new inertia sensor. The code we have for it is above , and the robot will just spin in a circle and never seems to stop and I cant seem to figure out what is causing it to do this. Is this a common problem? Or did I overlook something/misplace something in the code. Also the robot even though programed to turn left 90 degrees seems to turn left and right randomly when the program is started I also printed the value of inertia sensor and it just says that it is reading 359 degrees.

1 Like

I think the problem is that you call Inertial.calibrate(); and then start moving the robot immediately.

When you Calibrate inertial sensor the robot has to be motionless or it will lead to bad gyro calibration values.

Also, if you are using the latest firmware, you don’t need to call calibrate inside your function - firmware will do that for you at the beginning.

Finally, you may want to search the forum for “Inertial” + “PID” examples. Then you could significantly simplify your code by switching to generic PID.

2 Likes