52 lines
1.9 KiB
Python
52 lines
1.9 KiB
Python
import sys
|
|
from os import path
|
|
sys.path.append(path.abspath(path.join(path.dirname(__file__), '..')))
|
|
import time
|
|
|
|
from ParticipantUDP import ParticipantUDP
|
|
from Thing import *
|
|
from Things.DifferentialDrive import DifferentialDrive
|
|
from Things.TouchSensor import TouchSensor
|
|
|
|
# Create a local participant for handling communcation
|
|
# using default settings (UDP communciation over port 7681)
|
|
participant = ParticipantUDP(port=7681, local_port=7682, 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/Roller2anim1.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)
|
|
|
|
# Update the roboid state
|
|
participant.Update()
|
|
# and sleep for 100ms
|
|
time.sleep(0.1)
|