108 lines
3.1 KiB
Python
108 lines
3.1 KiB
Python
|
import umath
|
||
|
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||
|
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||
|
|
from pybricks.tools import run_task, multitask
|
||
|
|
from pybricks.tools import wait, StopWatch
|
||
|
|
from pybricks.robotics import DriveBase
|
||
|
|
from pybricks.hubs import PrimeHub
|
||
|
|
|
||
|
|
|
||
|
|
# Initialize hub and devices
|
||
|
|
hub = PrimeHub()
|
||
|
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||
|
|
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
|
||
|
|
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
|
||
|
|
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36]],[[12,20,24]]) #Added gear train list for gear ration
|
||
|
|
lazer_ranger = UltrasonicSensor(Port.E)
|
||
|
|
color_sensor = ColorSensor(Port.F)
|
||
|
|
|
||
|
|
|
||
|
|
# DriveBase configuration
|
||
|
|
WHEEL_DIAMETER = 62.4 # mm (adjust for your wheels)
|
||
|
|
AXLE_TRACK = 150 # mm (distance between wheels)
|
||
|
|
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||
|
|
drive_base.settings() #settings(600, 500, 300, 200)
|
||
|
|
drive_base.use_gyro(True)
|
||
|
|
|
||
|
|
|
||
|
|
"""
|
||
|
|
Debugging helps
|
||
|
|
"""
|
||
|
|
DEBUG = 1 # Enable when you want to show logs
|
||
|
|
# Example conversion function (adjust min/max values as needed for your hub)
|
||
|
|
async def get_battery_percentage(voltage_mV:float):
|
||
|
|
max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us
|
||
|
|
min_voltage = 5000.0 # min battery level
|
||
|
|
percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100
|
||
|
|
return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100
|
||
|
|
|
||
|
|
|
||
|
|
async def wait_button_release():
|
||
|
|
"""Wait for all buttons to be released"""
|
||
|
|
while hub.buttons.pressed():
|
||
|
|
await wait(500)
|
||
|
|
await wait(1000) # Debounce delay
|
||
|
|
|
||
|
|
|
||
|
|
WALL_DISTANCE = 300 # mm
|
||
|
|
async def drive_forward():
|
||
|
|
"""Drive forward continuously using DriveBase."""
|
||
|
|
drive_base.drive(1000,0)
|
||
|
|
|
||
|
|
|
||
|
|
async def drive_backward():
|
||
|
|
"""Drive forward continuously using DriveBase."""
|
||
|
|
drive_base.drive(400, 0)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
async def monitor_distance(stop_mm: int = WALL_DISTANCE):
|
||
|
|
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||
|
|
while True:
|
||
|
|
distance = await lazer_ranger.distance()
|
||
|
|
print('Distancing...', distance)
|
||
|
|
|
||
|
|
if distance is None:
|
||
|
|
await wait(50)
|
||
|
|
continue
|
||
|
|
if distance < stop_mm:
|
||
|
|
drive_base.stop()
|
||
|
|
print(f"Wall detected at {distance}mm!")
|
||
|
|
break
|
||
|
|
|
||
|
|
|
||
|
|
# Small delay to prevent overwhelming the sensor
|
||
|
|
await wait(50)
|
||
|
|
|
||
|
|
|
||
|
|
# Use this to set default
|
||
|
|
def set_default_speed():
|
||
|
|
drive_base.settings(600, 500, 300, 200)
|
||
|
|
|
||
|
|
|
||
|
|
# Use this to change drive base movement
|
||
|
|
def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
|
||
|
|
drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
async def main():
|
||
|
|
await drive_base.straight(500)
|
||
|
|
#await drive_base.turn(-15)
|
||
|
|
await right_arm.run_angle(100,-90)
|
||
|
|
await drive_base.straight(-176)
|
||
|
|
await drive_base.turn(15)
|
||
|
|
await right_arm.run_angle(100,90)
|
||
|
|
|
||
|
|
run_task(main())
|