JackM323
2b2125bfde
old code was uploaded in the initial upload changed everything with working programm and updated the readme
81 lines
1.9 KiB
Python
81 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
|
|
emote = None
|
|
display_text = ""
|
|
|
|
if cfg.sim:
|
|
shared_sim = Simulation()
|
|
else:
|
|
if cfg.arduinoConnection:
|
|
robotCommunication = ArduinoCommunication()
|
|
else:
|
|
robotCommunication = ESP32Communication()
|
|
|
|
vector_dirmov = [0, 0, 0] # Direction Movement [vx, vy, omega]
|
|
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()) |