Drivetrain controls with inertial sensor in Python

[Stressed]

Hi everyone, I have working on programming my team’s robot for High Stakes. I am trying to code an advanced movement system in Python to make our robot easy to control.

What I am trying to do:
I am trying to make the robot rotate in the same direction that the right joystick is. So, if I were to move my joystick 45 degrees (to the top left), the robot would move 45 degrees to face the same direction my joystick is. This should be able to work with any angle. The robot must rotate in the correct direction to reach the joystick angle the quickest. I must also be able to control the speed of the turning depending on how much you move the joystick from the center.

What I accomplished:
I have been working on this idea for quite some time now. So far, I have:
-found the joystick coordinates
-calculated the joystick angle based on the coordinates
-calculated the joysticks distance from the center based on the coordinates (the displacement) - acts as a speed percentage
-found the rotation of the robot using an inertial sensor

MY CODE IS AT THE BOTTOM OF THIS POST

What I have left to do and my problems doing it:
Now that I have all the data needed, I must plug them into functions. I want the robot to rotate with the joystick, so I was planning on doing this:

while True:
drivetrain.turn_to_heading(joystickAngle, DEGREES)

Unfortunately, this did not work. I was expecting the robot to always turn to match the joystick angle but that is not what happened. Instead, it paused then slowly rotated in one direction without stopping no matter where I moved the joystick. I also tried doing this:

while(robotAngle != joystickAngle):
drivetrain.turn_to_heading(joystickAngle, DEGREES)

But once again, I had no control over it. This is where I noticed another problem. The heading of inertial sensor kept either raising or lowering slowly on its own. No matter how still it is when I calibrate it, it is always drifting up or down. I believe that was the cause of the slow turning. But that still doesn’t explain why my code isn’t working as it should.

Can anyone help make the robot angle always rotate to the joystick angle? Also, if anyone has some advice to accurately calibrate the sensor, that would be a huge help. I appreciate those who took the time to read this post and anyone willing to help.

My code:

def user_control():
    brain.screen.clear_screen()
    drivetrain_inertial.calibrate()

    while True:
        
        #Find the joystick coordinates
        joyX = controller_1.axis1.position()
        joyY = controller_1.axis2.position()

        #Find the joystick angle
        angle_rad = math.atan2(joyY, joyX)
        joystickAngle1 = round(math.degrees(angle_rad))
        if(joyX == 0 and joyY == 0):
            joystickAngle = 0
        else:
            joystickAngle = (90 - joystickAngle1) % 360

        #Calculate the distance from (0, 0) for the speed
        speed = round(math.sqrt(joyX**2 + joyY**2))

        robotAngle = round(drivetrain_inertial.heading(DEGREES))

        while True:
            drivetrain.turn_to_heading(joystickAngle, DEGREES)

        # Uncomment for debugging
        # print("Joystick:", joyX, ",", joyY)
        # print("robotAngle:", robotAngle)
        # print("joystickAngle:", joystickAngle)
        # print("speed:", speed)

[sizzlet]

Could use a PID with the setpoint be the joystick heading and the current value the inertial heading, then use that to influence the drivetrain. If you have a tank drive robot you can just apply the positive PID output to one side and negative output to the other.

# hPID is the output of my PID control
# 0.6 is just a constant
lmOut = (-hPid) * 0.6 # left motor output
rmOut = (hPid) * 0.6 # right motor output
...
# this is for pure pursuit so I have a minimum temp_velocity, 
# but that would be your forward velocity (turn + forward)
leftMotorA.spin(FORWARD, lmOut + temp_vel, VOLT)
...
rightMotorA.spin(FORWARD, rmOut + temp_vel, VOLT)
...

[sizzlet]

Also calibrating IMU:

imu.calibrate()
while imu.is_calibrating(): wait(25, MSEC)