update gravitycal logic
Description
Separating functions of the main loop and the control loop. The goal is to minimize what needs to happen during the control loop. Hopefully this is also cleaner and easier to understand.
Control loop is responsible for:
- updating trajectory
- calculating and sending torques
- capturing data if needed (only in certain states)
Main loop is responsible for:
- state transitions
- calculations
- handling key press input
TODO:
-
test run on robot
What to focus on
- put content here.
Reviewers
Resources
Issues
- Fixes #53