Firmware Documentation

Cart-Pole v1.0 Firmware

Main application entry point and firmware for CartPole controller.

Initializes CPU power management, GPIOs, UART, and CAN interfaces. Creates FreeRTOS tasks for motor control, safety monitoring, UART command handling, sensor data reporting, and hardware tests.

Defines

CTRL_TSK_PRIO: < Motor control task priority UART and sensor info task priority

SERIAL_TSK_PRIO: Safety monitor task priority

SAVER_TSK_PRIO: Hardware test task priority

TEST_TASK_PRIO: Default interrupt flag

ESP_INTR_FLAG_DEFAULT: CAN TX GPIO

TX_GPIO_CAN: CAN RX GPIO

RX_GPIO_CAN: Linear encoder channel A

ENC_LINEAR_GPIO_1: Linear encoder channel B

ENC_LINEAR_GPIO_2: Angular encoder channel A

ENC_ANGULAR_GPIO_A: Angular encoder channel B

ENC_ANGULAR_GPIO_B: Angular encoder index signal

ENC_ANGULAR_GPIO_C: User button GPIO

BTN_GPIO

GPIO_PIN_MASK(PIN): Angular step [rad]

ANGLE_STEP_SIZE: Linear encoder max count

MAX_ECNODER_DATA: Safe region threshold

SAFE_REGION: Initial torque for motor initialization

INIT_TORQUE: Reinitialization torque for motor

REINIT_TORQUE: UART TX pin

TXD_PIN: UART RX pin

RXD_PIN: UART port

UART_PORT: UART buffer size

BUF_SIZE: UART baud rate

BAUD_RATE: UART read delay [ms]

SERIAL_MS_DELAY: Error log tag

ERROR_TAG: Main log tag

MAIN_TAG: Debug log tag

DEBUG_TAG: Motor sending log tag

SEND_TAG: Motor receiving log tag

RECIEVE_TAG: PC receiving log tag

READ_TAG: PC sending log tag

WRITE_TAG: Stop command code

MOTOR_STOP_COMMAND: Ready state command code

READY_STATE_COMMAND: Info request command code

MOTOR_INFO_COMMMAND: Reset command code

RESET_COMMAND

Functions

static void motor_operate_by_torque(uint32_t torque)

static void motor_operate_by_speed(uint32_t speed)

static void motor_operate_by_position(uint32_t position)

static void uart_ready_state(char *data)

UART handler for READY state commands.

Parses incoming text commands such as START_OPER, RESTART, HELP, and routes to appropriate actions or state transitions.

Parameters:

data – Null-terminated command string.

static void uart_operational_state(char *data)

static void uart_set_sender_delay(char *data)

static void uart_set_motor_drive_mode(char *data)

static void uart_test_button(char *data)

static void uart_test_encoder(char *data)

static void uart_test_angle(char *data)

static void sensor_timer_callback()

static bool twai_request(const twai_message_t *_tx_message, twai_message_t *_rx_message)

Sends TWAI message and verifies response ID.

Parameters:

_tx_message – [in] Transmit message pointer.
_rx_message – [out] Receive buffer pointer.

Returns:

true if response ID matches, false otherwise.

static bool twai_request_wo_id_check(const twai_message_t *_tx_message, twai_message_t *_rx_message)

Sends TWAI message without ID check.

Parameters:

_tx_message – [in] Transmit message pointer.
_rx_message – [out] Receive buffer pointer.

Returns:

true if any response received, false otherwise.

static void twai_output(char *tag, twai_message_t *message)

Logs TWAI message content.

Parameters:

tag – [in] Log tag string.
message – [in] Message pointer to log.

static void find_my_id(): Discovers motor CAN ID by scanning range.

static bool motor_request(uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7)

Sends motor command over CAN and waits for optional reply.

Constructs a CAN frame with the provided data bytes and transmits it. If DEBUG is disabled, waits for a confirmation response via twai_request().

Parameters:

dX – Data bytes to include in the frame. (0-7)

Returns:

