JackM323
1e660fcdb6
Add copy() to PosArray, DegArray and RadArray to allow easy shallow copies. Change RobotState imports in idle.py and walking.py to use explicit 'from RobotState.RobotState import RobotState' to avoid import issues. In main.py create a threading.Lock and pass it into the controller thread args to prepare for synchronized access in controller_loop.
63 lines
1.4 KiB
Python
63 lines
1.4 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()
|
|
lock = threading.Lock()
|
|
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, lock),
|
|
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()
|