diff --git a/include/vesc_comm.h b/include/vesc_comm.h new file mode 100644 index 0000000..9010eb5 --- /dev/null +++ b/include/vesc_comm.h @@ -0,0 +1,31 @@ +#ifndef VESC_COMM_H +#define VESC_COMM_H + +/* + * VESC Communication Interface — Dual FSESC 6.7 Pro (FW 6.6) + * + * Motor1: connected directly to USART6 (PC6=TX, PC7=RX) @ 115200 + * Motor2: bridged via COMM_FORWARD_CAN (cmd 34) to CAN ID 68 on motor1 + * + * Transport implementations: + * src/vesc_uart.c — USART6 UART (active) + * src/vesc_can.c — STM32 CAN + SN65HVD230 (phase 2, stub) + */ + +/* Initialise the VESC transport (USART6 GPIO + peripheral) */ +void vesc_comm_init(void); + +/* + * Send a current command to both VESCs. + * current_motor1/2 in Amperes (positive = forward, negative = reverse). + * Range limited by VESC motor configuration; typical max ±30 A for FSESC 6.7 Pro. + */ +void vesc_comm_send_current(float current_motor1, float current_motor2); + +/* + * Send a duty-cycle command to both VESCs. + * duty_motor1/2 in range -1.0 .. +1.0 (full reverse / full forward). + */ +void vesc_comm_send_duty(float duty_motor1, float duty_motor2); + +#endif /* VESC_COMM_H */ diff --git a/src/balance.c b/src/balance.c index d6382fb..fed71e1 100644 --- a/src/balance.c +++ b/src/balance.c @@ -1,8 +1,14 @@ #include "balance.h" #include "slope_estimator.h" +#include "vesc_comm.h" #include "config.h" #include +/* Scale motor_cmd (±MAX_SPEED_LIMIT) to VESC current in Amperes. + * 10 A peak is conservative for FSESC 6.7 Pro on a balance bot; + * adjust once motor winding and robot mass are characterised. */ +#define VESC_CURRENT_MAX_A 10.0f + void balance_init(balance_t *b) { b->state = BALANCE_DISARMED; b->pitch_deg = 0.0f; @@ -42,6 +48,7 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) { b->motor_cmd = 0; b->integral = 0.0f; slope_estimator_reset(&b->slope); + vesc_comm_send_current(0.0f, 0.0f); return; } } @@ -50,6 +57,7 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) { b->motor_cmd = 0; b->integral = 0.0f; b->prev_error = 0.0f; + vesc_comm_send_current(0.0f, 0.0f); return; } @@ -76,6 +84,14 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) { b->motor_cmd = (int16_t)output; b->prev_error = error; + + /* Drive both VESCs with the same current command. + * motor_cmd is clamped to ±MAX_SPEED_LIMIT (100); scale to Amperes. + * Both motors run in the same direction — forward/back balance only. + * Steering differential is handled separately by motor_driver_update(). */ + float current_a = (float)b->motor_cmd + * (VESC_CURRENT_MAX_A / (float)MAX_SPEED_LIMIT); + vesc_comm_send_current(current_a, current_a); } void balance_arm(balance_t *b) { diff --git a/src/vesc_can.c b/src/vesc_can.c new file mode 100644 index 0000000..ff6126a --- /dev/null +++ b/src/vesc_can.c @@ -0,0 +1,42 @@ +/* + * VESC CAN Transport — Phase 2 stub + * + * Hardware: STM32 bxCAN peripheral + SN65HVD230 CAN transceiver + * + * When implemented this will drive both VESCs directly over CAN + * using the native VESC CAN protocol (extended frame, 29-bit IDs). + * For now all functions are no-ops; UART transport (vesc_uart.c) is active. + * + * VESC CAN frame format (FW 6.6, extended IDs): + * arbitration ID = (command << 8) | vesc_can_id (29-bit extended) + * Example: COMM_SET_CURRENT (6) to CAN ID 1 → 0x00000601 + * + * TODO (phase 2): + * - Initialise bxCAN peripheral (500 kbps, extended frames) + * - Implement vesc_comm_send_current() using HAL_CAN_AddTxMessage() + * - Implement vesc_comm_send_duty() using HAL_CAN_AddTxMessage() + * - Add RX handler to read VESC status frames (current, temp, fault) + * - Gate on build flag (VESC_TRANSPORT=CAN) so UART and CAN builds + * are mutually exclusive at link time + */ + +#include "vesc_comm.h" + +void vesc_comm_init(void) +{ + /* TODO (phase 2): init bxCAN + SN65HVD230 transceiver GPIO */ +} + +void vesc_comm_send_current(float current_motor1, float current_motor2) +{ + /* TODO (phase 2): encode COMM_SET_CURRENT and transmit extended CAN frame */ + (void)current_motor1; + (void)current_motor2; +} + +void vesc_comm_send_duty(float duty_motor1, float duty_motor2) +{ + /* TODO (phase 2): encode COMM_SET_DUTY and transmit extended CAN frame */ + (void)duty_motor1; + (void)duty_motor2; +} diff --git a/src/vesc_uart.c b/src/vesc_uart.c new file mode 100644 index 0000000..24d34b3 --- /dev/null +++ b/src/vesc_uart.c @@ -0,0 +1,186 @@ +/* + * VESC UART Transport — Dual FSESC 6.7 Pro (FW 6.6) + * + * Hardware: MAMBA F722S USART6 (PC6=TX AF8, PC7=RX AF8) @ 115200 baud + * + * Motor1: directly attached to USART6 RX line + * Motor2: bridged via COMM_FORWARD_CAN (cmd 34, 0x22) to CAN ID 68 (0x44) + * Motor1 VESC relays the packet to Motor2 over its CAN bus + * + * VESC packet framing (FW 6.6 short form, payload ≤ 255 bytes): + * [0x02] [len:u8] [payload...] [crc_hi:u8] [crc_lo:u8] [0x03] + * + * Command IDs (VESC FW 6.6): + * COMM_SET_DUTY = 5 + * COMM_SET_CURRENT = 6 + * COMM_FORWARD_CAN = 34 (0x22) + * + * CRC: CRC16-CCITT, poly 0x1021, init 0x0000, no reflection, no final XOR. + * Computed over payload bytes only. + */ + +#include "vesc_comm.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include +#include + +/* ----------------------------------------------------------------------- + * VESC command IDs (FW 6.6) + * --------------------------------------------------------------------- */ +#define VESC_COMM_SET_DUTY 5u +#define VESC_COMM_SET_CURRENT 6u +#define VESC_COMM_FORWARD_CAN 34u /* 0x22 */ + +/* CAN ID of the second VESC on the shared CAN bus */ +#define VESC_CAN_ID_MOTOR2 68u /* 0x44 */ + +/* Maximum bytes in one VESC packet (start + len + 255 payload + crc*2 + stop) */ +#define VESC_PKT_MAX 261u + +/* ----------------------------------------------------------------------- + * USART6 peripheral handle + * --------------------------------------------------------------------- */ +static UART_HandleTypeDef s_uart; + +/* ----------------------------------------------------------------------- + * CRC16-CCITT (poly 0x1021, init 0x0000) + * --------------------------------------------------------------------- */ +static uint16_t crc16_ccitt(const uint8_t *data, uint8_t len) +{ + uint16_t crc = 0x0000u; + for (uint8_t i = 0; i < len; i++) { + crc ^= (uint16_t)data[i] << 8u; + for (uint8_t bit = 0; bit < 8u; bit++) { + if (crc & 0x8000u) { + crc = (uint16_t)((crc << 1u) ^ 0x1021u); + } else { + crc = (uint16_t)(crc << 1u); + } + } + } + return crc; +} + +/* ----------------------------------------------------------------------- + * Build and transmit a framed VESC packet + * payload: pointer to payload bytes (command ID + data) + * plen: payload length in bytes + * --------------------------------------------------------------------- */ +static void vesc_send_packet(const uint8_t *payload, uint8_t plen) +{ + /* Short packet framing: 0x02 + 1-byte length (≤255) */ + uint8_t buf[VESC_PKT_MAX]; + uint8_t idx = 0; + + uint16_t crc = crc16_ccitt(payload, plen); + + buf[idx++] = 0x02u; /* start byte */ + buf[idx++] = plen; /* payload length */ + memcpy(&buf[idx], payload, plen); + idx = (uint8_t)(idx + plen); + buf[idx++] = (uint8_t)(crc >> 8u); /* CRC high */ + buf[idx++] = (uint8_t)(crc & 0xFFu); /* CRC low */ + buf[idx++] = 0x03u; /* stop byte */ + + /* Blocking TX — balance loop runs at 1 kHz; 6-byte packet at 115200 + * takes ~0.52 ms. Keep plen small (≤9) to stay well under 1 ms. */ + HAL_UART_Transmit(&s_uart, buf, idx, 5u /* ms timeout */); +} + +/* ----------------------------------------------------------------------- + * Encode a signed 32-bit value big-endian into dst[0..3] + * --------------------------------------------------------------------- */ +static void encode_int32_be(uint8_t *dst, int32_t val) +{ + dst[0] = (uint8_t)((uint32_t)val >> 24u); + dst[1] = (uint8_t)((uint32_t)val >> 16u); + dst[2] = (uint8_t)((uint32_t)val >> 8u); + dst[3] = (uint8_t)((uint32_t)val ); +} + +/* ----------------------------------------------------------------------- + * Public API + * --------------------------------------------------------------------- */ + +void vesc_comm_init(void) +{ + /* Enable clocks */ + __HAL_RCC_USART6_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + + /* PC6 = USART6_TX (AF8), PC7 = USART6_RX (AF8) */ + GPIO_InitTypeDef gpio = {0}; + gpio.Pin = UART6_TX_PIN | UART6_RX_PIN; /* PC6 | PC7 */ + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + gpio.Alternate = GPIO_AF8_USART6; + HAL_GPIO_Init(UART6_TX_PORT, &gpio); /* both pins on GPIOC */ + + /* 115200 8N1, no flow control */ + s_uart.Instance = USART6; + s_uart.Init.BaudRate = 115200u; + s_uart.Init.WordLength = UART_WORDLENGTH_8B; + s_uart.Init.StopBits = UART_STOPBITS_1; + s_uart.Init.Parity = UART_PARITY_NONE; + s_uart.Init.Mode = UART_MODE_TX_RX; + s_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE; + s_uart.Init.OverSampling = UART_OVERSAMPLING_16; + HAL_UART_Init(&s_uart); +} + +void vesc_comm_send_current(float current_motor1, float current_motor2) +{ + /* Convert amps to milliamps (VESC FW 6.6 expects mA as int32) */ + int32_t ma1 = (int32_t)(current_motor1 * 1000.0f); + int32_t ma2 = (int32_t)(current_motor2 * 1000.0f); + + /* --- Motor 1: direct COMM_SET_CURRENT --- + * payload: [0x06, mA[3], mA[2], mA[1], mA[0]] (5 bytes) */ + { + uint8_t payload[5]; + payload[0] = VESC_COMM_SET_CURRENT; + encode_int32_be(&payload[1], ma1); + vesc_send_packet(payload, sizeof(payload)); + } + + /* --- Motor 2: COMM_FORWARD_CAN to CAN ID 68, then COMM_SET_CURRENT --- + * payload: [0x22, 0x44, 0x06, mA[3], mA[2], mA[1], mA[0]] (7 bytes) */ + { + uint8_t payload[7]; + payload[0] = VESC_COMM_FORWARD_CAN; /* 0x22 = 34 */ + payload[1] = VESC_CAN_ID_MOTOR2; /* 0x44 = 68 */ + payload[2] = VESC_COMM_SET_CURRENT; /* 0x06 */ + encode_int32_be(&payload[3], ma2); + vesc_send_packet(payload, sizeof(payload)); + } +} + +void vesc_comm_send_duty(float duty_motor1, float duty_motor2) +{ + /* VESC encodes duty as int32 scaled by 100000: + * 1.0 = 100000, -1.0 = -100000 */ + int32_t d1 = (int32_t)(duty_motor1 * 100000.0f); + int32_t d2 = (int32_t)(duty_motor2 * 100000.0f); + + /* --- Motor 1: direct COMM_SET_DUTY --- + * payload: [0x05, d[3], d[2], d[1], d[0]] (5 bytes) */ + { + uint8_t payload[5]; + payload[0] = VESC_COMM_SET_DUTY; + encode_int32_be(&payload[1], d1); + vesc_send_packet(payload, sizeof(payload)); + } + + /* --- Motor 2: COMM_FORWARD_CAN to CAN ID 68, then COMM_SET_DUTY --- + * payload: [0x22, 0x44, 0x05, d[3], d[2], d[1], d[0]] (7 bytes) */ + { + uint8_t payload[7]; + payload[0] = VESC_COMM_FORWARD_CAN; + payload[1] = VESC_CAN_ID_MOTOR2; + payload[2] = VESC_COMM_SET_DUTY; + encode_int32_be(&payload[3], d2); + vesc_send_packet(payload, sizeof(payload)); + } +}