true if transmission (and optional reply) succeeded, false otherwise.

static void motor_request_wo_reply(uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7)

Sends motor command over CAN without waiting for a reply.

Constructs and transmits a CAN frame with the provided data bytes, then ignores any response.

Parameters:

dX – Data bytes to include in the frame. (0-7)

static void motor_request_stop()

Issues immediate stop command to motor.

Sends two sequential stop frames to ensure motor halts.

static void motor_request_system_reset()

Triggers a system reset command on the motor.

Sends reset frame and delays for 1 second to allow reboot.

static void motor_request_info()

Requests telemetry information from the motor.

Sends info request frame to retrieve voltage, temperature, and error codes.

static void motor_request_shutdown(): Commands motor to enter shutdown state.

static void motor_request_torque(int16_t tau)

Applies signed torque command to the motor.

Parameters:

tau – Desired torque value (signed).

static void motor_request_speed(int32_t vel)

Sets motor target velocity.

Multiplies velocity by 100 (unit scaling) and sends command frame.

Parameters:

vel – Desired speed in encoder units/sec.

static bool motor_request_is_connected()

Checks if motor responds to info request.

Returns:: true if motor acknowledges, false otherwise.

static void motor_control_task(void *arg)

FreeRTOS task for motor control operations.

Performs a system reset and info request on the motor, then enters a loop waiting for torque commands via the move_sem semaphore.

Parameters:

arg – Unused parameter.

static void motor_init_function()

Initializes motor by centering encoder and establishing zero position.

Applies torque in opposite directions and waits for user confirmation via btn_sem. Monitors encoder_position to detect center; enforces timeout of 3 seconds. Signals completion through init_done_sem if first initialization.

static void motor_self_saver_task(void *arg)

FreeRTOS safety monitor task for motor encoder limits.

Waits for initial motor setup completion, then continuously checks the encoder_position against SAFE_REGION boundaries. If out-of-bounds, logs danger, reinitializes the motor, and restores safe state.

Parameters:

arg – Unused parameter.

void uart_init_setup()

Configures UART parameters and installs driver.

Sets UART baud rate, data bits, parity, stop bits, and flow control, then configures TX/RX pins and installs the UART driver with buffer.

void uart_event_task(void *pvParameters)

FreeRTOS task for UART event processing.

Continuously reads bytes from UART into a buffer and invokes the current state handler function pointer with the received data.

Parameters:

pvParameters – Unused task parameter.

void rtoo()

Switches UART handler to operational state.

Creates and starts a periodic timer to trigger sensor info reporting.

void otor()

Switches UART handler to ready state.

Stops and deletes the sensor info reporting timer if active.

void uart_get_mode(char *data)

UART handler to set control mode.

Parses TORQUE, SPEED, or POSITION strings and logs selected mode. Then returns to ready state.

Parameters:

data – Null-terminated mode string.

void uart_get_delay(char *data)

UART handler to configure sensor info delay.

Converts ASCII data to integer and updates delay for sensor info timer.

Parameters:

data – Null-terminated numeric string.

void uart_oper_state(char *data)

UART handler for OPERATIONAL state control packets.

Parses numeric packets for torque commands or special commands (stop, ready, info, reset), with safety checks on encoder limits.

Parameters:

data – Null-terminated ASCII packet string.

void sensor_info_sender_task()

FreeRTOS task to publish sensor information.

Waits on info_please_sem, computes linear and angular velocities, and prints position, velocities, and angle if safe.

void sensor_info_sender_timer()

Timer callback to trigger sensor info task.

Gives the info_please_sem semaphore to schedule data reporting.

static void IRAM_ATTR enc_linear_isr_handler (void *arg)

Interrupt Service Routine for linear encoder channel.

Called on any edge of the linear encoder’s first channel. Compares states of both encoder channels to determine movement direction and increments or decrements the linear encoder position counter.

Parameters:

arg – Unused argument.

static void IRAM_ATTR enc_angular_change_isr_handler (void *arg)

