Motion Repository
Motion High-Level Code Overview
Upon running setup in main.cpp, the following happens:
A serial monitor is set up
sparky.setup() is called
In sparky.setup(), serial connections are created for all 6 of the ODrives. The IMU is then initialized, and all ODrive connections are made
The majority of functionality occurs in sparky.update(), which is called from main.loop(). The following happen:
The current time is fetched. If it has been < 10ms since the last update, sparky does not update
Ensures controller and ODrives are connected
Checks current mode, then calls the respective method in kinematics.cpp (walk, pushUp, or dance)
Checks the IMU for current roll/pitch, and stores them in class variables
Checks for updates to controller state, and stores button values in class variables
Checks for ODrive errors
Motion Low-Level Code Overview
Axis.cpp
Contains methods for initializing axis, fetching coordinates, and resetting ODrives. Contains method for sending position to ODrives.
Axis::Axis(Odrive& _odrive, int _id)
Sets ODrive and id of axis to specified parameters (there are 2 Odrives per axis)
Checks for errors then tries to reset them by calling
Updates zero position offsets
Sets the axis to closed loop by calling ``--Axis::setClosedLoop()``
Calls Axis::fetchState() and logs the state of the axis
Sends axis config to ODrives on each axis
Checks if ODrive response has a nonzero length
If the response length is nonzero, sending the axis config failed
ODrive response is logged if setting the axis config failed
Gets the position estimate of the ODrive, converts it to a float, and stores it in offset
- Attempts to reset the ODrive by setting controller, encoder, motor, and general error flags to 0
- Sends “sc:\n” to the ODrive, clearing communications
- Sends data to the ODrive, requesting closed loop state
- Returns an integer state of the ODrive's control mode
- Checks if provided position is different from target position
- If positions are different, sets target position to pos, then sends targetPos + offset to the ODrive
- Returns the current offset from starting position
- Returns ODrive error as an integer
- Takes a float in, multiplies it by static GLOBAL_SPEED variable, then logs the calculated speed
- Sets speed of each axis of the ODrive to calculated speed
kinematics.cpp
Contains method for inverse kinematics calculations, which takes position (x, y, z, roll, pitch, yaw), and translates them to relative ODrive movements for each joint. Contains methods for walking, pushups, and dancing.
Kinematics::translate(int leg, float xIn, float yIn, float zIn, float roll, float pitch, float yawIn)
Inverse kinematics calculation that takes in the requested position of a leg, then outputs necessary hip/shoulder/knee angles
Figure it out if you really need to. You probably don’t.
Kinematics::walk(int RFB, int RLR, int LT, float IMUpitch, float IMUroll)
(Implementation handles walking logic)
Kinematics::pushUp(bool cross_press, bool triangle_press, float IMUpitch, float IMUroll)
Checks if cross or triangle are pressed. If cross is pressed, all legs go down. If triangle is pressed, only the back legs go down. If both are pressed, all legs go down.
Corrects for roll and pitch based on IMU data, scaled by 0.3
Kinematics::dance(bool up, bool down, bool left, bool right, float IMUpitch, float IMUroll)
Loops through step 1-4
Based on the given D-pad input, chooses one of the following
leg.cpp
Contains helper method for movement of all joints in a leg.
Leg::move(JointAngles angles)
Moves the leg by updating all three joint motors (hip, shoulder, knee) to their respective target angles.
Calls hip.move(), shoulder.move(), and knee.move() with the given joint angles.
log.cpp
Contains helper method for logging errors/information while DEBUG flag is set. Running in debug mode slows serial communications for the rest of the robot.
Log(const char* format, ...)
- Variadic function that prints formatted messages to the serial monitor.
- Executes only when the DEBUG flag is enabled to prevent slowdowns during normal operation.
- Uses va_list to handle variable arguments and SerialMon.vprintf() for formatted output.
main.cpp
Initializes serial communications of the robot, then calls setup method in sparky.cpp. Contains loop code, which calls update method in sparky.cpp.
- Initializes the serial monitor at 115200 baud.
- Prints crash reports if available.
- Logs initialization message.
- Calls sparky.setup() to initialize all components.
- Main loop repeatedly calls sparky.update() to process robot logic and motion control.
odrive.cpp
Contains methods for initializing and connecting to an ODrive. Initializes axis for each ODrive. Defines parameters for ODrive setup.
Sends configuration commands to the ODrive hardware from a predefined array.
Logs any failed configuration responses.
Initializes both axis0 and axis1.
Sends “ss” to save the ODrive configuration.
Marks the ODrive as initialized.
- Establishes serial communication with the ODrive.
- Flushes input/output buffers to clear any previous communication.
- Reads firmware version and serial number to verify a valid connection.
- If connection succeeds, reconstructs and stores the serial number.
- Logs connection status and initializes the ODrive.
- Sets timeouts and flags _connected to true.
sparky.cpp
Contains setup method, which initializes all serial communications on the robot. Contains update method, which is called in the main.cpp loop. Update checks tick time so that the robot is updated every 10ms, then checks which mode (walk, pushup, dance) the robot is in. IMU data is sent to the respective kinematics function. The IMU is then updated with current values. Updates to internal states are then made (button presses, operation mode).
- Constructor initializing six ODrive objects, four Leg objects, and a Kinematics instance.
- Maps legs and axes to specific ODrives and serial interfaces.
- Initializes serial communication for USB and six ODrives.
- Initializes and configures the MPU6050 IMU with acceleration and gyro ranges.
- Connects to each ODrive by calling od.connect().
- Logs setup status.
- Called every TICK_MS (10ms).
- Updates IMU readings, calculates pitch and roll using a complementary filter.
- Handles remote control communication and updates robot motion mode.
- Commands kinematics functions (walk, push-up, dance) based on mode.
- Manages ODrive and axis error checking, reconnections, and resets as needed.
- Handles MotionProtocol messages received via SerialUSB.
- Sends ODrive connection and error status back through USB using FlatBuffers.
- Updates the motion speed scaling factor across all ODrives.
- Calls axis0.setSpeed() and axis1.setSpeed() for every ODrive.
utils.cpp
Contains methods for trimming stick positions (controller deadzone) and filtering motions.
- Applies a deadzone threshold (±10) to controller stick input.
- Returns 0 if the stick value is within threshold; otherwise returns the raw value.
filter(float prevValue, float currentValue, int filter)
Applies a simple weighted moving average filter.
Smooths motion input by blending previous and current values based on filter weight.
Platform Repository
Platform High-Level Code Overview
Message.py
Automatically generated FlatBuffers module defining the Message object for robot communication. Handles serialization and deserialization of messages containing type information and optional Remote data.
Message.pyi
Type stub providing signatures and type hints for the generated Message FlatBuffers class and helper functions.
MessageType.py
Enumerates message types for FlatBuffers communication schema. Defines integer constants representing message categories.
MessageType.pyi
Type stub defining type hints and integer constants for message types.
ODriveStatus.py
Automatically generated FlatBuffers module defining the structure for ODrive connection and error reporting across six motor controllers. Contains helper functions for FlatBuffers serialization.
ODriveStatus.pyi
Type stub defining methods and type hints for the ODriveStatus class and its FlatBuffers builder helper functions.
Remote.py
Automatically generated FlatBuffers module defining the Remote class used for representing remote control input state (buttons, sticks, and triggers). Includes methods for reading, constructing, and serializing controller data fields.
Remote.pyi
Type stub defining method signatures, field types, and FlatBuffers builder helpers for the Remote object.
Platform Low-Level Code Overview
Message.py
Contains generated FlatBuffers methods for reading, writing, and building Message objects.
Returns a new Message instance from a FlatBuffer, initializing its internal table at the given offset.
Deprecated alias of GetRootAs kept for backward compatibility.
Initializes the FlatBuffers table reference for the Message.
Returns the message type as an integer enum value.
Returns a Remote object if present; otherwise returns None.
Begins FlatBuffers object construction for a Message.
Adds the type field to the FlatBuffer.
Adds a Remote object reference to the FlatBuffer.
Finalizes and returns the Message object offset.
Message.pyi
Contains type-stub equivalents for Message methods and FlatBuffers builder helpers.
Type-annotated version of GetRootAs returning a Message instance.
Type-hinted initialization of the internal Message buffer.
Returns a literal value from MessageType.
Returns a typed Remote object or None.
Type-annotated helper functions used to build Message objects.
MessageType.py
Contains integer constants for message categories.
Integer constant 0 for unknown message type.
Integer constant 1 for remote-control message type.
MessageType.pyi
Contains typed constants matching the generated MessageType enum.
Type stub constant for unknown message type.
Type stub constant for remote message type.
ODriveStatus.py
Contains generated FlatBuffers methods for ODrive connectivity and error status.
Initializes an ODriveStatus object from FlatBuffer data.
Deprecated alias for backward compatibility.
Initializes the internal FlatBuffers table reference.
Returns boolean connection flags for all six ODrives.
Returns integer error codes for all twelve ODrive axes.
Begins FlatBuffers object creation for ODriveStatus.
Adds one ODrive connection field (X = 0..5).
Adds one axis error code field.
Finalizes and returns the ODriveStatus object offset.
ODriveStatus.pyi
Contains type-stub definitions for ODriveStatus accessors and builder helpers.
Type-stub versions for object construction and initialization.
Typed boolean accessors for ODrive connection flags.
Typed integer accessors for axis error codes.
Type-annotated FlatBuffers builder helpers.
Remote.py
Contains generated FlatBuffers methods for remote state fields and builders.
Initializes a Remote object from FlatBuffer data.
Deprecated alias kept for backward compatibility.
Initializes the FlatBuffers table reference for Remote.
Enabled, Mode, stick and trigger values, D-pad flags, and button flags return the current remote state values.
Begins FlatBuffers object creation for Remote.
Adds remote enabled and mode fields to the object.
Adds right and left analog inputs to the object.
Adds D-pad and face-button states to the object.
Finalizes and returns the Remote object offset.
Remote.pyi
Contains type-stub definitions for Remote accessors and FlatBuffers helper functions.
Typed construction and initialization methods.
Typed accessors for mode, analog values, D-pad states, and button states.
Type-stub helpers for building a typed Remote object.
Platform/Robot Code
High-Level Code Overview
audio_manager.py
Handles audio playback for the robot, including mixer initialization, sound/song loading, volume setup, and mode-specific playback.
controller.py
Manages DualShock/PS4 controller input via evdev, updates LED state, and streams input state to the robot with asynchronous battery polling.
mode.py
Defines the base Mode class used by robot operating modes and provides coroutine lifecycle handling for start, run, and stop.
motion.py
Handles serial communication with the Teensy motion controller, including FlatBuffers message sending, status reception, and reconnect logic.
sparky.py
Main orchestration module that initializes controller, motion, mode, and UI subsystems and manages runtime tasks.
ui.py
PyQt6 user interface module defining MainWindow controls for enable/disable, mode selection, and async integration with the Sparky backend.
Low-Level Code Overview
audio_manager.py
Contains audio playback setup and utility methods.
Initializes the pygame mixer, loads sound assets, and sets per-sound volume defaults.
Plays a sound effect by key when audio is enabled.
Maps mode IDs to mode-switch sounds and plays the mapped clip.
Plays a random or specific song based on the input argument.
controller.py
Contains controller device discovery, LED control, and asynchronous input processing.
Initializes LED file paths for red, green, and blue channels.
Writes LED brightness values asynchronously.
Sets RGB LED output values for controller status feedback.
Initializes controller state, LED control, and readiness flags.
Finds and returns the PS4 controller input device path.
Updates buttons, sticks, and trigger state from evdev events.
Asynchronously consumes controller events and updates robot input state.
Polls controller battery level on a periodic async interval.
Placeholder for stopping controller event processing.
Returns a formatted summary string of current controller state.
mode.py
Contains the base mode lifecycle interface for robot behavior modules.
Stores references to the robot and motion subsystem.
Coroutine placeholder implemented by concrete mode classes.
Executes the mode coroutine and handles cancellation behavior.
Creates an event loop and runs the mode coroutine continuously.
Cancels running mode tasks and stops the mode loop.
motion.py
Contains serial transport logic between platform software and Teensy motion firmware.
Initializes serial state and remote-control field defaults.
Opens serial communication to the Teensy motion controller.
Repeatedly attempts reconnection when serial communication fails.
Scans ports and returns the Teensy serial device path.
Updates cached controller values to be transmitted to firmware.
Resets all control values to neutral state.
Builds and sends FlatBuffers messages, receives ODrive status responses, and handles errors/reconnects in a continuous loop.
sparky.py
Contains top-level runtime orchestration for robot enable state, tasks, and mode transitions.
Initializes core runtime fields, default mode, and executor resources.
Provides async context manager lifecycle hooks and cleanup behavior.
Enables a selected mode or disables active motion and mode tasks.
Runs LED heartbeat logic reflecting enable and health state.
Forwards movement commands to the motion subsystem.
Starts UI, controller, motion, and heartbeat tasks and runs the app loop.
Cancels tasks, resets LED state, and shuts down executor threads.
ui.py
Contains the PyQt6 main window and asynchronous UI handlers for robot control.
Loads platform UI layout, initializes controls, and connects async button handlers.
Enables or disables mode selection controls.
Calls robot enable/disable flow and updates UI state accordingly.
Handles enable-button action and starts the selected mode.
Handles disable-button action and safely disables the robot.
Enforces mode button mutual exclusivity.
Creates the Qt app and event loop bridge, then launches the main window.
Handles app close and triggers graceful robot shutdown.