from machine import Pin, I2C
from time import sleep
from buzzer import Buzzer
import oled
import mpu6050
from stepper import Stepper
from hcsr04 import HCSR04

i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)




led = Pin(12, Pin.OUT) 
button = Pin(27, Pin.IN, Pin.PULL_UP) 
buzzer = Buzzer(15)        

mpu = mpu6050.accel(i2c) 
oled = oled.SSD1306_I2C(128, 64, i2c) 
trigger_pin = 5
echo_pin = 18
ultrasonic = HCSR04(trigger_pin, echo_pin)  

left_step = Stepper([13])   
right_step = Stepper([19])  



def wait_button_press():
    while button.value() == 1:
        pass

def get_steps_from_distance(distance_cm):
    steps_per_cm = 200 / (2 * 3.1416 * 3)  
    steps = int(distance_cm * steps_per_cm)
    return steps


while True:
        led.value(0) 

        
        oled.fill(0)
        oled.text("Press the button", 1, 20)
        oled.text("to start Robot", 15, 35)
        oled.show()

        wait_button_press()  

        led.value(1)  

        buzzer.beep_once()  

        
        distance_cm = ultrasonic.get_distance_cm()

        
        steps = get_steps_from_distance(distance_cm)

        
        oled.fill(0)
        oled.text("{:.2f}cm".format(distance_cm), 10, 20)
        oled.text("{} steps".format(steps), 10, 40)
        oled.show()
        sleep(1)  

        reached = True  

        
        for _ in range(steps):
            left_step.move_one_step()
            right_step.move_one_step()
            sleep(0.005)  

            
            values = mpu.get_values()
            y_axis = values["AcY"]

            
            if y_axis > 10000 or y_axis < -10000:
                reached = False 
                break  

        
        if reached:
            oled.fill(0)
            oled.text("Destination Reached", 30, 30)
            oled.show()
            buzzer.beep_once()  
        else:
            led.value(1)  
            oled.fill(0)
            oled.text("Flipped Over", 30, 30)
            oled.show()
            
            for _ in range(3):
                buzzer.beep_once()
                sleep(0.5)

        sleep(2.5)