Interrupt Service Routine for angular encoder channel B.

Called on the rising edge of angular encoder channel B. Updates the angular position by a fixed step size depending on the state of channel A to determine direction.

Parameters:

arg – Unused argument.

static void IRAM_ATTR enc_angular_zero_isr_handler (void *arg)

Interrupt Service Routine for angular encoder zero marker.

Called when the zero position marker is detected on channel C. Resets the angular encoder position to zero for calibration.

Parameters:

arg – Unused argument.

static void IRAM_ATTR btn_isr_handler (void *arg)

Interrupt Service Routine for button press.

If initialization is active, gives semaphore from ISR context to signal that the button was pressed.

Parameters:

arg – Unused argument.

static void gpio_init_setup()

Initialize GPIO pins and configure interrupts.

Configures all required GPIOs for the linear and angular encoders and the user button. Sets pin modes, pull-downs, and interrupt types. Installs ISR service and attaches handlers to GPIO pins.

void cpu_setup()

Configure CPU power management settings.

Sets the minimum and maximum CPU clock frequencies to maximum value, and disables light sleep to ensure full performance during operation.

void sensor_tests()

Runs a full sensor and button test sequence.

Runs in a separate FreeRTOS task. Guides the user through manual tests for the button, linear encoder, and angular encoder. Waits for user confirmation via UART after each step and logs results. Loops endlessly.

void uart_test_btn_state(char *data)

Handles UART input to verify button test result.

Parses user input string to determine whether the button works. Logs result, updates test status, and advances test sequence.

Parameters:

data – Pointer to user input string (yes/no).

void uart_test_encoder_state(char *data)

Handles UART input to verify linear encoder test result.

Parses user input string to determine whether the linear encoder works. Logs result, updates test status, and advances test sequence.

Parameters:

data – Pointer to user input string (yes/no).

void uart_test_angle_state(char *data)

Handles UART input to verify angular encoder test result.

Parses user input string to determine whether the angular encoder works. Logs result, updates test status, and finishes test sequence.

Parameters:

data – Pointer to user input string (yes/no).

void app_main(void)

Main entry point for the application.

Initializes CPU, GPIO, UART, CAN bus, tasks, and semaphores. Starts all system tasks and waits for completion. After tasks finish, cleans up all resources and restarts the device.

Variables

static const twai_timing_config_t can_timing_config = TWAI_TIMING_CONFIG_1MBITS()

static const twai_filter_config_t can_filter_config = TWAI_FILTER_CONFIG_ACCEPT_ALL()

static const twai_general_config_t can_general_config = TWAI_GENERAL_CONFIG_DEFAULT(TX_CAN_GPIO, RX_CAN_GPIO, TWAI_MODE_NORMAL)

static SemaphoreHandle_t restart_command_semaphore

static SemaphoreHandle_t initialization_start_semaphore

static SemaphoreHandle_t initialization_done_semaphore

static SemaphoreHandle_t button_press_semaphore

static SemaphoreHandle_t motor_command_semaphore

static SemaphoreHandle_t sensor_data_request_semaphore

static SemaphoreHandle_t test_sync_semaphore

static uint32_t motor_can_id = 0x141

static int64_t uart_recieved_packet

static void (*motor_operate)(uint32_t) = motor_operate_by_torque 

static volatile double current_encoder_position

static volatile double current_encoder_angle = 179.93

static double previous_encoder_position

static double previous_encoder_angle

static bool initialization_in_progress = false

static bool not_initiazatied_yet = true

static bool system_in_safe_state = true

static void (*uart_state)(char*) = uart_ready_state 

static uint16_t sensor_timer_delay = UART_DEFAULT_SEND_DELAY

static TimerHandle_t sensor_timer_handle

static TaskHandle_t motor_control_task_handle

static TaskHandle_t uart_event_task_handle

static TaskHandle_t safety_monitor_task_handle

static TaskHandle_t sensor_info_sender_task_handle

static TaskHandle_t hardware_test_task_handle

static bool hardware_tests_passed = false