/systemCheck

Question

KevinGuoUniAGI

I found almost /systemCheck programs two years ago is not working with including the latest "/src programs". I will rebuild all UnitTest case. If our project team had already build the new /systemCheck programs, please update.

 · Answer

Rebuild Gait.ino (Working on my building OpenExo instance in HongKong)

#include <FlexCAN_T4.h>

#include "Arduino.h"

// =================================================================

// Configuration / 配置

// =================================================================

// --- Motor CAN IDs / 电机CAN ID定义 ---

#define LEFT_HIP_ID 65

#define RIGHT_HIP_ID 33

// --- Pin Definitions / 管脚定义 ---

const int ENABLE_LEFT_PIN[] = {28, 29};

const int ENABLE_RIGHT_PIN[] = {8, 7};

const int MOTOR_STOP_PIN = 9;

// --- Motion Control Mode Parameters (from documentation) / 运控模式参数(来自文档) ---

// Note: Position values are in RADIANS, not degrees.

// 注意：位置单位是弧度，不是角度。

const float P_MIN = -12.5f; // Min position (rad)

const float P_MAX = 12.5f; // Max position (rad)

const float V_MIN = -30.0f; // Min velocity (rad/s)

const float V_MAX = 30.0f; // Max velocity (rad/s)

const float KP_MIN = 0.0f; // Min Kp

const float KP_MAX = 500.0f; // Max Kp

const float KD_MIN = 0.0f; // Min Kd

const float KD_MAX = 5.0f; // Max Kd

const float T_MIN = -18.0f; // Min torque (N-m)

const float T_MAX = 18.0f; // Max torque (N-m)

// --- Control Parameters / 控制参数 ---

const float KP = 80.0f; // Proportional gain / 比例增益

const float KD = 1.5f; // Derivative gain / 微分增益

// =================================================================

// Global Variables / 全局变量

// =================================================================

FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> Can0;

// Actual motor states updated by CAN feedback

// 由CAN反馈更新的电机实际状态

volatile float leftActualPos_rad = 0.0f;

volatile float leftActualVel_rad = 0.0f;

volatile float leftActualTorque = 0.0f;

volatile float rightActualPos_rad = 0.0f;

volatile float rightActualVel_rad = 0.0f;

volatile float rightActualTorque = 0.0f;

bool emergencyStop = false;

// =================================================================

// Utility Functions / 辅助函数

// =================================================================

// --- Wait for user input to proceed / 等待用户输入以继续 ---

void waitForKeyPress() {

Serial.println("\n请按空格键继续下一步...");

while (true) {

if (Serial.available() > 0) {

char c = Serial.read();

if (c == ' ') {

Serial.println("继续执行...");

// Clear buffer

while(Serial.available() > 0) Serial.read();

break;

}

delay(50);

}

// --- Convert float to unsigned int for CAN packing / 浮点数转为整数用于CAN打包 ---

unsigned int float_to_uint(float x, float x_min, float x_max, int bits) {

float span = x_max - x_min;

float offset = x_min;

// Clamp the input value within the specified range

if (x > x_max) x = x_max;

else if (x < x_min) x = x_min;

// Perform the conversion

return (unsigned int)((x - offset) * ((float)((1 << bits) -1)) / span);

}

// --- Convert unsigned int from CAN to float / CAN数据中的整数转为浮点数 ---

float uint_to_float(int x_int, float x_min, float x_max, int bits) {

float span = x_max - x_min;

float offset = x_min;

// Perform the conversion

return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;

}

// =================================================================

// CAN Communication Functions (Motion Control Mode)

// CAN通信函数 (运控模式)

// =================================================================

/**

* @brief Sends a generic command with an 8-byte payload using a standard CAN frame.

* @param id The motor's CAN ID.

* @param data An 8-byte array containing the command payload.

*/

void sendMotionCommand(uint8_t id, const uint8_t* data) {

CAN_message_t msg;

msg.id = id;

msg.flags.extended = 0; // Use Standard Frame for Motion Control Mode

msg.len = 8;

memcpy(msg.buf, data, 8);

Can0.write(msg);

}

// --- Enter Motor Control Mode / 进入电机控制模式 ---

void enterControlMode(uint8_t id) {

const uint8_t cmd[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};

sendMotionCommand(id, cmd);

Serial.printf("电机ID %u: 已发送 [进入控制模式] 命令\n", id);

}

// --- Exit Motor Control Mode / 退出电机控制模式 ---

void exitControlMode(uint8_t id) {

const uint8_t cmd[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};

sendMotionCommand(id, cmd);

Serial.printf("电机ID %u: 已发送 [退出控制模式] 命令\n", id);

}

// --- Set Current Position as Zero / 设置当前位置为零点 ---

void setOrigin(uint8_t id) {

const uint8_t cmd[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE};

sendMotionCommand(id, cmd);

Serial.printf("电机ID %u: 已发送 [设置原点] 命令\n", id);

}

/**

* @brief Packs motor command data and sends it over CAN.

* @param id Motor CAN ID.

* @param p_des Desired position (radians).

* @param v_des Desired velocity (rad/s).

* @param kp Kp gain.

* @param kd Kd gain.

* @param t_ff Feed-forward torque (N-m).

*/

void packAndSendMotionCommand(uint8_t id, float p_des_rad, float v_des_rad, float kp, float kd, float t_ff) {

if (emergencyStop) return;

// Convert floats to unsigned ints

unsigned int p_int = float_to_uint(p_des_rad, P_MIN, P_MAX, 16);

unsigned int v_int = float_to_uint(v_des_rad, V_MIN, V_MAX, 12);

unsigned int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);

unsigned int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);

unsigned int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);

