Thank you @2775Josh.I tunned the inertial sensor like you said and compared it to the Wheel heading after driving to the end of a 25 ft hallway with random turns and back, the errors where 1 deg to13 deg the inertial sensor did better by a lot. I drove it around and got the same. I had to convert inertial rotation to custom heading because the heading that the sensor generated was wrong Testing Code:` double CalulatedIMUHeading = fmod(Inertial1.rotation(deg)1.017731141.011366640.988717633,360);
void IMUHeadingInit(){
while(true){
CalulatedIMUHeading = fmod(Inertial1.rotation(deg)1.017731141.011366640.988717633,360);
if(CalulatedIMUHeading < 0 ){
while(CalulatedIMUHeading < 0){
CalulatedIMUHeading = 360 + CalulatedIMUHeading;
}
}
}
}
double CalulatedHeadingRad = 0;
double CalulatedHeadingDeg = 0;
double OrientationStart = 0;
void OrientationInit(){
while(true){
if (LeftEncoderY.position(deg)>RightEncoderY.position(deg)){
int difference = LeftEncoderY.position(deg)-RightEncoderY.position(deg);
difference = difference % 2076;
CalulatedHeadingDeg = difference * 0.17230968573* 1.010271089411.012931761.0756543564*0.99538803543
+OrientationStart;
CalulatedHeadingDeg = fmod(CalulatedHeadingDeg,360);
}
else{
int difference = RightEncoderY.position(deg)-LeftEncoderY.position(deg);
difference = difference % 2076;
CalulatedHeadingDeg = 360 -( difference * 0.17230968573* 1.010271089411.012931761.0756543564*0.99538803543
)
+OrientationStart;
CalulatedHeadingDeg = fmod(CalulatedHeadingDeg,360);
}
if (CalulatedHeadingDeg ==360){
CalulatedHeadingDeg= 0;
}
CalulatedHeadingRad= CalulatedHeadingDeg* M_PI / 180;
task::sleep(15);
}}
void usercontrol(){
double LeftV = 0;
double RightV = 0;
double PreviousLeftV = 0;
double PreviousRightV = 0;
while(true){
double moveamount = Controller1.Axis3.position(percentUnits::pct); // Delcares the Left Vertical axis on the joystick
double turnamount = Controller1.Axis4.position(percentUnits::pct); // Delcares the Left Horizational axis on the joystick
LeftV = ((moveamount/2) + (turnamount/2))/1.25; // Delares the LeftV by calulating it using the joystick
RightV = ((moveamount/2) - (turnamount/2))/1.25;// Delares the RightV by calulating it using the joystick
if (PreviousLeftV == 0 && PreviousRightV == 0){ // Checks if it is stopped
LF.spin(directionType::fwd,LeftV,percentUnits::pct); // Spins the Left Front Motor with the calulated Velocity
LR.spin(directionType::fwd,LeftV,percentUnits::pct); // Spins the Left Rear Motor with the calulated Velocity
RF.spin(directionType::fwd,RightV,percentUnits::pct);// Spins the Right Front Motor with the calulated Velocity
RR.spin(directionType::fwd,RightV,percentUnits::pct);// Spins the Right Rear Motor with the calulated Velocity
}else{ // Else
LF.spin(directionType::fwd,(LeftV + PreviousLeftV) * 0.75 ,percentUnits::pct); // Caluates The Final Velocity by Adding the left velocity with the previous and multipling it by 3 fourths
LR.spin(directionType::fwd,(LeftV + PreviousLeftV) * 0.75,percentUnits::pct);// Caluates The Final Velocity by Adding the left velocity with the previous and multipling it by 3 fourths
RF.spin(directionType::fwd,(RightV + PreviousRightV)* 0.75,percentUnits::pct);// Caluates The Final Velocity by Adding the right velocity with the previous and multipling it by 3 fourths
RR.spin(directionType::fwd,(RightV + PreviousRightV)* 0.75,percentUnits::pct);}// Caluates The Final Velocity by Adding the right velocity with the previous and multipling it by 3 fourths
if (PreviousLeftV >= 0.0000000000000001 || PreviousLeftV <= -0.00000000000001){ // If the previous state was stopped
LeftV = (LeftV + PreviousLeftV) * 0.75; // Delcares The Velocity
}
if (PreviousRightV >= 0.0000000000000001 || PreviousRightV <= -0.00000000000001){// If the previous state was stopped
RightV =(RightV + PreviousRightV) * 0.75;// Delcares The Velocity
}
PreviousLeftV = LeftV; // Sets the LeftV to the previous LeftV
PreviousRightV = RightV; // Sets the RightV the previous RightV
if (PreviousLeftV >= 0.000000000000000000000000000001){ // Checks If The Left V is greater than 0
PreviousLeftV = PreviousLeftV - 0.09; // Decreases the Velocity
}
else{
if (PreviousLeftV <= -0.000000000000000000000000000001){ // Checks If The Left V is less than 0
PreviousLeftV = PreviousLeftV +0.09; // Decreases the Velocity
}
}
if (PreviousRightV >= 0.000000000000000000000000000001){ // Checks If The Right V is greater than 0
PreviousRightV = PreviousRightV - 0.09;// Decreases the Velocity
}
else{
if (PreviousRightV <= -0.000000000000000000000000000001){// Checks If The Right V is less than 0
PreviousRightV = PreviousRightV+0.09;// Decreases the Velocity
}
}
wait(25,msec); // A Small wait to save resources
}
}
competition VRC;
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
Inertial1.calibrate();
while(Inertial1.isCalibrating()){
wait(25,msec);
}
vexcodeInit();
VRC.drivercontrol(usercontrol);
thread InitializeT1(OrientationInit);
thread InitializeT2(IMUHeadingInit);
while (true){
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print(“IMU Heading:”);
Brain.Screen.print(CalulatedIMUHeading);
Brain.Screen.setCursor(2, 1);
Brain.Screen.print("IMU Rotation:");
Brain.Screen.print(Inertial1.rotation(deg) * 1.01773114*1.01136664*0.988717633);
Brain.Screen.setCursor(3, 1);
Brain.Screen.print("Wheel Heading:");
Brain.Screen.print(CalulatedHeadingDeg);
task::sleep(1025);
}
}`