merge conflict and readme update
old code was uploaded in the initial upload changed everything with working programm and updated the readme
This commit is contained in:
+179
-72
@@ -1,93 +1,200 @@
|
||||
import pygame
|
||||
import pygame
|
||||
import math
|
||||
import time
|
||||
import GlobalVariables as gv
|
||||
|
||||
import DataTypes as dt
|
||||
|
||||
def normalize(x, y, deadzone=0.15):
|
||||
if abs(x) < deadzone:
|
||||
x = 0.0
|
||||
if abs(y) < deadzone:
|
||||
y = 0.0
|
||||
|
||||
mag = math.hypot(x, y)
|
||||
if mag > 1.0:
|
||||
x /= mag
|
||||
y /= mag
|
||||
|
||||
return x, y
|
||||
|
||||
|
||||
def controller_loop(shared_intent: dt.ControlIntent, lock):
|
||||
def controller():
|
||||
pygame.init()
|
||||
pygame.joystick.init()
|
||||
|
||||
# This is a simple class that will help us print to the screen.
|
||||
# It has nothing to do with the joysticks, just outputting the
|
||||
# information.
|
||||
class TextPrint:
|
||||
def __init__(self):
|
||||
self.reset()
|
||||
self.font = pygame.font.Font(None, 25)
|
||||
|
||||
joysticks = {}
|
||||
prev_buttons = {}
|
||||
def tprint(self, screen, text):
|
||||
text_bitmap = self.font.render(text, True, (0, 0, 0))
|
||||
screen.blit(text_bitmap, (self.x, self.y))
|
||||
self.y += self.line_height
|
||||
|
||||
clock = pygame.time.Clock()
|
||||
def reset(self):
|
||||
self.x = 10
|
||||
self.y = 10
|
||||
self.line_height = 15
|
||||
|
||||
while True:
|
||||
# 🔑 REQUIRED: keeps joystick state updating
|
||||
pygame.event.pump()
|
||||
def indent(self):
|
||||
self.x += 10
|
||||
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.JOYDEVICEADDED:
|
||||
joy = pygame.joystick.Joystick(event.device_index)
|
||||
joysticks[joy.get_instance_id()] = joy
|
||||
prev_buttons[joy.get_instance_id()] = [0] * joy.get_numbuttons()
|
||||
print(f"[Controller] Joystick connected: {joy.get_name()}")
|
||||
def unindent(self):
|
||||
self.x -= 10
|
||||
|
||||
elif event.type == pygame.JOYDEVICEREMOVED:
|
||||
joysticks.pop(event.instance_id, None)
|
||||
prev_buttons.pop(event.instance_id, None)
|
||||
print("[Controller] Joystick disconnected")
|
||||
|
||||
if not joysticks:
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
def main():
|
||||
|
||||
def normalize_controller_input(x, y):
|
||||
# Berechne die Laenge des Vektors
|
||||
magnitude = math.sqrt(x**2 + y**2)
|
||||
|
||||
# Wenn die Laenge groesser als 1 ist, normalisiere sie
|
||||
if magnitude > 1.0:
|
||||
x /= magnitude
|
||||
y /= magnitude
|
||||
|
||||
return x, y
|
||||
|
||||
# Set the width and height of the screen (width, height), and name the window.
|
||||
screen = pygame.display.set_mode((500, 700))
|
||||
pygame.display.set_caption("Controller Inputs")
|
||||
|
||||
# Use first joystick
|
||||
joy = next(iter(joysticks.values()))
|
||||
jid = joy.get_instance_id()
|
||||
# Used to manage how fast the screen updates.
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
# ---- AXES ----
|
||||
forward = -joy.get_axis(1) # usually inverted
|
||||
sideways = joy.get_axis(0)
|
||||
# Get ready to print.
|
||||
text_print = TextPrint()
|
||||
|
||||
forward, sideways = normalize(forward, sideways)
|
||||
# This dict can be left as-is, since pygame will generate a
|
||||
# pygame.JOYDEVICEADDED event for every joystick connected
|
||||
# at the start of the program.
|
||||
joysticks = {}
|
||||
|
||||
walk = abs(forward) > 0 or abs(sideways) > 0
|
||||
done = False
|
||||
while not done:
|
||||
# Event processing step.
|
||||
# Possible joystick events: JOYAXISMOTION, JOYBALLMOTION, JOYBUTTONDOWN,
|
||||
# JOYBUTTONUP, JOYHATMOTION, JOYDEVICEADDED, JOYDEVICEREMOVED
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
done = True # Flag that we are done so we exit this loop.
|
||||
|
||||
# ---- BUTTONS ----
|
||||
buttons = joy.get_numbuttons()
|
||||
current_buttons = [joy.get_button(i) for i in range(buttons)]
|
||||
if event.type == pygame.JOYBUTTONDOWN:
|
||||
print("Joystick button pressed.")
|
||||
if event.button == 3 and gv.emote is None:
|
||||
gv.emote = "wave"
|
||||
if event.button == 2 and gv.emote is None:
|
||||
gv.robotCommunication.send_text("Huhrensohn")
|
||||
if event.button == 0:
|
||||
joystick = joysticks[event.instance_id]
|
||||
gv
|
||||
if joystick.rumble(0, 0.7, 500):
|
||||
print(f"Rumble effect played on joystick {event.instance_id}")
|
||||
|
||||
emote = None
|
||||
quit_flag = False
|
||||
if event.type == pygame.JOYBUTTONUP:
|
||||
print("Joystick button released.")
|
||||
|
||||
for i in range(buttons):
|
||||
if current_buttons[i] and not prev_buttons[jid][i]:
|
||||
# Button DOWN (edge)
|
||||
if i == 0:
|
||||
emote = "HELLO"
|
||||
elif i == 1:
|
||||
emote = "SAD"
|
||||
elif i == 9:
|
||||
quit_flag = True
|
||||
# Handle hotplugging
|
||||
if event.type == pygame.JOYDEVICEADDED:
|
||||
# This event will be generated when the program starts for every
|
||||
# joystick, filling up the list without needing to create them manually.
|
||||
joy = pygame.joystick.Joystick(event.device_index)
|
||||
joysticks[joy.get_instance_id()] = joy
|
||||
print(f"Joystick {joy.get_instance_id()} connencted")
|
||||
|
||||
prev_buttons[jid] = current_buttons
|
||||
if event.type == pygame.JOYDEVICEREMOVED:
|
||||
del joysticks[event.instance_id]
|
||||
print(f"Joystick {event.instance_id} disconnected")
|
||||
|
||||
# ---- UPDATE INTENT (atomic) ----
|
||||
with lock:
|
||||
shared_intent.move_x = forward
|
||||
shared_intent.move_y = sideways
|
||||
shared_intent.walk = walk
|
||||
# Drawing step
|
||||
# First, clear the screen to white. Don't put other drawing commands
|
||||
# above this, or they will be erased with this command.
|
||||
screen.fill((255, 255, 255))
|
||||
text_print.reset()
|
||||
|
||||
if emote:
|
||||
shared_intent.emote = emote
|
||||
# Get count of joysticks.
|
||||
joystick_count = pygame.joystick.get_count()
|
||||
|
||||
if quit_flag:
|
||||
shared_intent.quit = True
|
||||
text_print.tprint(screen, f"Number of joysticks: {joystick_count}")
|
||||
text_print.indent()
|
||||
|
||||
clock.tick(60)
|
||||
# For each joystick:
|
||||
for joystick in joysticks.values():
|
||||
jid = joystick.get_instance_id()
|
||||
|
||||
text_print.tprint(screen, f"Joystick {jid}")
|
||||
text_print.indent()
|
||||
|
||||
# Get the name from the OS for the controller/joystick.
|
||||
name = joystick.get_name()
|
||||
text_print.tprint(screen, f"Joystick name: {name}")
|
||||
|
||||
guid = joystick.get_guid()
|
||||
text_print.tprint(screen, f"GUID: {guid}")
|
||||
|
||||
power_level = joystick.get_power_level()
|
||||
text_print.tprint(screen, f"Joystick's power level: {power_level}")
|
||||
|
||||
# Usually axis run in pairs, up/down for one, and left/right for
|
||||
# the other. Triggers count as axes.
|
||||
axes = joystick.get_numaxes()
|
||||
text_print.tprint(screen, f"Number of axes: {axes}")
|
||||
text_print.indent()
|
||||
|
||||
for i in range(axes):
|
||||
axis = joystick.get_axis(i)
|
||||
gv.vector_dirmov = [axis]
|
||||
text_print.tprint(screen, f"Axis {i} value: {axis:>6.3f}")
|
||||
text_print.unindent()
|
||||
|
||||
# Get Movement Direction Vector
|
||||
# Left stick (translation)
|
||||
vx = joystick.get_axis(1) # forward/back
|
||||
vy = joystick.get_axis(0) # strafe
|
||||
|
||||
vx, vy = normalize_controller_input(vx, vy)
|
||||
|
||||
# Right stick X (rotation) — adjust axis index if needed
|
||||
omega = joystick.get_axis(2)
|
||||
|
||||
# Deadzone for rotation
|
||||
if abs(omega) < 0.2:
|
||||
omega = 0.0
|
||||
|
||||
# Apply axis snapping (only for translation)
|
||||
if 0.8 < vx and -0.2 < vy < 0.2:
|
||||
vx, vy = 1.0, 0.0
|
||||
elif vx < -0.8 and -0.2 < vy < 0.2:
|
||||
vx, vy = -1.0, 0.0
|
||||
elif -0.2 < vx < 0.2 and 0.8 < vy:
|
||||
vx, vy = 0.0, 1.0
|
||||
elif -0.2 < vx < 0.2 and vy < -0.8:
|
||||
vx, vy = 0.0, -1.0
|
||||
|
||||
# Final movement vector
|
||||
gv.vector_dirmov = [vx, vy, omega]
|
||||
|
||||
# Robot state
|
||||
if abs(vx) < 0.2 and abs(vy) < 0.2 and abs(omega) < 0.2:
|
||||
gv.robot_state = "idle"
|
||||
else:
|
||||
gv.robot_state = "walking"
|
||||
|
||||
buttons = joystick.get_numbuttons()
|
||||
text_print.tprint(screen, f"Number of buttons: {buttons}")
|
||||
text_print.indent()
|
||||
|
||||
for i in range(buttons):
|
||||
button = joystick.get_button(i)
|
||||
text_print.tprint(screen, f"Button {i:>2} value: {button}")
|
||||
text_print.unindent()
|
||||
|
||||
hats = joystick.get_numhats()
|
||||
text_print.tprint(screen, f"Number of hats: {hats}")
|
||||
text_print.indent()
|
||||
|
||||
# Hat position. All or nothing for direction, not a float like
|
||||
# get_axis(). Position is a tuple of int values (x, y).
|
||||
for i in range(hats):
|
||||
hat = joystick.get_hat(i)
|
||||
text_print.tprint(screen, f"Hat {i} value: {str(hat)}")
|
||||
text_print.unindent()
|
||||
|
||||
text_print.unindent()
|
||||
|
||||
# Go ahead and update the screen with what we've drawn.
|
||||
pygame.display.flip()
|
||||
|
||||
# Limit to 30 frames per second.
|
||||
clock.tick(30)
|
||||
|
||||
main()
|
||||
pygame.quit()
|
||||
+4
-13
@@ -19,8 +19,6 @@ class PosArray:
|
||||
def __getitem__(self, key): return self.data[key]
|
||||
def __iter__(self): return iter(self.data)
|
||||
def __repr__(self): return f"PosArray(\n{self.data}\n)"
|
||||
def copy(self):
|
||||
return PosArray(self.data.copy())
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -39,8 +37,6 @@ class DegArray:
|
||||
def __getitem__(self, key): return self.data[key]
|
||||
def __iter__(self): return iter(self.data)
|
||||
def __repr__(self): return f"PosArray(\n{self.data}\n)"
|
||||
def copy(self):
|
||||
return DegArray(self.data.copy())
|
||||
|
||||
@dataclass
|
||||
class RadArray:
|
||||
@@ -57,8 +53,7 @@ class RadArray:
|
||||
def __getitem__(self, key): return self.data[key]
|
||||
def __iter__(self): return iter(self.data)
|
||||
def __repr__(self): return f"PosArray(\n{self.data}\n)"
|
||||
def copy(self):
|
||||
return RadArray(self.data.copy())
|
||||
|
||||
|
||||
@dataclass
|
||||
class RobotCommand:
|
||||
@@ -68,10 +63,6 @@ class RobotCommand:
|
||||
|
||||
@dataclass
|
||||
class ControlIntent:
|
||||
move_x: float = 0.0
|
||||
move_y: float = 0.0
|
||||
turn: float = 0.0
|
||||
emote: str | None = None
|
||||
walk: bool = False
|
||||
quit: bool = False
|
||||
|
||||
move_vector: Vector2
|
||||
turn: float
|
||||
emote: str | None
|
||||
+81
-156
@@ -8,139 +8,113 @@ from queue import Queue
|
||||
import config as cfg
|
||||
import DataTypes as dt
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Protocol definitions (SHARED with ESP32)
|
||||
# Protocol definitions (MATCHES THE ULTIMATE .INO)
|
||||
# ============================================================
|
||||
|
||||
PACKET_VERSION = 1
|
||||
|
||||
# Packet IDs
|
||||
PKT_COMMAND = 1
|
||||
PKT_STATUS = 2
|
||||
PKT_DEBUG = 3
|
||||
PKT_CONFIG = 2
|
||||
PKT_DISPLAY = 3
|
||||
|
||||
# Field Types
|
||||
FT_ARRAY_F32 = 1 # float32 array
|
||||
FT_STRING = 2 # utf-8 string
|
||||
FT_ARRAY_F32 = 1
|
||||
FT_STRING = 2
|
||||
FT_UINT8 = 3
|
||||
FT_FLOAT32 = 4
|
||||
FT_CONFIG_VAL = 4
|
||||
|
||||
# Header: packet_id | version | payload_len | timestamp_ms
|
||||
HEADER_FMT = "<B B H I"
|
||||
HEADER_SIZE = struct.calcsize(HEADER_FMT)
|
||||
|
||||
# TLV field: type | length | value
|
||||
HEADER_FMT = "<B B H I" # ID, Ver, Len, Timestamp
|
||||
TLV_FMT = "<B H"
|
||||
TLV_SIZE = struct.calcsize(TLV_FMT)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# ESP32 Communication Thread
|
||||
# ============================================================
|
||||
|
||||
class ESP32Communication(Thread):
|
||||
def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port, timeout=cfg.timeout):
|
||||
def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port):
|
||||
super().__init__(daemon=True)
|
||||
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.sock.settimeout(timeout)
|
||||
|
||||
print(f"Connecting to ESP32 at {host}:{port} ...")
|
||||
self.sock.connect((host, port))
|
||||
print("Connected to ESP32!")
|
||||
|
||||
self.addr = (host, port)
|
||||
|
||||
# UDP Socket - Zero Lag
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
|
||||
self.tx_queue = Queue()
|
||||
self.rx_queue = Queue()
|
||||
self.running = Event()
|
||||
self.running.set()
|
||||
self.last_motion_data = None
|
||||
print(f"Communication Initialized: Target {host}:{port}")
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Thread loop
|
||||
# --------------------------------------------------------
|
||||
def run(self):
|
||||
self.sock.setblocking(False)
|
||||
|
||||
# --- THE FIX: Send Configs immediately upon starting the thread ---
|
||||
self.sync_robot_configs()
|
||||
|
||||
while self.running.is_set():
|
||||
self._handle_tx()
|
||||
self._handle_rx()
|
||||
time.sleep(0.002)
|
||||
if not self.tx_queue.empty():
|
||||
data = self.tx_queue.get()
|
||||
try:
|
||||
self.sock.sendto(data, self.addr)
|
||||
except Exception as e:
|
||||
print(f"UDP Send Error: {e}")
|
||||
|
||||
# Tiny sleep to prevent 100% CPU usage
|
||||
time.sleep(0.001)
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Public API
|
||||
# --------------------------------------------------------
|
||||
def send_command(self, radial_array :dt.RadArray, state=0, name=""):
|
||||
def sync_robot_configs(self):
|
||||
"""
|
||||
Example command packet:
|
||||
- 3x6 float array
|
||||
- uint8 state
|
||||
- optional string
|
||||
Sends the parameters from your config.py to the ESP32.
|
||||
This tells the ESP32 how to map degrees to pulses and
|
||||
how fast the Python loop is running for smoothing.
|
||||
"""
|
||||
data = np.degrees(radial_array).astype(np.uint8)
|
||||
print(data)
|
||||
fields = [
|
||||
self._tlv_array(data),
|
||||
self._tlv_uint8(state)
|
||||
print("Sending Config Sync to Robot...")
|
||||
|
||||
# Calculate interval in ms based on your FPS (tickpersec)
|
||||
interval_ms = int(1000 / cfg.standard_tickpersec)
|
||||
|
||||
# Config IDs: 1=Min, 2=Max, 3=Interval
|
||||
configs = [
|
||||
(1, 125), # servoMin (Default for 50Hz)
|
||||
(2, 625), # servoMax (Default for 50Hz)
|
||||
(3, interval_ms)
|
||||
]
|
||||
|
||||
fields = []
|
||||
for c_id, val in configs:
|
||||
# Payload: 1 byte for ID, 2 bytes for Value (Little Endian)
|
||||
payload = struct.pack("<B h", c_id, val)
|
||||
fields.append(struct.pack(TLV_FMT, FT_CONFIG_VAL, len(payload)) + payload)
|
||||
|
||||
packet = self._build_packet(PKT_CONFIG, fields)
|
||||
self.tx_queue.put(packet)
|
||||
|
||||
if name:
|
||||
fields.append(self._tlv_string(name))
|
||||
|
||||
def send_motion(self, radial_array: dt.RadArray):
|
||||
"""Sends motion only if the data has changed."""
|
||||
# 1. Convert to degrees/uint8 as you did before
|
||||
new_data = np.degrees(radial_array.data).astype(np.uint8)
|
||||
|
||||
# 2. Check if it's different from the last sent packet
|
||||
if self.last_motion_data is not None:
|
||||
if np.array_equal(new_data, self.last_motion_data):
|
||||
return # Skip sending if data is identical
|
||||
|
||||
# 3. Update the tracker and send
|
||||
self.last_motion_data = new_data.copy()
|
||||
|
||||
fields = [self._tlv(FT_ARRAY_F32, new_data.tobytes())]
|
||||
packet = self._build_packet(PKT_COMMAND, fields)
|
||||
self.tx_queue.put(packet)
|
||||
|
||||
def receive(self):
|
||||
"""
|
||||
Non-blocking receive.
|
||||
Returns (packet_id, timestamp, fields) or None
|
||||
"""
|
||||
try:
|
||||
return self.rx_queue.get_nowait()
|
||||
except Exception:
|
||||
return None
|
||||
def send_text(self, text=""):
|
||||
"""Sends a dedicated display packet independent of motion."""
|
||||
# Convert string to bytes
|
||||
text_bytes = text.encode("utf-8")
|
||||
|
||||
# Wrap in TLV format (Type 2 = FT_STRING)
|
||||
field = self._tlv(FT_STRING, text_bytes)
|
||||
|
||||
# Build the packet using the PKT_DISPLAY (ID 3)
|
||||
packet = self._build_packet(PKT_DISPLAY, [field])
|
||||
|
||||
self.tx_queue.put(packet)
|
||||
|
||||
def stop(self):
|
||||
self.running.clear()
|
||||
self.join()
|
||||
self.sock.close()
|
||||
def _tlv(self, ftype, data):
|
||||
return struct.pack(TLV_FMT, ftype, len(data)) + data
|
||||
|
||||
# --------------------------------------------------------
|
||||
# TX / RX handlers
|
||||
# --------------------------------------------------------
|
||||
def _handle_tx(self):
|
||||
if not self.tx_queue.empty():
|
||||
data = self.tx_queue.get()
|
||||
self.sock.sendall(data)
|
||||
|
||||
def _handle_rx(self):
|
||||
try:
|
||||
header = self._recv_exact(HEADER_SIZE)
|
||||
if not header:
|
||||
return
|
||||
|
||||
packet_id, version, payload_len, timestamp = struct.unpack(HEADER_FMT, header)
|
||||
|
||||
if payload_len == 0:
|
||||
# Nothing to parse
|
||||
self.rx_queue.put((packet_id, timestamp, []))
|
||||
return
|
||||
|
||||
payload = self._recv_exact(payload_len)
|
||||
if payload is None:
|
||||
return # incomplete, skip
|
||||
|
||||
fields = self._parse_tlv(payload)
|
||||
self.rx_queue.put((packet_id, timestamp, fields))
|
||||
|
||||
except BlockingIOError:
|
||||
pass
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Packet construction
|
||||
# --------------------------------------------------------
|
||||
def _build_packet(self, packet_id, fields):
|
||||
payload = b"".join(fields)
|
||||
|
||||
header = struct.pack(
|
||||
HEADER_FMT,
|
||||
packet_id,
|
||||
@@ -148,57 +122,8 @@ class ESP32Communication(Thread):
|
||||
len(payload),
|
||||
int(time.time() * 1000) & 0xFFFFFFFF
|
||||
)
|
||||
|
||||
return header + payload
|
||||
|
||||
# --------------------------------------------------------
|
||||
# TLV helpers
|
||||
# --------------------------------------------------------
|
||||
def _tlv(self, ftype, data):
|
||||
return struct.pack(TLV_FMT, ftype, len(data)) + data
|
||||
|
||||
def _tlv_array(self, array):
|
||||
# Convert to uint8 degrees before sending
|
||||
arr_uint8 = array.astype(np.uint8)
|
||||
return self._tlv(FT_ARRAY_F32, arr_uint8.tobytes())
|
||||
|
||||
def _tlv_string(self, text):
|
||||
return self._tlv(FT_STRING, text.encode("utf-8"))
|
||||
|
||||
def _tlv_uint8(self, value):
|
||||
return self._tlv(FT_UINT8, struct.pack("<B", value))
|
||||
|
||||
# --------------------------------------------------------
|
||||
# TLV parsing
|
||||
# --------------------------------------------------------
|
||||
def _parse_tlv(self, payload):
|
||||
pos = 0
|
||||
fields = []
|
||||
|
||||
while pos < len(payload):
|
||||
ftype, flen = struct.unpack(
|
||||
TLV_FMT, payload[pos:pos + TLV_SIZE]
|
||||
)
|
||||
pos += TLV_SIZE
|
||||
|
||||
data = payload[pos:pos + flen]
|
||||
pos += flen
|
||||
|
||||
fields.append((ftype, data))
|
||||
|
||||
return fields
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Socket helper
|
||||
# --------------------------------------------------------
|
||||
def _recv_exact(self, size):
|
||||
data = b""
|
||||
while len(data) < size:
|
||||
try:
|
||||
chunk = self.sock.recv(size - len(data))
|
||||
if not chunk:
|
||||
return None
|
||||
data += chunk
|
||||
except BlockingIOError:
|
||||
return None
|
||||
return data
|
||||
def stop(self):
|
||||
self.running.clear()
|
||||
self.sock.close()
|
||||
+20
-17
@@ -9,6 +9,9 @@ import config as cfg
|
||||
# Globale Variablen
|
||||
robotCommunication = None
|
||||
shared_sim = None
|
||||
emote = None
|
||||
display_text = ""
|
||||
|
||||
if cfg.sim:
|
||||
shared_sim = Simulation()
|
||||
else:
|
||||
@@ -17,7 +20,7 @@ else:
|
||||
else:
|
||||
robotCommunication = ESP32Communication()
|
||||
|
||||
vector_dirmov = [0, 0] # Direction Movement
|
||||
vector_dirmov = [0, 0, 0] # Direction Movement [vx, vy, omega]
|
||||
current_rad: dt.RadArray # Current Rad
|
||||
current_pos: dt.PosArray # Current Position
|
||||
|
||||
@@ -43,26 +46,26 @@ control_pause: bool = False
|
||||
# [90, 150, 65],
|
||||
# ]
|
||||
#)
|
||||
init_deg: dt.DegArray = dt.DegArray(
|
||||
[
|
||||
[90, 30, 95],
|
||||
[90, 30, 95],
|
||||
[90, 30, 95],
|
||||
[90, 150, 85],
|
||||
[90, 150, 85],
|
||||
[90, 150, 85],
|
||||
]
|
||||
)
|
||||
#init_deg: dt.DegArray = dt.DegArray(
|
||||
# [
|
||||
# [90, 45, 140],
|
||||
# [90, 45, 140],
|
||||
# [90, 45, 140],
|
||||
# [90, 135, 40],
|
||||
# [90, 135, 40],
|
||||
# [90, 135, 40],
|
||||
# [90, 30, 95],
|
||||
# [90, 30, 95],
|
||||
# [90, 30, 95],
|
||||
# [90, 150, 85],
|
||||
# [90, 150, 85],
|
||||
# [90, 150, 85],
|
||||
# ]
|
||||
#)
|
||||
init_deg: dt.DegArray = dt.DegArray(
|
||||
[
|
||||
[90, 45, 140],
|
||||
[90, 45, 140],
|
||||
[90, 45, 140],
|
||||
[90, 135, 40],
|
||||
[90, 135, 40],
|
||||
[90, 135, 40],
|
||||
]
|
||||
)
|
||||
|
||||
init90_deg: dt.DegArray = dt.DegArray(
|
||||
[
|
||||
|
||||
@@ -1,67 +1,103 @@
|
||||
|
||||
# JackBot — Hexapod Control & Simulation
|
||||
|
||||
**Purpose**
|
||||
- **Overview**: JackBot is a Python project for controlling and simulating a six-legged (hexapod) robot. It computes foot trajectories, runs inverse kinematics against a URDF model, and sends joint commands to either a hardware controller (ESP32 / Arduino) or a PyBullet simulation.
|
||||
JackBot is a Python project for controlling and simulating a six-legged hexapod robot.
|
||||
It uses IKPy for inverse kinematics, PyBullet for optional simulation, and can send joint commands to ESP32 or Arduino hardware.
|
||||
|
||||
**Setup Dependencies and Virtual Enviroment**
|
||||
- **Python packages**: At minimum the project uses `numpy`, `pygame`, `ikpy`, `pybullet`, `pyserial`, and `matplotlib`.
|
||||
- **Install** (recommended inside a venv):
|
||||
## Requirements
|
||||
|
||||
Linux / macOS (Bash):
|
||||
- Python 3.12 recommended on Windows
|
||||
- Python 3.11 / 3.12 is safest for `pygame` compatibility
|
||||
- Required Python packages:
|
||||
- `numpy`
|
||||
- `pygame`
|
||||
- `ikpy`
|
||||
- `pybullet`
|
||||
- `pyserial`
|
||||
- `matplotlib`
|
||||
|
||||
## Setup
|
||||
|
||||
### Linux / macOS (Bash)
|
||||
|
||||
```bash
|
||||
python3 -m venv .venv
|
||||
source .venv/bin/activate
|
||||
python -m pip install --upgrade pip setuptools wheel
|
||||
pip install numpy pygame ikpy pybullet pyserial matplotlib
|
||||
```
|
||||
|
||||
Windows (PowerShell):
|
||||
### Windows (PowerShell)
|
||||
|
||||
```powershell
|
||||
python -m venv .venv
|
||||
.\.venv\Scripts\Activate.ps1
|
||||
python -m pip install --upgrade pip setuptools wheel
|
||||
pip install numpy pygame ikpy pybullet pyserial matplotlib
|
||||
```
|
||||
|
||||
If PowerShell blocks activation, run:
|
||||
If PowerShell blocks activation:
|
||||
|
||||
```powershell
|
||||
Set-ExecutionPolicy -Scope Process -ExecutionPolicy RemoteSigned
|
||||
.\.venv\Scripts\Activate.ps1
|
||||
```
|
||||
|
||||
**Quick Start**
|
||||
- **Simulation**: enable the simulator in `config.py` by setting `sim = True`, then run:
|
||||
## Run
|
||||
|
||||
From the repository root:
|
||||
|
||||
```powershell
|
||||
python main.py
|
||||
```
|
||||
|
||||
or on Linux/macOS:
|
||||
|
||||
```bash
|
||||
python3 main.py
|
||||
```
|
||||
- **Hardware**: set `sim = False` in `config.py` and choose `arduinoConnection = True` or provide your ESP32 settings. Verify `port`, `baudrate`, `esp32_ip`, and `esp32_port` in `config.py`.
|
||||
|
||||
**Project structure (key files)**
|
||||
- **Entry point**: [main.py](main.py) — starts the controller and state loop.
|
||||
- **Controller**: [Controller.py](Controller.py) — joystick input and `ControlIntent` updates.
|
||||
- **Kinematics**: [kinematics.py](kinematics.py) — URDF-based forward/inverse kinematics using IKPy.
|
||||
- **Simulation**: [simulation.py](simulation.py) — PyBullet visualization and joint control.
|
||||
- **Hardware comms**: [EspCommunication.py](EspCommunication.py) and [ArduinoCommunication.py](ArduinoCommunication.py) — send commands to ESP32 or Arduino.
|
||||
- **Runtime globals**: [GlobalVariables.py](GlobalVariables.py) — central runtime objects and flags.
|
||||
- **Types**: [DataTypes.py](DataTypes.py) — `PosArray`, `DegArray`, `RadArray`, `ControlIntent`.
|
||||
- **States**: [RobotState/idle.py](RobotState/idle.py) and [RobotState/walking.py](RobotState/walking.py) — state machine implementations.
|
||||
- **Config**: [config.py](config.py) — ports, flags (`sim`, `arduinoConnection`), URDF path, timing values.
|
||||
## Configuration
|
||||
|
||||
**How it works**
|
||||
- `main.py` creates a `ControlIntent` and `RobotContext`, starts the controller thread, and runs a state loop. The `WalkingState` computes per-leg foot paths (Bezier/linear), converts positions to joint angles with `kinematics.ikpyInverse()`, then sends commands to hardware or updates the simulator.
|
||||
Edit `config.py` before running:
|
||||
|
||||
**Configuration notes**
|
||||
- **URDF**: the robot description file is `JackBotUrdf.urdf`; ensure the path in `config.py` (`urdf_path`) is correct.
|
||||
- **Timing**: step timing and tick rate are configured in `config.py` (`standard_duration`, `standard_tickpersec`).
|
||||
- `sim = True` to enable PyBullet simulation
|
||||
- `sim = False` to use hardware control
|
||||
- `arduinoConnection = True` to use Arduino
|
||||
- `arduinoConnection = False` to use ESP32
|
||||
- `port` and `baudrate` for Arduino
|
||||
- `esp32_ip` and `esp32_port` for ESP32
|
||||
- `urdf_path = "JackBotUrdf.urdf"`
|
||||
|
||||
**Suggested next steps**
|
||||
- Add a `requirements.txt` or `pyproject.toml` for reproducible installs.
|
||||
- Document hardware wiring and expected serial/ESP packet formats if you plan to use hardware.
|
||||
- Provide an example `config.py` for common setups (sim vs hardware).
|
||||
## Project structure
|
||||
|
||||
**License & Contributing**
|
||||
- No license file included. Add `LICENSE` if you want to publish or share.
|
||||
- Contributions: open an issue or PR with improvements; add tests for kinematics if possible.
|
||||
- `main.py` — main application entry point
|
||||
- `Controller.py` — Pygame-based controller and input display
|
||||
- `kinematics.py` — IKPy forward/inverse kinematics
|
||||
- `simulation.py` — PyBullet simulation wrapper
|
||||
- `GlobalVariables.py` — shared runtime state and comms
|
||||
- `DataTypes.py` — typed arrays and control intent
|
||||
- `RobotState/idle.py` — idle robot state
|
||||
- `RobotState/walking.py` — walking robot state
|
||||
- `EspCommunication.py` — ESP32 communication
|
||||
- `ArduinoCommunication.py` — Arduino communication
|
||||
- `JackBotUrdf.urdf` — robot model file
|
||||
|
||||
## Notes
|
||||
|
||||
- `GlobalVariables.py` initializes either `Simulation()` or the selected hardware comm class.
|
||||
- `DataTypes.py` declares `PosArray`, `DegArray`, `RadArray`, `RobotCommand`, and `ControlIntent`.
|
||||
- `Controller.py` updates `gv.vector_dirmov` and `gv.robot_state` from joystick input.
|
||||
- `kinematics.py` loads leg chains from `JackBotUrdf.urdf` and computes IK.
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
- If `pygame` installation fails on Windows, use Python 3.12 and upgrade `pip setuptools wheel` first.
|
||||
- If `python` is not found on Windows, install Python and enable "Add Python to PATH".
|
||||
- If the program crashes on startup, verify `JackBotUrdf.urdf` path and `config.py` settings.
|
||||
|
||||
## Suggested improvements
|
||||
|
||||
- Add a `requirements.txt` or `pyproject.toml`.
|
||||
- Add a `LICENSE` file before sharing the project.
|
||||
- Document hardware wiring and packet formats for ESP32/Arduino.
|
||||
+319
@@ -0,0 +1,319 @@
|
||||
import GlobalVariables as gv
|
||||
import config as cfg
|
||||
import DataTypes as dt
|
||||
import kinematics as kin
|
||||
import numpy as np
|
||||
import math
|
||||
import time
|
||||
|
||||
# Leg_Pair 1 {leg[num] = 0,2,4}
|
||||
# Leg Pair 2 {leg[num] = 1,3,5}
|
||||
|
||||
def emitCalculation(target_rad: dt.RadArray):
|
||||
if gv.shared_sim != None:
|
||||
gv.shared_sim.updatePos(target_rad)
|
||||
gv.shared_sim.step()
|
||||
if gv.robotCommunication != None:
|
||||
gv.robotCommunication.send_motion(target_rad)
|
||||
gv.current_rad = target_rad
|
||||
|
||||
def initPos():
|
||||
# Init Position and gv.current_rad
|
||||
gv.current_rad = gv.init_deg.to_rad()
|
||||
gv.current_pos = gv.center_points
|
||||
target_rad: dt.RadArray = kin.ikpyInverse(gv.center_points)
|
||||
emitCalculation(target_rad)
|
||||
|
||||
|
||||
def walking(duration=cfg.standard_duration, tickpersec=cfg.standard_tickpersec, curve_height=cfg.step_height):
|
||||
# init Values
|
||||
ticks = int(duration * tickpersec)
|
||||
tick_duration = 1 / tickpersec
|
||||
tick_pos: dt.PosArray # aktuelle XYZ-Positionen
|
||||
current_pos_copy: dt.PosArray = gv.current_pos
|
||||
vx, vy, omega = gv.vector_dirmov
|
||||
|
||||
# Apply config scaling
|
||||
vx *= cfg.translation_gain
|
||||
vy *= cfg.translation_gain
|
||||
omega *= cfg.rotation_gain
|
||||
|
||||
target_pos_temp = []
|
||||
|
||||
for i in range(6):
|
||||
cx, cy, cz = gv.center_points[i]
|
||||
|
||||
# relative position (assuming body center = 0,0)
|
||||
rx = cx
|
||||
ry = cy
|
||||
|
||||
# rotation component
|
||||
v_rot_x = -omega * ry
|
||||
v_rot_y = omega * rx
|
||||
|
||||
# combine translation + rotation
|
||||
v_x = vx + v_rot_x
|
||||
v_y = vy + v_rot_y
|
||||
|
||||
# normalize combined vector (important!)
|
||||
length = (v_x**2 + v_y**2) ** 0.5
|
||||
if length > 1.0:
|
||||
v_x /= length
|
||||
v_y /= length
|
||||
|
||||
target_pos_temp.append([
|
||||
cx + v_x * cfg.step_length,
|
||||
cy + v_y * cfg.step_length,
|
||||
cz
|
||||
])
|
||||
target_pos: dt.PosArray = target_pos_temp
|
||||
|
||||
def interpolate(t, p0, p1, p2):
|
||||
return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
|
||||
|
||||
# Smooth Step
|
||||
for tick in range(ticks + 1):
|
||||
loop_start = time.perf_counter()
|
||||
t = tick / ticks
|
||||
tick_pos_temp = []
|
||||
|
||||
for leg_id in range(6):
|
||||
if gv.leg_state[leg_id] == "drag":
|
||||
# lineares Gleiten in die Center-Position
|
||||
tick_pos_temp.append(current_pos_copy[leg_id] + (gv.center_points[leg_id] - current_pos_copy[leg_id]) * t)
|
||||
|
||||
elif gv.leg_state[leg_id] == "step":
|
||||
# Mittlerer Kontrollpunkt für Bezier-Kurve
|
||||
mid_point = [
|
||||
(current_pos_copy[leg_id][0] + target_pos[leg_id][0]) / 2,
|
||||
(current_pos_copy[leg_id][1] + target_pos[leg_id][1]) / 2,
|
||||
max(current_pos_copy[leg_id][2], target_pos[leg_id][2])
|
||||
+ cfg.step_height,
|
||||
]
|
||||
x = interpolate(
|
||||
t, current_pos_copy[leg_id][0], mid_point[0], target_pos[leg_id][0]
|
||||
)
|
||||
y = interpolate(
|
||||
t, current_pos_copy[leg_id][1], mid_point[1], target_pos[leg_id][1]
|
||||
)
|
||||
z = interpolate(
|
||||
t, current_pos_copy[leg_id][2], mid_point[2], target_pos[leg_id][2]
|
||||
)
|
||||
tick_pos_temp.append([x, y, z])
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos_temp)
|
||||
# IK mit letzter Winkelstellung als Startpunkt
|
||||
emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
print("Tick Position: \n", tick_pos)
|
||||
if (cfg.sim == True):
|
||||
gv.shared_sim.step()
|
||||
|
||||
# Timing anpassen, damit Loop gleichmäßig bleibt
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Endposition sichern
|
||||
#emitCalculation(kin.ikpyInverse(target_pos))
|
||||
#print("END TargetPos:\n", target_pos, "\n\n\n")
|
||||
|
||||
# Update current pos
|
||||
gv.current_pos = tick_pos
|
||||
gv.current_rad = kin.ikpyInverse(tick_pos)
|
||||
#emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
|
||||
# Gait-Zustand wechseln
|
||||
if gv.leg_state[0] == "step":
|
||||
gv.leg_state = np.array(["drag", "step", "drag", "step", "drag", "step"])
|
||||
else:
|
||||
gv.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
|
||||
|
||||
def walking_four(duration=cfg.standard_duration, tickpersec=cfg.standard_tickpersec, curve_height=cfg.step_height):
|
||||
# init Values
|
||||
ticks = duration * tickpersec
|
||||
tick_duration = 1 / tickpersec
|
||||
tick_pos: dt.PosArray # aktuelle XYZ-Positionen
|
||||
current_rad_copy: dt.RadArray = gv.current_rad
|
||||
current_pos_copy: dt.PosArray = gv.current_pos
|
||||
dirmov_copy = gv.vector_dirmov
|
||||
|
||||
# Zielpunkte für jede Beinspitze berechnen
|
||||
target_pos_temp = []
|
||||
for i in range(6):
|
||||
target_pos_temp.append(
|
||||
[
|
||||
gv.center_points[i][0] + dirmov_copy[0] * cfg.step_length,
|
||||
gv.center_points[i][1] + dirmov_copy[1] * cfg.step_length,
|
||||
gv.center_points[i][2]
|
||||
]
|
||||
)
|
||||
target_pos: dt.PosArray = target_pos_temp
|
||||
|
||||
def interpolate(t, p0, p1, p2):
|
||||
return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
|
||||
|
||||
# Smooth Step
|
||||
for tick in range(int(ticks) + 1):
|
||||
loop_start = time.perf_counter()
|
||||
t = tick / ticks
|
||||
tick_pos_temp = []
|
||||
|
||||
for leg_id in range(6):
|
||||
if gv.leg_state[leg_id] == "drag":
|
||||
# lineares Gleiten in die Center-Position
|
||||
tick_pos_temp.append(current_pos_copy[leg_id] + (gv.center_points[leg_id] - current_pos_copy[leg_id]) * t)
|
||||
|
||||
elif gv.leg_state[leg_id] == "step":
|
||||
# Mittlerer Kontrollpunkt für Bezier-Kurve
|
||||
mid_point = [
|
||||
(current_pos_copy[leg_id][0] + target_pos[leg_id][0]) / 2,
|
||||
(current_pos_copy[leg_id][1] + target_pos[leg_id][1]) / 2,
|
||||
max(current_pos_copy[leg_id][2], target_pos[leg_id][2])
|
||||
+ cfg.step_height,
|
||||
]
|
||||
x = interpolate(
|
||||
t, current_pos_copy[leg_id][0], mid_point[0], target_pos[leg_id][0]
|
||||
)
|
||||
y = interpolate(
|
||||
t, current_pos_copy[leg_id][1], mid_point[1], target_pos[leg_id][1]
|
||||
)
|
||||
z = interpolate(
|
||||
t, current_pos_copy[leg_id][2], mid_point[2], target_pos[leg_id][2]
|
||||
)
|
||||
tick_pos_temp.append([x, y, z])
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos_temp)
|
||||
# IK mit letzter Winkelstellung als Startpunkt
|
||||
emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
print("Tick Position: \n", tick_pos)
|
||||
|
||||
# Timing anpassen, damit Loop gleichmäßig bleibt
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Endposition sichern
|
||||
#emitCalculation(kin.ikpyInverse(target_pos))
|
||||
print("END TargetPos:\n", target_pos, "\n\n\n")
|
||||
|
||||
# Gait-Zustand wechseln
|
||||
if gv.leg_state[0] == "step":
|
||||
gv.leg_state = np.array(["drag", "drag", "step", "drag", "drag", "drag"])
|
||||
elif gv.leg_state[2] == "step":
|
||||
gv.leg_state = np.array(["drag", "drag", "drag", "step", "drag", "drag"])
|
||||
elif gv.leg_state[3] == "step":
|
||||
gv.leg_state = np.array(["drag", "drag", "drag", "drag", "drag", "step"])
|
||||
elif gv.leg_state[5] == "step":
|
||||
gv.leg_state = np.array(["step", "drag", "drag", "drag", "drag", "drag"])
|
||||
|
||||
def wave_emote(cycles=3, duration=1.5, tickpersec=20,
|
||||
height=40, amplitude=25, inward_offset=10):
|
||||
"""
|
||||
Greeting wave using front leg (leg 0)
|
||||
|
||||
Motion:
|
||||
- Z: lifts leg up
|
||||
- Y: waves left/right
|
||||
- X: slightly pulled inward to avoid IK limits
|
||||
"""
|
||||
|
||||
ticks = int(duration * tickpersec)
|
||||
tick_duration = 1 / tickpersec
|
||||
|
||||
# Safe copy
|
||||
base_pos = dt.PosArray(np.copy(gv.current_pos.data))
|
||||
|
||||
leg_id = 0 # front leg
|
||||
|
||||
for cycle in range(cycles):
|
||||
for tick in range(ticks):
|
||||
loop_start = time.perf_counter()
|
||||
|
||||
t = tick / ticks
|
||||
tick_pos = np.copy(base_pos.data)
|
||||
|
||||
cx, cy, cz = base_pos[leg_id]
|
||||
|
||||
# smooth outward-only wave
|
||||
y_wave = math.sin(4 * math.pi * t)
|
||||
y_offset = amplitude * (0.5 * (y_wave + 1))
|
||||
|
||||
# vertical lift
|
||||
z_offset = height * math.sin(math.pi * t)
|
||||
|
||||
# clamp sideways motion
|
||||
max_y_dev = 30
|
||||
new_y = cy + y_offset
|
||||
new_y = max(cy - max_y_dev, min(cy + max_y_dev, new_y))
|
||||
|
||||
tick_pos[leg_id] = [
|
||||
cx + 10, # small forward bias (IMPORTANT)
|
||||
new_y,
|
||||
cz + z_offset
|
||||
]
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos)
|
||||
|
||||
emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
|
||||
# Timing
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Return to base pose
|
||||
emitCalculation(kin.ikpyInverse(base_pos))
|
||||
gv.current_pos = base_pos
|
||||
|
||||
def laola_wave_emote(cycles=3, duration=1.5, tickpersec=cfg.standard_tickpersec, height=10, amplitude=30):
|
||||
ticks = int(duration * tickpersec)
|
||||
tick_duration = 1 / tickpersec
|
||||
|
||||
# Safe copy of base position
|
||||
base_pos = dt.PosArray(np.copy(gv.current_pos.data))
|
||||
|
||||
# Legs that perform the wave
|
||||
wave_legs = [1, 4]
|
||||
|
||||
for cycle in range(cycles):
|
||||
for tick in range(ticks):
|
||||
loop_start = time.perf_counter()
|
||||
t = tick / ticks # normalized 0 → 1
|
||||
tick_pos = np.copy(base_pos.data)
|
||||
|
||||
for leg_id in wave_legs:
|
||||
cx, cy, cz = base_pos[leg_id]
|
||||
|
||||
# Phase shift for Laola wave
|
||||
phase = 0 if leg_id == 1 else math.pi
|
||||
|
||||
# Sideways motion (Y) — reduced amplitude to avoid overextension
|
||||
y_offset = amplitude * math.sin(2 * math.pi * t + phase)
|
||||
|
||||
# Vertical motion (Z) — full up/down oscillation
|
||||
z_offset = height * math.sin(2 * math.pi * t + phase)
|
||||
|
||||
# Apply movement in YZ plane, X stays fixed
|
||||
tick_pos[leg_id] = [
|
||||
cx,
|
||||
cy + y_offset,
|
||||
cz + z_offset
|
||||
]
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos)
|
||||
|
||||
# IK + send command
|
||||
emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
|
||||
# Timing control
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Return to original stance
|
||||
emitCalculation(kin.ikpyInverse(base_pos))
|
||||
gv.current_pos = base_pos
|
||||
@@ -16,8 +16,10 @@ urdf_path = "JackBotUrdf.urdf"
|
||||
|
||||
# Dimensions in m
|
||||
#robot_height = -0.1
|
||||
step_height = 0.05
|
||||
step_length = 0.08
|
||||
step_height = 0.03
|
||||
step_length = 0.04
|
||||
translation_gain = 1.0
|
||||
rotation_gain = 2.0
|
||||
# Leglength in m
|
||||
# L1 = 0.068
|
||||
# L2 = 0.0602
|
||||
@@ -25,5 +27,5 @@ step_length = 0.08
|
||||
|
||||
# standard_tickpersec = # 20 Fluessige Bewegung
|
||||
standard_tickpersec:float = 25 # [ticks/s]
|
||||
standard_duration:float = 0.4 # [s],ip
|
||||
standard_duration:float = 0.8 # [s]
|
||||
standard_tickduration:float = 1 / standard_tickpersec
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
from ikpy.chain import Chain
|
||||
from ikpy.link import OriginLink, URDFLink
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import math
|
||||
import time
|
||||
|
||||
@@ -1,62 +1,60 @@
|
||||
from threading import Thread
|
||||
from multiprocessing import Process, Manager, Queue
|
||||
import time
|
||||
|
||||
# Selfmade Libraries
|
||||
import Controller as ctr
|
||||
import RobotState as rs
|
||||
|
||||
# Global Variables
|
||||
import GlobalVariables as gv
|
||||
|
||||
# main.py
|
||||
import time
|
||||
import threading
|
||||
|
||||
from DataTypes import ControlIntent
|
||||
from RobotState.RobotState import RobotContext
|
||||
from Controller import controller_loop
|
||||
from RobotState.idle import IdleState
|
||||
from RobotState.walking import WalkingState
|
||||
def robot_control():
|
||||
# Connection
|
||||
if gv.robotCommunication != None:
|
||||
gv.robotCommunication.start()
|
||||
|
||||
import DataTypes as dt
|
||||
# Start Position
|
||||
rs.initPos()
|
||||
time.sleep(1)
|
||||
|
||||
def main():
|
||||
intent = ControlIntent()
|
||||
lock = threading.Lock()
|
||||
ctx = RobotContext()
|
||||
############################## Main Loop ##############################
|
||||
tick = 0.05
|
||||
next_time = time.time()
|
||||
|
||||
ctx.center_points = dt.PosArray([
|
||||
[50, 50, -80],
|
||||
[50,-50, -80],
|
||||
[0, 70, -80],
|
||||
[0,-70, -80],
|
||||
[-50,50, -80],
|
||||
[-50,-50,-80]
|
||||
])
|
||||
ctx.current_pos = ctx.center_points.copy()
|
||||
while gv.control_pause == False:
|
||||
next_time += tick
|
||||
|
||||
controller_thread = threading.Thread(
|
||||
target=controller_loop,
|
||||
args=(intent, lock),
|
||||
daemon=True
|
||||
)
|
||||
controller_thread.start()
|
||||
if gv.emote == "wave":
|
||||
rs.initPos()
|
||||
time.sleep(0.3)
|
||||
rs.wave_emote()
|
||||
gv.emote = None
|
||||
gv.robot_state = "idle"
|
||||
continue
|
||||
|
||||
states = {
|
||||
"idle": IdleState(),
|
||||
"walking": WalkingState()
|
||||
}
|
||||
# Robot
|
||||
if gv.robot_state == "idle":
|
||||
rs.initPos()
|
||||
time.sleep(0.2)
|
||||
elif gv.robot_state == "walking":
|
||||
rs.walking()
|
||||
|
||||
current = states["idle"]
|
||||
current.on_enter(ctx)
|
||||
# Simulation
|
||||
if gv.shared_sim != None:
|
||||
gv.shared_sim.step()
|
||||
|
||||
last = time.perf_counter()
|
||||
sleep_time = next_time - time.time()
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
while not intent.quit:
|
||||
now = time.perf_counter()
|
||||
dt_s = now - last
|
||||
last = now
|
||||
|
||||
next_state = current.update(ctx, intent, dt_s)
|
||||
if next_state:
|
||||
current.on_exit(ctx)
|
||||
current = states[next_state]
|
||||
current.on_enter(ctx)
|
||||
|
||||
time.sleep(0.01)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
controller_queue = Queue()
|
||||
robot_queue = Queue()
|
||||
|
||||
controller_thread = Thread(target=ctr.controller)
|
||||
controller_thread.start()
|
||||
robot_control_thread = Thread(target=robot_control)
|
||||
robot_control_thread.start()
|
||||
Reference in New Issue
Block a user