from machine import Pin, PWM, I2C, ADC
import time
# ==========================================
# 1. PIN SETUP (Matches Your PCB V2 Pinout)
# ==========================================
# Drive Motors (TB6612FNG / H-Bridge)
pwm_l = PWM(Pin(2)); pwm_l.freq(1000)
in1_l = Pin(3, Pin.OUT)
in2_l = Pin(4, Pin.OUT)
pwm_r = PWM(Pin(5)); pwm_r.freq(1000)
in1_r = Pin(6, Pin.OUT)
in2_r = Pin(7, Pin.OUT)
# Vacuum Blower & Brushes
fan_pwm = PWM(Pin(12)); fan_pwm.freq(1000)
main_brush = PWM(Pin(13)); main_brush.freq(1000)
side_brush = PWM(Pin(14)); side_brush.freq(1000)
# Bumpers & Cliff Sensors (Active LOW with internal Pull-Up)
bumper_l = Pin(15, Pin.IN, Pin.PULL_UP)
bumper_r = Pin(17, Pin.IN, Pin.PULL_UP)
cliff_l = Pin(20, Pin.IN, Pin.PULL_UP)
# Battery & Dock Voltage ADCs + Charging Switch
adc_bat = ADC(Pin(26))
adc_dock = ADC(Pin(27))
charge_en = Pin(22, Pin.OUT, value=0)
# I2C IMU (MPU6050 in Wokwi / BMI160 on real PCB)
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
MPU_ADDR = 0x68
# ==========================================
# 2. HELPER FUNCTIONS
# ==========================================
def init_imu():
try:
i2c.writeto_mem(MPU_ADDR, 0x6B, b'\x00') # Wake up MPU6050
print("[OK] IMU initialized on I2C0 (GP0/GP1)")
except Exception as e:
print("[WARN] IMU not found:", e)
def read_gyro_z():
try:
data = i2c.readfrom_mem(MPU_ADDR, 0x47, 2)
raw = (data[0] << 8) | data[1]
if raw > 32767:
raw -= 65536
return raw / 131.0 # deg/sec
except:
return 0.0
def set_motors(left_pct, right_pct):
# left_pct / right_pct: -100 to +100
left_pct = max(-100, min(100, left_pct))
right_pct = max(-100, min(100, right_pct))
if left_pct >= 0:
in1_l.value(1); in2_l.value(0)
pwm_l.duty_u16(int(65535 * (left_pct / 100.0)))
else:
in1_l.value(0); in2_l.value(1)
pwm_l.duty_u16(int(65535 * (-left_pct / 100.0)))
if right_pct >= 0:
in1_r.value(1); in2_r.value(0)
pwm_r.duty_u16(int(65535 * (right_pct / 100.0)))
else:
in1_r.value(0); in2_r.value(1)
pwm_r.duty_u16(int(65535 * (-right_pct / 100.0)))
def set_vacuum(suction_pct, brush_on):
fan_pwm.duty_u16(int(65535 * (suction_pct / 100.0)))
duty = 52000 if brush_on else 0
main_brush.duty_u16(duty)
side_brush.duty_u16(duty)
def read_scaled_voltage(adc_obj):
# Maps 0..3.3V potentiometer to 0..18.8V (matching 47k/10k divider)
return (adc_obj.read_u16() / 65535.0) * 3.3 * 5.7
# ==========================================
# 3. MAIN ROBOT CONTROL LOOP
# ==========================================
init_imu()
yaw_angle = 0.0
target_yaw = 0.0
turn_dir = 1
last_t = time.ticks_ms()
last_print = time.ticks_ms()
print("=== NOVA VACUUM SIMULATOR STARTED ===")
print("Tip: Click MPU6050 and change Gyro Z, press buttons for bumpers, or turn Pots for Battery/Dock!")
while True:
now = time.ticks_ms()
dt = time.ticks_diff(now, last_t) / 1000.0
last_t = now
# 1. Read Sensors
gz = read_gyro_z()
yaw_angle += gz * dt
v_bat = read_scaled_voltage(adc_bat)
v_dock = read_scaled_voltage(adc_dock)
hit_left = (bumper_l.value() == 0)
hit_right = (bumper_r.value() == 0)
hit_cliff = (cliff_l.value() == 0)
# 2. Charging State Check (Dock > 15V)
if v_dock > 15.0:
set_motors(0, 0)
set_vacuum(0, False)
if v_bat < 16.6:
charge_en.value(1)
state = "CHARGING"
else:
charge_en.value(0)
state = "CHARGED_FULL"
# 3. Low Battery Cutoff (< 12.0V for 4S Li-ion)
elif v_bat < 12.0:
charge_en.value(0)
set_motors(0, 0)
set_vacuum(0, False)
state = "LOW_BAT_STOP"
# 4. Obstacle / Cliff Avoidance & Zig-Zag Turn
elif hit_left or hit_right or hit_cliff:
charge_en.value(0)
state = "OBSTACLE_TURN"
print(f"[EVENT] Hazard detected! (L={hit_left}, R={hit_right}, Cliff={hit_cliff}) -> Reversing & U-Turning")
set_motors(-50, -50)
time.sleep_ms(500)
# Pivot 180 degrees for next zig-zag row
set_motors(55 * turn_dir, -55 * turn_dir)
time.sleep_ms(700)
target_yaw = yaw_angle
turn_dir *= -1
# 5. Normal Straight-Line Gyro Cleaning
else:
charge_en.value(0)
state = "CLEANING"
set_vacuum(85, True)
# Proportional steering correction from Gyro yaw error
error = target_yaw - yaw_angle
correction = max(-35, min(35, error * 2.5))
set_motors(65 - correction, 65 + correction)
# Print telemetry every 1 second
if time.ticks_diff(now, last_print) >= 1000:
last_print = now
print(f"State: {state:12s} | Vbat: {v_bat:4.1f}V | Vdock: {v_dock:4.1f}V | Yaw: {yaw_angle:6.1f} deg")
time.sleep_ms(20)