diff options
| -rw-r--r-- | robot/.gitignore | 4 | ||||
| -rw-r--r-- | robot/main.py | 89 |
2 files changed, 93 insertions, 0 deletions
diff --git a/robot/.gitignore b/robot/.gitignore new file mode 100644 index 0000000..7e576c0 --- /dev/null +++ b/robot/.gitignore @@ -0,0 +1,4 @@ +__pycache__/ +*.pyc +.venv/ +.vscode/
\ No newline at end of file diff --git a/robot/main.py b/robot/main.py new file mode 100644 index 0000000..9d789f8 --- /dev/null +++ b/robot/main.py @@ -0,0 +1,89 @@ +#!/usr/bin/env pybricks-micropython +from pybricks.hubs import EV3Brick +from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, + InfraredSensor, UltrasonicSensor, GyroSensor) +from pybricks.parameters import Port, Stop, Direction, Button, Color +from pybricks.tools import wait, StopWatch, DataLog +from pybricks.robotics import DriveBase +from pybricks.media.ev3dev import SoundFile, ImageFile +from umqtt.robust import MQTTClient +import time + + +# This program requires LEGO EV3 MicroPython v2.0 or higher. +# Click "Open user guide" on the EV3 extension tab for more information. + +# MQTT setup +MQTT_ClientID = 'bob' +MQTT_Broker = 'marcelus.net' +MQTT_Topic_Status = 'move' +client = MQTTClient(MQTT_ClientID, MQTT_Broker, 1883) + +# Objects +ev3 = EV3Brick() +left_motor = Motor(Port.B) +right_motor = Motor(Port.C) +bob = DriveBase(left_motor, right_motor, wheel_diameter = 54, axle_track = 105) +UltraSensor = UltrasonicSensor(Port.S4) +get_tick_rate = 0.1 +send_tick_rate = 0.5 +sensitivity = 400 +ultraSend = [False] + + +# Functions +def listen(topic, msg) : + if topic == MQTT_Topic_Status.encode() : + data = str(msg.decode()) + ev3.screen.print(data) + if data == "STOP" : + bob.stop() + if data == "ULT1" : + ultraSend[0] = True + if data == "ULT0" : + ultraSend[0] = False + if data[0] == 'T' : + bob.turn(int(data[1:])) + if data[0] == 'D' : + bob.drive(int(data[1:]),0) + + + +def publish(data) : + client.publish(MQTT_Topic_Status, str(data)) + # debug + print(data) + +def sendUltra() : + dist = UltraSensor.distance() + if (dist < sensitivity): + publish(dist) + + +# MQTT connection +client.connect() +time.sleep(0.5) +ev3.screen.print("Connected") +client.set_callback(listen) +client.subscribe(MQTT_Topic_Status) +time.sleep(0.5) +ev3.screen.print("Listenning...") + +# Code +############################################################ + +# Start is used for the tick_rate to send and get messages +start = time.time() +# ult is used to differentiate the get and send tick rate +ult = None + +while True : + if (time.time() > start + get_tick_rate) : + client.check_msg() + if (ultraSend[0]) : + if (ult == None) : + ult = start + if (ult != None and time.time() > ult + send_tick_rate) : + sendUltra() + ult = None + start = time.time() |