Inertial Issue with Detecting Yaw/Rotation/Heading
Hello!
I was trying to get some data from the inertial sensor, but the data seems to be printing incorrectly (ex. printing 1 deg even though it rotated 180 deg)
The weirder part is that when I manually turn the robot, it prints the correct data. Do you know what this could be?
Here is the code for reference:
// bool end = false;
dti.resetHeading();
dti.resetRotation();
// dti.calibrate(0);
Controller1.Screen.clearScreen();
angle = angle *-1;
while (1) {
Controller1.Screen.clearScreen();
Controller1.Screen.setCursor(1,1);
// Controller1.Screen.print("%d", static_cast<int>(dti.yaw(deg)));
Controller1.Screen.print("%d, : %d : %d: %d: %d",
static_cast<int>(dti.yaw(deg)),
static_cast<int>(dti.pitch(deg)),
static_cast<int>(dti.roll(deg)),
static_cast<int>(dti.heading(deg)),
static_cast<int>(dti.rotation(deg))
);
LS.spin(reverse, speed, velocityUnits::pct);
RS.spin(forward, speed, velocityUnits::pct);
}
if (0 == angle) {
return;
} else if(angle > 0){
Controller1.Screen.setCursor(1,1);
// Controller1.Screen.print("%d", static_cast<int>(dti.yaw(deg)));
Controller1.Screen.print("%d, : %d : %d: %d: %d",
static_cast<int>(dti.yaw(deg)),
static_cast<int>(dti.pitch(deg)),
static_cast<int>(dti.roll(deg)),
static_cast<int>(dti.heading(deg)),
static_cast<int>(dti.rotation(deg))
);
while(dti.yaw(deg) < (angle - 5)){
LS.spin(reverse, speed, velocityUnits::pct);
RS.spin(forward, speed, velocityUnits::pct);
if(slowDown && (dti.yaw(deg) < angle - 40) && (speed > 1)){
// speed = 1;
slowDown = false;
}
Controller1.Screen.clearScreen();
Controller1.Screen.setCursor(1,1);
// Controller1.Screen.print("%d", static_cast<int>(dti.yaw(deg)));
Controller1.Screen.print("%d, : %d : %d: %d: %d",
static_cast<int>(dti.yaw(deg)),
static_cast<int>(dti.pitch(deg)),
static_cast<int>(dti.roll(deg)),
static_cast<int>(dti.heading(deg)),
static_cast<int>(dti.rotation(deg))
);
}
LS.stop();
RS.stop();
} else /*if(angle < 0) */{
Controller1.Screen.setCursor(1,1);
Controller1.Screen.print("%d, : %d : %d: %d: %d",
static_cast<int>(dti.yaw(deg)),
static_cast<int>(dti.pitch(deg)),
static_cast<int>(dti.roll(deg)),
static_cast<int>(dti.heading(deg)),
static_cast<int>(dti.rotation(deg))
);
while (dti.yaw(deg) > (angle + 5)){
LS.spin(forward, speed, velocityUnits::pct);
RS.spin(reverse, speed, velocityUnits::pct);
if(slowDown && (dti.yaw(deg) > (angle + 40)) && speed > 1){
// speed = 1;
slowDown = false;
}
Controller1.Screen.clearScreen();
Controller1.Screen.setCursor(1,1);
Controller1.Screen.print("%d, : %d : %d: %d: %d",
static_cast<int>(dti.yaw(deg)),
static_cast<int>(dti.pitch(deg)),
static_cast<int>(dti.roll(deg)),
static_cast<int>(dti.heading(deg)),
static_cast<int>(dti.rotation(deg))
);
}
LS.stop();
RS.stop();
}
}
I do know that the function vex::smartdrive::turnFor() exists (or if not using a gyro, vex::drivetrain::turnFor() ), but I just want to experiment a bit with creating my own program for this.