58 lines
2.2 KiB
Python
58 lines
2.2 KiB
Python
import sys
|
|
from pathlib import Path
|
|
# Add the project root to sys.path
|
|
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
|
import time
|
|
|
|
from RoboidControl.Participants.SiteServer import SiteServer
|
|
from RoboidControl.ParticipantUDP import ParticipantUDP
|
|
from RoboidControl.Thing import *
|
|
from RoboidControl.Things.DifferentialDrive import DifferentialDrive
|
|
from RoboidControl.Things.TouchSensor import TouchSensor
|
|
|
|
# Start a site server
|
|
site = SiteServer(port=7691)
|
|
|
|
# Create a local participant for handling communcation
|
|
# using default settings (UDP communciation over port 7681)
|
|
participant = ParticipantUDP(port=7691, local_port=7683, ip_address="127.0.0.1")
|
|
# The robot's propulsion is a differential drive
|
|
bb2b = DifferentialDrive(participant)
|
|
bb2b.name = "BB2B"
|
|
bb2b.SetDriveDimensions(0.064, 0.128)
|
|
bb2b.SetOrientation(SwingTwist.Degrees(40, 0, 0))
|
|
bb2b.SetPosition(Spherical(0.15, Direction.up))
|
|
|
|
model = Thing(parent=bb2b)
|
|
model.model_url = "https://passer.life/extras/Roller1anim1.png"
|
|
model.SetOrientation(SwingTwist.Degrees(90, 0, 0))
|
|
|
|
# It has a touch sensor at the front left of the roboid
|
|
touch_left = TouchSensor(parent=bb2b)
|
|
touch_left.name = "Touch left"
|
|
touch_left.SetPosition(Spherical(0.12, Direction.Degrees(-30, 0)))
|
|
|
|
# and other one on the right
|
|
touch_right = TouchSensor(parent=bb2b)
|
|
touch_right.name = "Touch right"
|
|
touch_right.SetPosition(Spherical(0.12, Direction.Degrees(30, 0)))
|
|
|
|
# Do forever:
|
|
while True:
|
|
# The left wheel turns forward when nothing is touched on the right side
|
|
# and turn backward when the roboid hits something on the right
|
|
wheel_speed_left = -600 if touch_right.touched_something else 600
|
|
# The right wheel does the same, but instead is controlled by
|
|
# touches on the left side
|
|
wheel_speed_right = -600 if touch_left.touched_something else 600
|
|
# When both sides are touching something, both wheels will turn backward
|
|
# and the roboid will move backwards
|
|
bb2b.SetWheelVelocity(wheel_speed_left, wheel_speed_right)
|
|
if touch_left.touched_something or touch_right.touched_something:
|
|
print(f'{wheel_speed_left} {wheel_speed_right} {bb2b.linear_velocity.distance} {bb2b.angular_velocity.distance}')
|
|
|
|
# Update the roboid state
|
|
participant.Update()
|
|
# and sleep for 100ms
|
|
time.sleep(0.1)
|