#original script by PePe80 , THX !
#modified for Pi Pico Robot by Bobby Balsa

from imu import MPU6050
import time
from machine import Pin, I2C
from pico_car import pico_car


i2c=I2C(1, scl=Pin(15),sda=Pin(14), freq=400000)
imu = MPU6050(i2c)
Motor = pico_car()

while True:
    try :
        
        #ax=round(imu.accel.x,2)
        #ay=round(imu.accel.y,2)
        #az=round(imu.accel.z,2)
        #gx=round(imu.gyro.x)
        #gy=round(imu.gyro.y)
        gz=round(imu.gyro.z)
        #tem=round(imu.temperature,2)
        
        #we only need gz to keep the robot in a straight line
        
        Motor.Car_Run(150,150)
        
        if gz > 0:
            #print("Left turn")
             Motor.Car_Run(150,0)
             
            
        elif gz < 0:
            #print("Right turn")
             Motor.Car_Run(0,150)
             
        else :
            #print("going straigh")

             time.sleep(0.2)
        
    except KeyboardInterrupt :
        
        Motor.Car_Stop()
        break