// Pack ints into the CAN buffer

uint8_t data[8];

data[0] = p_int >> 8;

data[1] = p_int & 0xFF;

data[2] = v_int >> 4;

data[3] = ((v_int & 0xF) << 4) | (kp_int >> 8);

data[4] = kp_int & 0xFF;

data[5] = kd_int >> 4;

data[6] = ((kd_int & 0xF) << 4) | (t_int >> 8);

data[7] = t_int & 0xFF;

sendMotionCommand(id, data);

}

// --- Unpack motor reply from CAN message / 从CAN报文中解析电机反馈 ---

void unpack_reply(const CAN_message_t& msg) {

// According to the docs, the reply ID is 0x00, and the first data byte is the motor's ID.

// However, some implementations might use the motor's ID as the reply ID. We check the first byte.

uint8_t motor_id = msg.buf[0];

// Unpack ints from CAN buffer

int p_int = (msg.buf[1] << 8) | msg.buf[2];

int v_int = (msg.buf[3] << 4) | (msg.buf[4] >> 4);

int i_int = ((msg.buf[4] & 0x0F) << 8) | msg.buf[5];

// int temp = msg.buf[6]; // Temperature

// int error = msg.buf[7]; // Error code

// Convert ints to floats

float p = uint_to_float(p_int, P_MIN, P_MAX, 16);

float v = uint_to_float(v_int, V_MIN, V_MAX, 12);

float i = uint_to_float(i_int, T_MIN, T_MAX, 12);

// Update global state variables

if (motor_id == LEFT_HIP_ID) {

leftActualPos_rad = p;

leftActualVel_rad = v;

leftActualTorque = i;

} else if (motor_id == RIGHT_HIP_ID) {

rightActualPos_rad = p;

rightActualVel_rad = v;

rightActualTorque = i;

}

// --- Read and process motor feedback from CAN bus / 从CAN总线读取并处理电机反馈 ---

void readMotorFeedback() {

CAN_message_t msg;

while (Can0.read(msg)) {

// In Motion Control Mode, the reply is a standard frame.

if (!msg.flags.extended && msg.len == 8) {

unpack_reply(msg);

}

// =================================================================

// High-Level Control Functions / 高级控制函数

// =================================================================

void stopAllMotors() {

// Send a command to hold position at 0 with zero gains before exiting

packAndSendMotionCommand(LEFT_HIP_ID, 0.0, 0.0, 0.0, 0.0, 0.0);

delay(10);

packAndSendMotionCommand(RIGHT_HIP_ID, 0.0, 0.0, 0.0, 0.0, 0.0);

delay(10);

exitControlMode(LEFT_HIP_ID);

delay(10);

exitControlMode(RIGHT_HIP_ID);

Serial.println("已发送停止命令到所有电机");

}

void disableAllMotors() {

for (int pin : ENABLE_LEFT_PIN) digitalWrite(pin, LOW);

for (int pin : ENABLE_RIGHT_PIN) digitalWrite(pin, LOW);

Serial.println("所有电机驱动器已禁用");

}

/**

* @brief Runs a continuous gait simulation for a specified duration.

* Motors move in a sinusoidal pattern with a 180-degree phase shift.

*/

void runGaitSimulation() {

// --- Gait Parameters ---

const float AMPLITUDE_DEG = 30.0f; // Swing amplitude in degrees

const float PERIOD_S = 2.0f; // Period of one full cycle in seconds

const unsigned long DURATION_MS = 120000; // Total duration of the test (2 minutes)

const unsigned int LOOP_DELAY_MS = 20; // Loop delay, controls update frequency (50Hz)

const unsigned int PRINT_INTERVAL_MS = 200; // How often to print status

// --- Convert to radians and milliseconds ---

const float AMPLITUDE_RAD = AMPLITUDE_DEG * DEG_TO_RAD;

const float PERIOD_MS = PERIOD_S * 1000.0f;

Serial.printf("\n开始步态模拟: 幅度 ±%.1f度, 周期 %.1fs, 持续时间 %lus\n",

AMPLITUDE_DEG, PERIOD_S, DURATION_MS / 1000);

waitForKeyPress();

unsigned long startTime = millis();

unsigned long lastPrintTime = 0;

while (millis() - startTime < DURATION_MS) {

if (emergencyStop) break;

// --- Calculate Target Positions ---

unsigned long elapsedTime = millis() - startTime;

float angle_rad = AMPLITUDE_RAD * sin((float)elapsedTime * 2.0f * PI / PERIOD_MS);

float leftTargetRad = angle_rad;

float rightTargetRad = -angle_rad; // 180-degree phase shift

// --- Send Commands to Motors ---

packAndSendMotionCommand(LEFT_HIP_ID, leftTargetRad, 0.0, KP, KD, 0.0);

packAndSendMotionCommand(RIGHT_HIP_ID, rightTargetRad, 0.0, KP, KD, 0.0);

// --- Read Feedback ---

// This drains the CAN buffer and updates the global position variables

readMotorFeedback();

// --- Print Status Periodically ---

if (millis() - lastPrintTime >= PRINT_INTERVAL_MS) {

lastPrintTime = millis();

Serial.printf("左电机目标: %6.2f°, 实际: %6.2f° | 右电机目标: %6.2f°, 实际: %6.2f°\n",

leftTargetRad * RAD_TO_DEG, leftActualPos_rad * RAD_TO_DEG,

rightTargetRad * RAD_TO_DEG, rightActualPos_rad * RAD_TO_DEG);

}

delay(LOOP_DELAY_MS);

}

// --- Smoothly return to zero before stopping ---

if (!emergencyStop) {

Serial.println("步态模拟结束，正在平滑归零...");

unsigned long returnStartTime = millis();

float startPosLeft = leftActualPos_rad;

float startPosRight = rightActualPos_rad;

while(millis() - returnStartTime < 1000) { // 1 second to return to zero

unsigned long elapsedReturnTime = millis() - returnStartTime;

float factor = 1.0f - (float)elapsedReturnTime / 1000.0f;

packAndSendMotionCommand(LEFT_HIP_ID, startPosLeft * factor, 0.0, KP, KD, 0.0);

packAndSendMotionCommand(RIGHT_HIP_ID, startPosRight * factor, 0.0, KP, KD, 0.0);

delay(20);

}

Serial.println("已回到原点");

}

// =================================================================

// Main Setup and Loop / 主程序

// =================================================================

void setup() {

Serial.begin(115200);

while (!Serial) {}

Serial.println("串口初始化完成");

waitForKeyPress();

// Init E-stop pin

pinMode(MOTOR_STOP_PIN, OUTPUT);

digitalWrite(MOTOR_STOP_PIN, HIGH); // Release E-stop

Serial.printf("急停引脚状态: %s\n", digitalRead(MOTOR_STOP_PIN) ? "释放" : "激活");

waitForKeyPress();

// Init enable pins

for (int pin : ENABLE_LEFT_PIN) { pinMode(pin, OUTPUT); digitalWrite(pin, LOW); }

for (int pin : ENABLE_RIGHT_PIN) { pinMode(pin, OUTPUT); digitalWrite(pin, LOW); }

Serial.println("使能引脚初始化完成");

waitForKeyPress();

// Init CAN bus

Can0.begin();

Can0.setBaudRate(1000000); // 1 MHz

Serial.println("CAN总线初始化完成 (1MHz)");

waitForKeyPress();

// Enable motor power

for (int pin : ENABLE_LEFT_PIN) digitalWrite(pin, HIGH);

for (int pin : ENABLE_RIGHT_PIN) digitalWrite(pin, HIGH);

Serial.println("电机电源使能完成");

delay(1000); // Wait for power to stabilize

waitForKeyPress();

// Enter control mode on both motors

enterControlMode(LEFT_HIP_ID);

delay(10); // Small delay between commands

enterControlMode(RIGHT_HIP_ID);

delay(10);

// ** NEW STEP: Prevent initial jump **

// Immediately send a command with zero gains and zero torque to prevent the motor

// from moving to a default position. This holds the current physical position.

// **新增步骤：防止初始跳动**

// 立即发送一个增益和力矩都为零的命令，防止电机移动到一个默认位置，

// 从而让电机在当前物理位置保持不动。

packAndSendMotionCommand(LEFT_HIP_ID, 0.0, 0.0, 0.0, 0.0, 0.0);

delay(10);

packAndSendMotionCommand(RIGHT_HIP_ID, 0.0, 0.0, 0.0, 0.0, 0.0);

waitForKeyPress();

// Set origin

Serial.println("请确保髋关节摆动臂垂直地面 (0度位置)");

waitForKeyPress();

setOrigin(LEFT_HIP_ID);

delay(10);

setOrigin(RIGHT_HIP_ID);

Serial.println("原点设置完成");

delay(1000); // Wait for origin to be set

}

void loop() {

static bool testDone = false;

if (!testDone && !emergencyStop) {

// Run the new gait simulation

runGaitSimulation();

testDone = true;

stopAllMotors();

disableAllMotors();

Serial.println("\n测试结束");

}

// Non-blocking emergency stop check

if (Serial.available() > 0 && Serial.read() == 's') {

emergencyStop = true;

Serial.println("\n收到急停命令！");

stopAllMotors();

disableAllMotors();

}

if (emergencyStop) {

// Keep the program in a safe, halted state

delay(100);

}