i have a task that gets motor positioning and heading and anytime i run the program, the heading is really high. i ran a debug and Left is really high and is growing while Right is at where it should be
this is inside the while command
every variable is a double except for left and right which are ints and odom. left and right encoder values are floats
Blockquote
Left = FrontLeftMotor.get_position();
Right = FrontRightMotor.get_position();
odom.RightMotorEncoder = Right;
odom.LeftMotorEncoder = Left;
leftDistance = (Left * pi * diameter) / 360.0;
rightDistance = (Right * pi * diameter) / 360.0;
deltaLeft = (Left - LastLeft);
deltaRight = (Right - LastRight);
distance = (leftDistance + rightDistance) / 2.0; //distance traveled
deltaTheta = (leftDistance - rightDistance) / TrackLength;
odom.X += distance * cos(theta + deltaTheta / 2.0);
odom.Y += distance * sin(theta + deltaTheta / 2.0);
theta += deltaTheta;
heading = RadToDeg(theta);//heading is in degrees, theta is in radians
LastLeft = Left;
LastRight = Right;
pros::screen::print(pros::E_TEXT_MEDIUM,1,"X: %f", x);
pros::screen::print(pros::E_TEXT_MEDIUM,2,"Y: %f", y);
pros::screen::print(pros::E_TEXT_MEDIUM,3,"Facing Degrees %d",heading);
pros::screen::print(pros::E_TEXT_MEDIUM,9,"Left %f", Left);
pros::screen::print(pros::E_TEXT_MEDIUM,10,"Right %f",FrontRightMotor.get_position());
Blockquote