62 lines
1.3 KiB
Python
62 lines
1.3 KiB
Python
# 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
|
|
|
|
import DataTypes as dt
|
|
|
|
def main():
|
|
intent = ControlIntent()
|
|
ctx = RobotContext()
|
|
|
|
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()
|
|
|
|
controller_thread = threading.Thread(
|
|
target=controller_loop,
|
|
args=(intent,),
|
|
daemon=True
|
|
)
|
|
controller_thread.start()
|
|
|
|
states = {
|
|
"idle": IdleState(),
|
|
"walking": WalkingState()
|
|
}
|
|
|
|
current = states["idle"]
|
|
current.on_enter(ctx)
|
|
|
|
last = time.perf_counter()
|
|
|
|
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()
|