78 lines
1.9 KiB
Python
78 lines
1.9 KiB
Python
import numpy as np
|
|
from ArduinoCommunication import ArduinoCommunication
|
|
from EspCommunication import ESP32Communication
|
|
from simulation import Simulation
|
|
import DataTypes as dt
|
|
import kinematics as kin
|
|
import config as cfg
|
|
|
|
# Globale Variablen
|
|
robotCommunication = None
|
|
shared_sim = None
|
|
if cfg.sim:
|
|
shared_sim = Simulation()
|
|
else:
|
|
if cfg.arduinoConnection:
|
|
robotCommunication = ArduinoCommunication()
|
|
else:
|
|
robotCommunication = ESP32Communication()
|
|
|
|
vector_dirmov = [0, 0] # Direction Movement
|
|
current_rad: dt.RadArray # Current Rad
|
|
current_pos: dt.PosArray # Current Position
|
|
|
|
# current_deg: dt.DegArray # Current Degrees
|
|
# legarray: dt.DegArray # Working Leg Array
|
|
# target_pos: dt.PosArray # Target Position
|
|
|
|
robot_state = "idle" # Current State
|
|
# Init for 6 Legs
|
|
#leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
|
|
# Init for 4 Legs
|
|
leg_state = np.array(["step", "drag", "drag", "drag", "drag", "drag"])
|
|
control_pause: bool = False
|
|
|
|
# Initilize mit Start Position
|
|
#init_deg: dt.DegArray = dt.DegArray(
|
|
# [
|
|
# [90, 30, 115],
|
|
# [90, 30, 115],
|
|
# [90, 30, 115],
|
|
# [90, 150, 65],
|
|
# [90, 150, 65],
|
|
# [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],
|
|
# ]
|
|
#)
|
|
|
|
init90_deg: dt.DegArray = dt.DegArray(
|
|
[
|
|
[90, 90, 90],
|
|
[90, 90, 90],
|
|
[90, 90, 90],
|
|
[90, 90, 90],
|
|
[90, 90, 90],
|
|
[90, 90, 90],
|
|
]
|
|
)
|
|
|
|
center_points: dt.PosArray = kin.ikpyForward(init_deg.to_rad()) |