Implement the pseudocode in this code in a simple but elegant way. Make sure it’s optimized but understandable, and don’t make things complicated.
Here are more details about sensing the hill. In this case the “function” is straight():
Once the imu detects an upward slope, which is when GY > 5 degrees per second (dps), run the necessary function, until it detects a downward slope, which is when GY < -5. when it detects this downward slope, stop running the function only when GY equalizes at around 0 again. So, starts going up, keeps running the function, detects a download slope, keep running the function, GY equalizes back to normal value, stops function.


from time import sleep   # import the time library for the sleep function
from basehat import Button, LineFinder, IMUSensor
from buildhat import Motor, ColorSensor

# Initialize motors and sensors
motorA = Motor('A')
motorB = Motor('B')
lineFinderL = LineFinder(24)
lineFinderR = LineFinder(26)
IMU = IMUSensor()
colorPort = "C"
color = ColorSensor(colorPort)

direction = 1

def control_motors(speedA, speedB):
    motorA.start(speedA)
    motorB.start(speedB)

def stop_motors():
    """Stops both motors."""
    motorA.stop()
    motorB.stop()
    

def rightTurn(): # only turns right on crossroads
    if valL == 0 and valR == 0: # only color sensor detects a line
        control_motors(100, -100)  # forward
    elif valL == 0 and valR == 1:
        control_motors(100, 100)  # right
    elif valL == 1 and valR == 0:
        control_motors(-100, -100)  # left
    else:
        control_motors(100, 0)  # slow right
    
def leftTurn(): # only turns left on crossroads
    if valL == 0 and valR == 0: # only color sensor detects a line
        control_motors(100, -100)  # forward
    elif valL == 0 and valR == 1:
        control_motors(100, 100)  # right
    elif valL == 1 and valR == 0:
        control_motors(-100, -100)  # left
    else:
        control_motors(0, -100)  # slow left
    
def straight():
    control_motors(100, -100)
        
    if abs(encoderA) + 100 < abs(encoderB): #keeps the robot straight by observing motor rotation values
        control_motors(100, 100)
    elif abs(encoderA) > abs(encoderB) + 100:
        control_motors(-100, -100)
    
initialPositionA = motorA.get_position()
initialPositionB = motorB.get_position()

try:
    while True:
        encoderA = motorA.get_position()-initialPositionA #allows us to get a base encoder value for the motors
        encoderB = motorB.get_position()-initialPositionB
        
        valL, valR = lineFinderL.value, lineFinderR.value
        print(f"Sensor Left: {valL}, Sensor Right: {valR}") #Debugging
        magx, magy, magz = IMU.getMag()
        print(" MX = %7.2f uT  MY = %7.2f uT  MZ = %7.2f uT" % (magx, magy, magz))
        gyrx,gyry, gyrz = IMU.getGyro()
        print(" GX = %7.2f dps GY = %7.2f dps GZ = %7.2f dps" % (gyrx, gyry, gyrz))

        
        
        '''pseudocode
            1. start off with leftTurn()
            2. detects a hill
            3. runs straight() after resetting initialPositionA and initialPositionB
            4. gets off hill (will give more details about to detect hill)
            5. continues running leftTurn()
            6. detects a first magnet (when MX, MY, and MZ are all > 2000 uT)
            7. starts running rightTurn() now instead
            8. detects a second magnet
            9. Places cargo (just put a placeholder here for now so I know its supposed to drop cargo)
            10. continue with leftTurn()
            Repeat steps 1-6, but this time, when detects the first magnet, keep running leftTurn()
            11. Detects a second magnet
            12. This time, run rightTurn()
            13. Detect a third magnet
            14. Places cargo (placeholder again)
            15. Run leftTurn()
            16. Repeat steps 1-6 again.
            17. This time, when it detects the first magnet, keep running leftTurn()
            18. detects the second magnet
            19. keep running leftTurn()
            20. Detects the third magnet
            21. Drop Cargo (Placeholder)
            22. Continue with leftTurn()
            '''
            
        
        
        
        # Display encoder values for each motor
        try:
            print(f"Encoder A: {encoderA}, Encoder B: {encoderB}")
        except IOError as error:
            print("Error reading encoder:", error)

        # Small delay to reduce CPU load
        sleep(0.05)

except KeyboardInterrupt:
    stop_motors()