From 0199c1e38f45a750a4539439d3ffa5e0ce672777 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sat, 18 Jan 2025 18:26:45 +0300 Subject: [PATCH 1/9] Configure clang tools and update build settings - Added `.clang-tidy` with custom checks to enforce code style. - Created `.clangd` with specific compile flag additions and removals. - Updated `.gitignore` to include new cache, metadata, and configuration files. - Updated `platformio.ini`: - Bumped `Simple FOC` to version 2.3.4 and `STM32_CAN` to version 1.1.2. - Added `gen_compile_commands.py` as an extra script. --- controller/fw/embed/.clang-tidy | 1 + controller/fw/embed/.clangd | 18 ++++++++++++++++++ controller/fw/embed/.gitignore | 4 ++++ controller/fw/embed/check_gcc_version.py | 19 +++++++++++++++++++ controller/fw/embed/gen_compile_commands.py | 8 ++++++++ controller/fw/embed/platformio.ini | 5 +++-- 6 files changed, 53 insertions(+), 2 deletions(-) create mode 100644 controller/fw/embed/.clang-tidy create mode 100644 controller/fw/embed/.clangd create mode 100644 controller/fw/embed/check_gcc_version.py create mode 100644 controller/fw/embed/gen_compile_commands.py diff --git a/controller/fw/embed/.clang-tidy b/controller/fw/embed/.clang-tidy new file mode 100644 index 0000000..3f9824b --- /dev/null +++ b/controller/fw/embed/.clang-tidy @@ -0,0 +1 @@ +Checks: '-*, -misc-definitions-in-headers' diff --git a/controller/fw/embed/.clangd b/controller/fw/embed/.clangd new file mode 100644 index 0000000..05f45f8 --- /dev/null +++ b/controller/fw/embed/.clangd @@ -0,0 +1,18 @@ +CompileFlags: + Add: + [ + # -mlong-calls, + -DSSIZE_MAX, + -DLWIP_NO_UNISTD_H=1, + -Dssize_t=long, + -D_SSIZE_T_DECLARED, + ] + Remove: + [ + -fno-tree-switch-conversion, + -mtext-section-literals, + -mlongcalls, + -fstrict-volatile-bitfields, + -free, + -fipa-pta, + ] diff --git a/controller/fw/embed/.gitignore b/controller/fw/embed/.gitignore index 89cc49c..b0e6063 100644 --- a/controller/fw/embed/.gitignore +++ b/controller/fw/embed/.gitignore @@ -3,3 +3,7 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch +.cache/ +.metadata/ +cubemx_config/ +compile_commands.json diff --git a/controller/fw/embed/check_gcc_version.py b/controller/fw/embed/check_gcc_version.py new file mode 100644 index 0000000..41d1176 --- /dev/null +++ b/controller/fw/embed/check_gcc_version.py @@ -0,0 +1,19 @@ +Import("env") + +# Получаем путь к компилятору из окружения PlatformIO +gcc_path = env.subst("$CC") + +# Выполняем команду для получения версии компилятора +import subprocess + +try: + result = subprocess.run([gcc_path, "--version"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + if result.returncode == 0: + print(f"GCC version: {result.stdout}") + else: + print(f"Failed to get GCC version: {result.stderr}") +except Exception as e: + print(f"Error while getting GCC version: {e}") + +# Дополнительно проверяем путь к компилятору +print(f"Compiler path: {gcc_path}") diff --git a/controller/fw/embed/gen_compile_commands.py b/controller/fw/embed/gen_compile_commands.py new file mode 100644 index 0000000..0537d0b --- /dev/null +++ b/controller/fw/embed/gen_compile_commands.py @@ -0,0 +1,8 @@ +import os +Import("env") + +# include toolchain paths +env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True) + +# override compilation DB path +env.Replace(COMPILATIONDB_PATH="compile_commands.json") diff --git a/controller/fw/embed/platformio.ini b/controller/fw/embed/platformio.ini index 854186d..74e0b85 100644 --- a/controller/fw/embed/platformio.ini +++ b/controller/fw/embed/platformio.ini @@ -23,5 +23,6 @@ build_flags = -D HAL_CAN_MODULE_ENABLED -D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH lib_deps = - askuric/Simple FOC@^2.3.2 - pazi88/STM32_CAN@^1.1.0 + askuric/Simple FOC@^2.3.4 + pazi88/STM32_CAN@^1.1.2 +extra_scripts = pre:gen_compile_commands.py From 1534b854fcd68e4d20c719f0ba96d1e9d402f0a9 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sat, 18 Jan 2025 18:28:56 +0300 Subject: [PATCH 2/9] Refactor and enhance motor control implementation - Introduced `MotorControlInputs` struct to centralize motor control state. - Renamed `sensor` to `encoder` for clarity in naming conventions. - Added `send_velocity`, `send_angle`, and `send_motor_enabled` functions for modular CAN message handling. - Updated `setup_foc` to enhance motor initialization with configurable parameters. - Replaced `run_foc` with `foc_step` for improved motor control logic: - Dynamically switches between angle and velocity control based on inputs. - Enhanced CAN message handling with `read_can_step` to process specific control commands. - Adjusted current sensing and other configuration values for precision. - Added placeholders for temperature sensor setup and handling. --- controller/fw/embed/src/main.cpp | 231 ++++++++++++++++++++----------- 1 file changed, 152 insertions(+), 79 deletions(-) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index d446a0d..05567b7 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -4,7 +4,12 @@ #include #include #include +#include +#include +#include "common/base_classes/FOCMotor.h" #include "hal_conf_extra.h" +#include "wiring_analog.h" +#include "wiring_constants.h" // clang-format on STM32_CAN Can(CAN2, DEF); @@ -13,109 +18,177 @@ static CAN_message_t CAN_TX_msg; static CAN_message_t CAN_inMsg; SPIClass spi; -MagneticSensorAS5045 sensor(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK); +MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK); BLDCMotor motor(POLE_PAIRS); DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER, EN_U_GATE_DRIVER, EN_V_GATE_DRIVER, SLEEP_DRIVER, RESET_DRIVER, FAULT_DRIVER); -LowsideCurrentSense current_sense(0.0125, 20, CURRENT_SENSOR_1, CURRENT_SENSOR_2, - CURRENT_SENSOR_3); +LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1, + CURRENT_SENSOR_2, CURRENT_SENSOR_3); Commander command(Serial); +struct MotorControlInputs { + float target_angle; + float target_velocity; + bool motor_enabled; +}; + +MotorControlInputs motor_control_inputs; + void doMotor(char *cmd) { - command.motor(&motor, cmd); - digitalWrite(PC10, !digitalRead(PC10)); - delayMicroseconds(2); + command.motor(&motor, cmd); + // digitalWrite(PC10, !digitalRead(PC10)); + // delayMicroseconds(2); } -void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *driver, - LowsideCurrentSense *current_sense, Commander *commander, - CommandCallback callback) { - sensor->init(&spi); - motor->linkSensor(sensor); - driver->pwm_frequency = 20000; - driver->voltage_power_supply = 20; - driver->voltage_limit = 40; - driver->init(); - motor->linkDriver(driver); - current_sense->linkDriver(driver); - current_sense->init(); - motor->linkCurrentSense(current_sense); - SimpleFOCDebug::enable(nullptr); - commander->add('M', callback, "motor"); - motor->useMonitoring(Serial); - motor->monitor_start_char = 'M'; - motor->monitor_end_char = 'M'; - commander->verbose = VerboseMode::machine_readable; - motor->controller = MotionControlType::angle; - motor->torque_controller = TorqueControlType::voltage; - motor->foc_modulation = FOCModulationType::SinePWM; - motor->PID_velocity.P = 0.3; - motor->PID_velocity.I = 15; - motor->PID_velocity.D = 0.001; - motor->PID_velocity.output_ramp = 1000; - motor->LPF_velocity.Tf = 0.005; - motor->LPF_angle.Tf = 0.005; - motor->P_angle.P = 15; - motor->velocity_limit = 40; - motor->voltage_limit = 40; - motor->sensor_direction = Direction::CCW; - current_sense->skip_align = true; - motor->init(); - motor->initFOC(); +void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, + DRV8313Driver *driver, LowsideCurrentSense *current_sense, + Commander *commander, CommandCallback callback) { + encoder->init(&spi); + motor->linkSensor(encoder); + driver->pwm_frequency = 20000; + driver->voltage_power_supply = 20; + driver->voltage_limit = 40; + driver->init(); + motor->linkDriver(driver); + current_sense->linkDriver(driver); + current_sense->init(); + motor->linkCurrentSense(current_sense); + SimpleFOCDebug::enable(nullptr); + commander->add('M', callback, "motor"); + motor->useMonitoring(Serial); + motor->monitor_start_char = 'M'; + motor->monitor_end_char = 'M'; + // motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; + motor->monitor_downsample = 10; // default 10 + commander->verbose = VerboseMode::user_friendly; + motor->controller = MotionControlType::angle; + motor->torque_controller = TorqueControlType::voltage; + motor->foc_modulation = FOCModulationType::SinePWM; + motor->PID_velocity.P = 0.3; + motor->PID_velocity.I = 15; + motor->PID_velocity.D = 0.001; + motor->PID_velocity.output_ramp = 1000; + motor->LPF_velocity.Tf = 0.005; + motor->LPF_angle.Tf = 0.005; + motor->P_angle.P = 15; + motor->velocity_limit = 40; + motor->voltage_limit = 40; + motor->current_limit = 0.5; + motor->sensor_direction = Direction::CCW; + current_sense->skip_align = true; + motor->init(); + motor->initFOC(); } +void send_velocity() { + float current_velocity = motor.shaftVelocity(); + CAN_TX_msg.id = 1; + CAN_TX_msg.buf[0] = 'V'; + CAN_TX_msg.len = 5; + memcpy(&CAN_TX_msg.buf[1], ¤t_velocity, sizeof(current_velocity)); + Can.write(CAN_TX_msg); +} + +void send_angle() { + float current_angle = motor.shaftAngle(); + CAN_TX_msg.id = 1; + CAN_TX_msg.buf[0] = 'A'; + CAN_TX_msg.len = 5; + memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); + Can.write(CAN_TX_msg); +} + +void send_motor_enabled() { + CAN_TX_msg.id = 1; + CAN_TX_msg.buf[0] = 'E'; + memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled, + sizeof(motor_control_inputs.motor_enabled)); + Can.write(CAN_TX_msg); +} void send_data() { - // TODO(vanyabeat) add can functionability for ROS packets - CAN_TX_msg.id = 1; - CAN_TX_msg.buf[0] = 'A'; - CAN_TX_msg.len = 5; - Can.write(CAN_TX_msg); - digitalWrite(PC11, !digitalRead(PC11)); + // TODO(vanyabeat) add can functionability for ROS packets + send_velocity(); + send_angle(); + send_motor_enabled(); + // read_temperature(); + digitalWrite(PC11, !digitalRead(PC11)); } -void read_can_message() { - float angle; - memcpy(&angle, &CAN_inMsg.buf[1], sizeof(angle)); - motor.target = angle; - digitalWrite(PC10, !digitalRead(PC10)); +void read_can_step() { + char flag = CAN_inMsg.buf[0]; + if (flag == 'V') { + memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.target_velocity)); + } else if (flag == 'A') { + memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.target_angle)); + } else if (flag == 'E') { + bool enable_flag = CAN_inMsg.buf[1]; + if (enable_flag == 1) { + memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.motor_enabled)); + motor.enable(); + } else if (enable_flag == 0) { + memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.motor_enabled)); + motor.disable(); + } + } + // motor.target = angle; + digitalWrite(PC10, !digitalRead(PC10)); } -void run_foc(BLDCMotor *motor, Commander *commander) { - motor->loopFOC(); - motor->move(); - motor->monitor(); - commander->run(); +void foc_step(BLDCMotor *motor, Commander *commander) { + if (motor_control_inputs.target_velocity != 0 || + motor->controller == MotionControlType::velocity) { + if (motor->controller != MotionControlType::velocity) { + motor->controller = MotionControlType::velocity; + } + motor->target = motor_control_inputs.target_velocity; + } else { + if (motor->controller != MotionControlType::angle) { + motor->controller = MotionControlType::angle; + } + motor->target = motor_control_inputs.target_angle; + } + + motor->loopFOC(); + motor->move(); + motor->monitor(); + commander->run(); } void setup() { - Serial.setRx(HARDWARE_SERIAL_RX_PIN); - Serial.setTx(HARDWARE_SERIAL_TX_PIN); - Serial.begin(115200); - // setup led pin - pinMode(PC11, OUTPUT); - pinMode(PC10, OUTPUT); - Can.begin(); - Can.setBaudRate(1000000); - TIM_TypeDef *Instance = TIM2; - HardwareTimer *SendTimer = new HardwareTimer(Instance); - SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz - SendTimer->attachInterrupt(send_data); - SendTimer->resume(); - setup_foc(&sensor, &motor, &driver, ¤t_sense, &command, doMotor); - _delay(1000); + Serial.setRx(HARDWARE_SERIAL_RX_PIN); + Serial.setTx(HARDWARE_SERIAL_TX_PIN); + Serial.begin(115200); + // setup led pin + pinMode(PC11, OUTPUT); + pinMode(PC10, OUTPUT); + // Setup thermal sensor pin + // pinMode(TH1, INPUT_ANALOG); + Can.begin(); + Can.setBaudRate(1000000); + TIM_TypeDef *Instance = TIM2; + HardwareTimer *SendTimer = new HardwareTimer(Instance); + SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz + SendTimer->attachInterrupt(send_data); + SendTimer->resume(); + setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); + _delay(1000); } -float current_angle = 0; +float current_angle = 0.0; void loop() { - run_foc(&motor, &command); - current_angle = sensor.getAngle(); - memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); - while (Can.read(CAN_inMsg)) { - read_can_message(); - } + foc_step(&motor, &command); + // current_angle = encoder.getAngle(); + // memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); + while (Can.read(CAN_inMsg)) { + read_can_step(); + } } From 51e8fac95abf3313e1a4ba859e1bfc63c489ca22 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sat, 18 Jan 2025 18:34:05 +0300 Subject: [PATCH 3/9] Add Python test scripts for CAN communication - Created `python_can.py`: - Implements a CAN message receiver that processes angle, velocity, and enable/disable flags. - Added `python_enable_motor.py`: - Sends enable/disable commands to the motor via CAN. - Added `python_send_angle.py`: - Sends target angle commands over the CAN bus. - Added `python_send_velocity.py`: - Sends target velocity commands over the CAN bus. - Configured all scripts to use `python-can` library with `socketcan` interface for CAN communication. --- controller/fw/embed/test/python_can.py | 47 +++++++++++++++++++ .../fw/embed/test/python_enable_motor.py | 47 +++++++++++++++++++ controller/fw/embed/test/python_send_angle.py | 34 ++++++++++++++ .../fw/embed/test/python_send_velocity.py | 34 ++++++++++++++ 4 files changed, 162 insertions(+) create mode 100644 controller/fw/embed/test/python_can.py create mode 100644 controller/fw/embed/test/python_enable_motor.py create mode 100644 controller/fw/embed/test/python_send_angle.py create mode 100644 controller/fw/embed/test/python_send_velocity.py diff --git a/controller/fw/embed/test/python_can.py b/controller/fw/embed/test/python_can.py new file mode 100644 index 0000000..7a54a29 --- /dev/null +++ b/controller/fw/embed/test/python_can.py @@ -0,0 +1,47 @@ +import can +import struct +import time + +def process_can_message(msg): + if msg.dlc == 5: # Check the message length + print(f"Received message with ID: {msg.arbitration_id}") + print(f"Data: {msg.data}") + + # The first byte determines the data type (flag) + flag = chr(msg.data[0]) + + if flag == 'A': # Angle + angle_bytes = msg.data[1:5] + angle = struct.unpack('= 2: # Enable/Disable + enabled = msg.data[1] # Expecting 1 byte (0 or 1) + print(f"Enabled: {bool(enabled)}") + else: + print(f"Unknown flag: {flag}") + else: + print(f"Received message with unexpected length: {msg.dlc}") + +def receive_can_messages(): + try: + # Connect to the CAN bus + bus = can.interface.Bus(channel='can0', bustype='socketcan') + + print("Waiting for messages on the CAN bus...") + + while True: + msg = bus.recv() + if msg: + process_can_message(msg) + + except KeyboardInterrupt: + print("\nExiting program...") + except Exception as e: + print(f"Error: {e}") + +if __name__ == '__main__': + receive_can_messages() diff --git a/controller/fw/embed/test/python_enable_motor.py b/controller/fw/embed/test/python_enable_motor.py new file mode 100644 index 0000000..d42a47c --- /dev/null +++ b/controller/fw/embed/test/python_enable_motor.py @@ -0,0 +1,47 @@ +import can +import sys + +# Function to send the motor enable/disable command +def send_motor_enable(bus, enable): + """ + Sends a command to enable or disable the motor. + + :param bus: The CAN bus + :param enable: 1 to enable the motor, 0 to disable it + """ + msg = can.Message() + msg.arbitration_id = 1 # Message ID + msg.is_extended_id = False + msg.dlc = 2 # Message length (flag + 1 byte of data) + msg.data = [ord('E'), enable] # 'E' for the command, followed by 0 or 1 + + try: + bus.send(msg) + state = "enabled" if enable else "disabled" + print(f"Sent message to {state} motor") + print(f"Message data: {msg.data}") + except can.CanError: + print("Message failed to send") + sys.exit(1) # Exit the program on failure + +def main(): + # CAN interface setup + try: + bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings + print("CAN bus initialized.") + + # Ensure the state is passed via arguments + if len(sys.argv) != 2 or sys.argv[1] not in ['0', '1']: + print("Usage: python3 script_name.py <0|1>") + print("0 - Disable motor, 1 - Enable motor") + sys.exit(1) + + enable = int(sys.argv[1]) + send_motor_enable(bus, enable) + + except Exception as e: + print(f"Error initializing CAN bus: {e}") + sys.exit(1) + +if __name__ == '__main__': + main() diff --git a/controller/fw/embed/test/python_send_angle.py b/controller/fw/embed/test/python_send_angle.py new file mode 100644 index 0000000..cbb399d --- /dev/null +++ b/controller/fw/embed/test/python_send_angle.py @@ -0,0 +1,34 @@ +import can +import struct +import time + +# Function to send the target angle +def send_target_angle(bus, target_angle): + msg = can.Message() + msg.arbitration_id = 1 # Message ID + msg.is_extended_id = False + msg.dlc = 5 # Message length + msg.data = [ord('A')] + list(struct.pack(' Date: Sat, 18 Jan 2025 18:40:23 +0300 Subject: [PATCH 4/9] Add README file with instructions for CAN communication scripts - Added a README file containing instructions for using the Python scripts to test and interact with a CAN bus system. - Included details on prerequisites, usage, configuration, and troubleshooting. - Provided step-by-step guidance for running each script, including arguments and behavior. --- controller/fw/embed/test/README | 11 ---- controller/fw/embed/test/README.md | 86 ++++++++++++++++++++++++++++++ 2 files changed, 86 insertions(+), 11 deletions(-) delete mode 100644 controller/fw/embed/test/README create mode 100644 controller/fw/embed/test/README.md diff --git a/controller/fw/embed/test/README b/controller/fw/embed/test/README deleted file mode 100644 index 9b1e87b..0000000 --- a/controller/fw/embed/test/README +++ /dev/null @@ -1,11 +0,0 @@ - -This directory is intended for PlatformIO Test Runner and project tests. - -Unit Testing is a software testing method by which individual units of -source code, sets of one or more MCU program modules together with associated -control data, usage procedures, and operating procedures, are tested to -determine whether they are fit for use. Unit testing finds problems early -in the development cycle. - -More information about PlatformIO Unit Testing: -- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/controller/fw/embed/test/README.md b/controller/fw/embed/test/README.md new file mode 100644 index 0000000..4ac995f --- /dev/null +++ b/controller/fw/embed/test/README.md @@ -0,0 +1,86 @@ +# CAN Communication Scripts + +This repository contains Python scripts for testing and interacting with a CAN bus system. These scripts enable sending and receiving CAN messages to control a motor, set angles, and adjust velocities. + +## Prerequisites + +1. **Python 3.7+** installed on your system. +2. **`python-can` library** installed. Install it via pip: + ```bash + pip install python-can + ``` +3. **SocketCAN interface** properly configured on your Linux system. The default channel is `can0`. + +## Usage + +### 1. Receiving CAN Messages + +The script `python_can.py` listens to the CAN bus and processes incoming messages. + +#### Run: +```bash +python3 python_can.py +``` + +#### Features: +- Processes messages with data length 5. +- Parses the first byte (`flag`) to determine the type: + - `'A'`: Angle (float). + - `'V'`: Velocity (float). + - `'E'`: Enable/disable status (boolean). + +### 2. Enabling or Disabling the Motor + +The script `python_enable_motor.py` sends commands to enable or disable the motor. + +#### Run: +```bash +python3 python_enable_motor.py <0|1> +``` + +#### Arguments: +- `0`: Disable the motor. +- `1`: Enable the motor. + +### 3. Sending Target Angle + +The script `python_send_angle.py` sends a target angle to the CAN bus. + +#### Run: +```bash +python3 python_send_angle.py +``` + +#### Behavior: +- Sends a message with a predefined target angle every second. +- Adjust the target angle in the script (`target_angle` variable). + +### 4. Sending Target Velocity + +The script `python_send_velocity.py` sends a target velocity to the CAN bus. + +#### Run: +```bash +python3 python_send_velocity.py +``` + +#### Behavior: +- Sends a message with a predefined target velocity every second. +- Adjust the target velocity in the script (`target_speed` variable). + +## Configuration + +### CAN Interface +The scripts use the following default CAN bus settings: +- **Channel**: `can0` +- **Bitrate**: `1 Mbps` + +If your configuration differs, update the `Bus()` initialization in the scripts. + +## Troubleshooting + +1. **"Error initializing CAN bus"**: + - Ensure your CAN interface is correctly configured and active: + ```bash + sudo ip link set can0 up type can bitrate 1000000 + ``` From d5f772cb6d3139193db6b47c172fcaa054f3faa0 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 4 Feb 2025 13:40:15 +0300 Subject: [PATCH 5/9] Adjust serial monitor speed and refine motor control logic - Changed `monitor_speed` in `platformio.ini` from `115200` to `19200` for compatibility. - Initialized `MotorControlInputs` struct members with default values. - Uncommented digital write operations in `doMotor` for proper execution. - Disabled debug output and monitoring configurations for cleaner operation. - Adjusted motor tuning parameters by commenting out specific settings. - Changed `Serial.begin(115200)` to `Serial.begin(19200)` in `setup()` to match the new monitor speed. - Commented out `_delay(1000)` and `current_angle` variable to remove unnecessary delays and unused code. --- controller/fw/embed/platformio.ini | 2 +- controller/fw/embed/src/main.cpp | 28 +++++++++++++++------------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/controller/fw/embed/platformio.ini b/controller/fw/embed/platformio.ini index 74e0b85..a759816 100644 --- a/controller/fw/embed/platformio.ini +++ b/controller/fw/embed/platformio.ini @@ -16,7 +16,7 @@ board = genericSTM32F446RE framework = arduino upload_protocol = stlink debug_tool = stlink -monitor_speed = 115200 +monitor_speed = 19200 monitor_parity = N build_flags = -DSTM32F446xx diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 05567b7..36d46d5 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -31,17 +31,17 @@ LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1, Commander command(Serial); struct MotorControlInputs { - float target_angle; - float target_velocity; - bool motor_enabled; + float target_angle = 0.0; + float target_velocity = 0.0; + bool motor_enabled = false; }; MotorControlInputs motor_control_inputs; void doMotor(char *cmd) { command.motor(&motor, cmd); - // digitalWrite(PC10, !digitalRead(PC10)); - // delayMicroseconds(2); + digitalWrite(PC10, !digitalRead(PC10)); + delayMicroseconds(2); } void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, @@ -57,14 +57,14 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, current_sense->linkDriver(driver); current_sense->init(); motor->linkCurrentSense(current_sense); - SimpleFOCDebug::enable(nullptr); + // SimpleFOCDebug::enable(nullptr); commander->add('M', callback, "motor"); motor->useMonitoring(Serial); - motor->monitor_start_char = 'M'; - motor->monitor_end_char = 'M'; + // motor->monitor_start_char = 'M'; + // motor->monitor_end_char = 'M'; // motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; motor->monitor_downsample = 10; // default 10 - commander->verbose = VerboseMode::user_friendly; + // commander->verbose = VerboseMode::user_friendly; motor->controller = MotionControlType::angle; motor->torque_controller = TorqueControlType::voltage; motor->foc_modulation = FOCModulationType::SinePWM; @@ -75,11 +75,13 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, motor->LPF_velocity.Tf = 0.005; motor->LPF_angle.Tf = 0.005; motor->P_angle.P = 15; + // motor->P_angle.I = 15; + // motor->P_angle.3 = 15; motor->velocity_limit = 40; motor->voltage_limit = 40; motor->current_limit = 0.5; motor->sensor_direction = Direction::CCW; - current_sense->skip_align = true; + current_sense->skip_align = false; motor->init(); motor->initFOC(); } @@ -166,7 +168,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) { void setup() { Serial.setRx(HARDWARE_SERIAL_RX_PIN); Serial.setTx(HARDWARE_SERIAL_TX_PIN); - Serial.begin(115200); + Serial.begin(19200); // setup led pin pinMode(PC11, OUTPUT); pinMode(PC10, OUTPUT); @@ -180,10 +182,10 @@ void setup() { SendTimer->attachInterrupt(send_data); SendTimer->resume(); setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); - _delay(1000); + // _delay(1000); } -float current_angle = 0.0; +// float current_angle = 0.0; void loop() { foc_step(&motor, &command); // current_angle = encoder.getAngle(); From 74c5b1d12c8df71370cd370d4f50b34a4ea31101 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 4 Feb 2025 13:41:05 +0300 Subject: [PATCH 6/9] Enhance CAN bus scripts with argparse and error handling - Improved error handling in `python_enable_motor.py` by capturing and displaying exception details. - Ensured proper CAN bus shutdown in `python_enable_motor.py` with a `finally` block. - Refactored `python_send_angle.py` to accept a command-line argument for the target angle instead of looping indefinitely. - Refactored `python_send_velocity.py` to accept a command-line argument for target speed and removed infinite loops for better usability. - Added `send_velocity_impulses.py`, a new script to send alternating velocity impulses over the CAN bus. --- .../fw/embed/test/python_enable_motor.py | 11 ++++-- controller/fw/embed/test/python_send_angle.py | 15 ++++---- .../fw/embed/test/python_send_velocity.py | 20 ++++++----- .../fw/embed/test/send_velocity_impulses.py | 35 +++++++++++++++++++ 4 files changed, 64 insertions(+), 17 deletions(-) create mode 100644 controller/fw/embed/test/send_velocity_impulses.py diff --git a/controller/fw/embed/test/python_enable_motor.py b/controller/fw/embed/test/python_enable_motor.py index d42a47c..c491eb7 100644 --- a/controller/fw/embed/test/python_enable_motor.py +++ b/controller/fw/embed/test/python_enable_motor.py @@ -20,12 +20,13 @@ def send_motor_enable(bus, enable): state = "enabled" if enable else "disabled" print(f"Sent message to {state} motor") print(f"Message data: {msg.data}") - except can.CanError: - print("Message failed to send") + except can.CanError as e: + print(f"Message failed to send: {e}") sys.exit(1) # Exit the program on failure def main(): # CAN interface setup + bus = None # Define outside the try block for proper shutdown try: bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings print("CAN bus initialized.") @@ -43,5 +44,11 @@ def main(): print(f"Error initializing CAN bus: {e}") sys.exit(1) + finally: + # Ensure the bus is properly shut down + if bus is not None: + bus.shutdown() + print("CAN bus shut down.") + if __name__ == '__main__': main() diff --git a/controller/fw/embed/test/python_send_angle.py b/controller/fw/embed/test/python_send_angle.py index cbb399d..dc23d9b 100644 --- a/controller/fw/embed/test/python_send_angle.py +++ b/controller/fw/embed/test/python_send_angle.py @@ -1,6 +1,7 @@ import can import struct import time +import argparse # Function to send the target angle def send_target_angle(bus, target_angle): @@ -17,18 +18,20 @@ def send_target_angle(bus, target_angle): except can.CanError: print("Message failed to send") +# Main function def main(): + parser = argparse.ArgumentParser(description="Send target angles over CAN bus.") + parser.add_argument("--angle", type=float, required=True, help="Target angle to send over the CAN bus") + args = parser.parse_args() + + target_angle = args.angle + # CAN interface setup bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings print("CAN bus initialized, sending target angles...") - # Target angle - target_angle = 0.0 - # Loop to send messages - while True: - send_target_angle(bus, target_angle) - time.sleep(1) # Delay before sending the next message + send_target_angle(bus, target_angle) if __name__ == '__main__': main() diff --git a/controller/fw/embed/test/python_send_velocity.py b/controller/fw/embed/test/python_send_velocity.py index c485085..e01f24f 100644 --- a/controller/fw/embed/test/python_send_velocity.py +++ b/controller/fw/embed/test/python_send_velocity.py @@ -1,6 +1,6 @@ import can import struct -import time +import argparse # Function to send the target speed def send_target_speed(bus, target_speed): @@ -17,18 +17,20 @@ def send_target_speed(bus, target_speed): except can.CanError: print("Message failed to send") +# Main function def main(): + parser = argparse.ArgumentParser(description="Send target speed over CAN bus.") + parser.add_argument("--speed", type=float, required=True, help="Target speed to send over the CAN bus (in m/s)") + args = parser.parse_args() + + target_speed = args.speed + # CAN interface setup bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings - print("CAN bus initialized, sending target speeds...") + print("CAN bus initialized, sending target speed...") - # Target speed - target_speed = 0.0 # in meters per second - - # Loop to send messages - while True: - send_target_speed(bus, target_speed) - time.sleep(1) # Delay before sending the next message + # Send the message once + send_target_speed(bus, target_speed) if __name__ == '__main__': main() diff --git a/controller/fw/embed/test/send_velocity_impulses.py b/controller/fw/embed/test/send_velocity_impulses.py new file mode 100644 index 0000000..4d9a21c --- /dev/null +++ b/controller/fw/embed/test/send_velocity_impulses.py @@ -0,0 +1,35 @@ +import can +import struct +import time + +# Function to send the target speed +def send_target_speed(bus, target_speed): + msg = can.Message() + msg.arbitration_id = 1 # Message ID + msg.is_extended_id = False + msg.dlc = 5 # Message length + msg.data = [ord('V')] + list(struct.pack(' Date: Tue, 4 Feb 2025 13:41:56 +0300 Subject: [PATCH 7/9] Refactor ROS2 actuator with improved CAN handling and logging - Introduced `rclcpp::Logger` for consistent logging throughout `RbsServoActuator`. - Renamed joint state and command variables to `hw_joint_angle`, `hw_joint_velocity`, `hw_joint_target_angle`, and `hw_joint_target_velocity` for clarity. - Added parameters `can_interface` and `can_node_id`, allowing configurable CAN interface and node ID. - Enhanced CAN initialization and error handling in `on_init` to properly retrieve interface index and bind the socket. - Implemented structured CAN message handling: - Sends activation command when the actuator is enabled. - Reads and logs incoming CAN frames for joint state updates. - Ensures error handling for failed read/write operations. - Updated URDF (`simple_robot.urdf`) to include `can_interface` and `can_node_id` parameters for hardware configuration. --- .../rbs_servo_hardware/rbs_servo_actuator.hpp | 12 +- .../src/rbs_servo_actuator.cpp | 137 +++++++++++------- .../src/rbs_servo_test/urdf/simple_robot.urdf | 2 + 3 files changed, 92 insertions(+), 59 deletions(-) diff --git a/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp b/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp index 8bca87b..58b3896 100644 --- a/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp +++ b/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp @@ -3,6 +3,7 @@ // Libs for fake connection // WARN: Delete it in the future #include +#include #include // System libs #include @@ -58,11 +59,15 @@ private: // Parameters for the rbs_servo_hardware simulation // double hw_start_sec_; // double hw_stop_sec_; + + rclcpp::Logger logger_ = rclcpp::get_logger("rbs_servo_hardware"); // Store the command for the robot - double hw_joint_command_; + double hw_joint_target_angle_; + double hw_joint_target_velocity_; // Store the states for the robot - double hw_joint_state_; + double hw_joint_angle_; + double hw_joint_velocity_; // Fake "mechanical connection" between actuator and sensor using sockets // struct sockaddr_in address_; @@ -70,7 +75,8 @@ private: // int sockoptval_ = 1; // int sock_; - std::string can_interface_ = "can0"; + std::string can_interface_; + int can_node_id_; int can_socket_; }; diff --git a/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp b/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp index cb48ae7..fef7045 100644 --- a/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp +++ b/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp @@ -7,17 +7,19 @@ #include #include #include +#include #include // ROS specific libs #include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#include #include #include -#include -#include #include +#include +#include +#include #include namespace rbs_servo_hardware { @@ -28,26 +30,29 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) { return hardware_interface::CallbackReturn::ERROR; } - hw_joint_command_ = std::numeric_limits::quiet_NaN(); + hw_joint_target_angle_ = std::numeric_limits::quiet_NaN(); + + can_interface_ = info_.hardware_parameters["can_interface"]; + can_node_id_ = stoi(info_.hardware_parameters["can_node_id"]); const hardware_interface::ComponentInfo &joint = info_.joints[0]; // RbsServoActuator has exactly one command interface and one joint if (joint.command_interfaces.size() != 1) { - RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), + RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), + RCLCPP_FATAL(logger_, "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } else if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), + RCLCPP_FATAL(logger_, "Joint '%s' have %s state interface found. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); @@ -55,8 +60,7 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) { } if ((can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { - RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), - "Failed to create CAN socket"); + RCLCPP_ERROR(logger_, "Failed to create CAN socket"); return hardware_interface::CallbackReturn::ERROR; } @@ -64,8 +68,7 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) { strncpy(ifr.ifr_name, can_interface_.c_str(), IFNAMSIZ - 1); ifr.ifr_name[IFNAMSIZ - 1] = '\0'; if (ioctl(can_socket_, SIOCGIFINDEX, &ifr) < 0) { - RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), - "Failed to get interface index"); + RCLCPP_ERROR(logger_, "Failed to get interface index"); return hardware_interface::CallbackReturn::ERROR; } @@ -73,7 +76,7 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) { addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; if (bind(can_socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0) { - RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to bind CAN socket"); + RCLCPP_ERROR(logger_, "Failed to bind CAN socket"); return hardware_interface::CallbackReturn::ERROR; } @@ -85,14 +88,20 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) { // server->h_length); // address_.sin_port = htons(socket_port_); - // RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), + // RCLCPP_INFO(logger_, // "Trying to connect to port %d.", socket_port_); // if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) { - // RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), + // RCLCPP_FATAL(logger_, // "Connection over socket failed."); // return hardware_interface::CallbackReturn::ERROR; // } - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Connected to socket"); + RCLCPP_INFO(logger_, "Connected to socket"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn +on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { return hardware_interface::CallbackReturn::SUCCESS; } @@ -110,7 +119,7 @@ RbsServoActuator::export_state_interfaces() { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[0].name, hardware_interface::HW_IF_POSITION, - &hw_joint_state_)); + &hw_joint_angle_)); return state_interfaces; } @@ -121,7 +130,7 @@ RbsServoActuator::export_command_interfaces() { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[0].name, hardware_interface::HW_IF_POSITION, - &hw_joint_command_)); + &hw_joint_target_angle_)); return command_interfaces; } @@ -130,24 +139,31 @@ hardware_interface::CallbackReturn RbsServoActuator::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // START: This part here is for exemplary purposes - Please do not copy to // your production code - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), - "Activating ...please wait..."); - - // for (int i = 0; i < hw_start_sec_; i++) { - // rclcpp::sleep_for(std::chrono::seconds(1)); - // RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds - // left...", - // hw_start_sec_ - i); - // } - // END: This part here is for exemplary purposes - Please do not copy to your - // production code + RCLCPP_INFO(logger_, "Activating ...please wait..."); // set some default values for joints - if (std::isnan(hw_joint_command_)) { - hw_joint_command_ = 0; - hw_joint_state_ = 0; + if (std::isnan(hw_joint_target_angle_)) { + hw_joint_target_angle_ = 0; + hw_joint_angle_ = 0; } + // Создание CAN-кадра для включения двигателя + struct can_frame frame; + frame.can_id = can_node_id_; // Идентификатор сообщения (замените на ваш ID) + frame.can_dlc = 2; // Длина данных (1 байт для флага + 1 байт для значения) + frame.data[0] = 'E'; // Флаг для команды управления двигателем + frame.data[1] = true; // Значение для включения двигателя (true -> 0x01) + + // Отправка сообщения + if (::write(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) { + RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), + "Failed to send CAN frame for motor activation"); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), + "Motor activation command sent successfully"); + RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Successfully activated!"); @@ -158,18 +174,16 @@ hardware_interface::CallbackReturn RbsServoActuator::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // START: This part here is for exemplary purposes - Please do not copy to // your production code - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), - "Deactivating ...please wait..."); + RCLCPP_INFO(logger_, "Deactivating ...please wait..."); // for (int i = 0; i < hw_stop_sec_; i++) { // rclcpp::sleep_for(std::chrono::seconds(1)); - // RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds + // RCLCPP_INFO(logger_, "%.1f seconds // left...", // hw_stop_sec_ - i); // } - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), - "Successfully deactivated!"); + RCLCPP_INFO(logger_, "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your // production code close(can_socket_); @@ -179,40 +193,51 @@ hardware_interface::CallbackReturn RbsServoActuator::on_deactivate( hardware_interface::return_type RbsServoActuator::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - struct can_frame frame; - if (::read(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) { - RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to read CAN frame"); - } else { - float angle; - std::memcpy(&angle, &frame.data[1], sizeof(float)); - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Received CAN frame: angle=%f rad", angle); - hw_joint_state_ = static_cast(angle); + + // Чтение кадров в цикле + while (::read(can_socket_, &frame, sizeof(frame)) == sizeof(frame)) { + // Проверка флага 'A' + if (frame.data[0] == 'A') { + // Извлекаем угол + float angle; + std::memcpy(&angle, &frame.data[1], sizeof(float)); + + // Логируем полученный угол + RCLCPP_DEBUG(logger_, "Received CAN frame with angle: %f rad", angle); + + // Обновляем состояние сустава + hw_joint_angle_ = static_cast(angle); + return hardware_interface::return_type::OK; + } else { + // Игнорирование кадров с другими флагами + RCLCPP_DEBUG(logger_, "Ignoring CAN frame with unknown flag: %c", + frame.data[0]); + } } + + if (errno != EAGAIN) { + RCLCPP_ERROR(logger_, "Failed to read CAN frame: %s", strerror(errno)); + return hardware_interface::return_type::ERROR; + } + return hardware_interface::return_type::OK; } hardware_interface::return_type rbs_servo_hardware::RbsServoActuator::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // START: This part here is for exemplary purposes - Please do not copy to - // your production code - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Writing command: %f", - hw_joint_command_); struct can_frame frame; - frame.can_id = 0x2; // Replace with your desired CAN ID - - float angle = static_cast(hw_joint_command_); + frame.can_id = can_node_id_; + float angle = static_cast(hw_joint_target_angle_); frame.data[0] = 'A'; - std::memcpy(&frame.data[1], &angle, sizeof(float)); + std::memcpy(&frame.data[1], &angle, sizeof(float)); frame.can_dlc = 5; - // frame.data[speed_string.size() + 1] = '\0' - ; // You'll need to serialize speed to fit CAN frame if (::write(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) { - RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to send CAN frame"); + RCLCPP_ERROR(logger_, "Failed to send CAN frame"); } else { - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Sent CAN frame: angle=%f rad", angle); + RCLCPP_DEBUG(logger_, "Sent CAN frame: angle=%f rad", angle); } return hardware_interface::return_type::OK; diff --git a/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf b/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf index 5e9c1bc..a46738a 100644 --- a/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf +++ b/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf @@ -41,6 +41,8 @@ rbs_servo_hardware/RbsServoActuator + can0 + 1 From 631cadca269ddeef919bb4391f5b7832ebe4583f Mon Sep 17 00:00:00 2001 From: sosiskovich Date: Wed, 12 Feb 2025 21:57:27 +0800 Subject: [PATCH 8/9] New PID coefficients; remove legacy --- controller/fw/embed/src/main.cpp | 58 +++++++-------- .../fw/embed/test/python_send_velocity.py | 70 +++++++++++++++---- 2 files changed, 81 insertions(+), 47 deletions(-) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 36d46d5..c7640d8 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -48,40 +48,37 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, DRV8313Driver *driver, LowsideCurrentSense *current_sense, Commander *commander, CommandCallback callback) { encoder->init(&spi); - motor->linkSensor(encoder); + driver->pwm_frequency = 20000; - driver->voltage_power_supply = 20; - driver->voltage_limit = 40; - driver->init(); - motor->linkDriver(driver); + driver->voltage_power_supply = 24; + driver->voltage_limit = 24; + driver->init(); + current_sense->linkDriver(driver); current_sense->init(); + + motor->linkSensor(encoder); + motor->linkDriver(driver); motor->linkCurrentSense(current_sense); - // SimpleFOCDebug::enable(nullptr); - commander->add('M', callback, "motor"); motor->useMonitoring(Serial); - // motor->monitor_start_char = 'M'; - // motor->monitor_end_char = 'M'; - // motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; - motor->monitor_downsample = 10; // default 10 - // commander->verbose = VerboseMode::user_friendly; + motor->monitor_downsample = 5000; // default 0 motor->controller = MotionControlType::angle; motor->torque_controller = TorqueControlType::voltage; - motor->foc_modulation = FOCModulationType::SinePWM; - motor->PID_velocity.P = 0.3; - motor->PID_velocity.I = 15; - motor->PID_velocity.D = 0.001; - motor->PID_velocity.output_ramp = 1000; - motor->LPF_velocity.Tf = 0.005; - motor->LPF_angle.Tf = 0.005; - motor->P_angle.P = 15; - // motor->P_angle.I = 15; - // motor->P_angle.3 = 15; - motor->velocity_limit = 40; - motor->voltage_limit = 40; + motor->foc_modulation = FOCModulationType::SpaceVectorPWM; + + //PID start + motor->PID_velocity.P = 0.75; + motor->PID_velocity.I = 20; + motor->LPF_velocity.Tf = 0.005; + motor->P_angle.P = 0.5 ; + motor->LPF_angle.Tf = 0.001; + //PID end + + motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm) + motor->voltage_limit = 24; motor->current_limit = 0.5; + motor->sensor_direction = Direction::CCW; - current_sense->skip_align = false; motor->init(); motor->initFOC(); } @@ -113,7 +110,6 @@ void send_motor_enabled() { } void send_data() { - // TODO(vanyabeat) add can functionability for ROS packets send_velocity(); send_angle(); send_motor_enabled(); @@ -124,9 +120,11 @@ void send_data() { void read_can_step() { char flag = CAN_inMsg.buf[0]; if (flag == 'V') { + motor.enable(); memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1], sizeof(motor_control_inputs.target_velocity)); } else if (flag == 'A') { + motor.enable(); memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1], sizeof(motor_control_inputs.target_angle)); } else if (flag == 'E') { @@ -141,7 +139,6 @@ void read_can_step() { motor.disable(); } } - // motor.target = angle; digitalWrite(PC10, !digitalRead(PC10)); } @@ -152,6 +149,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) { motor->controller = MotionControlType::velocity; } motor->target = motor_control_inputs.target_velocity; + } else { if (motor->controller != MotionControlType::angle) { motor->controller = MotionControlType::angle; @@ -168,7 +166,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) { void setup() { Serial.setRx(HARDWARE_SERIAL_RX_PIN); Serial.setTx(HARDWARE_SERIAL_TX_PIN); - Serial.begin(19200); + Serial.begin(115200); // setup led pin pinMode(PC11, OUTPUT); pinMode(PC10, OUTPUT); @@ -182,14 +180,10 @@ void setup() { SendTimer->attachInterrupt(send_data); SendTimer->resume(); setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); - // _delay(1000); } -// float current_angle = 0.0; void loop() { foc_step(&motor, &command); - // current_angle = encoder.getAngle(); - // memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); while (Can.read(CAN_inMsg)) { read_can_step(); } diff --git a/controller/fw/embed/test/python_send_velocity.py b/controller/fw/embed/test/python_send_velocity.py index e01f24f..dd46c6c 100644 --- a/controller/fw/embed/test/python_send_velocity.py +++ b/controller/fw/embed/test/python_send_velocity.py @@ -1,6 +1,7 @@ import can import struct -import argparse +import time +import sys # Function to send the target speed def send_target_speed(bus, target_speed): @@ -8,29 +9,68 @@ def send_target_speed(bus, target_speed): msg.arbitration_id = 1 # Message ID msg.is_extended_id = False msg.dlc = 5 # Message length - msg.data = [ord('V')] + list(struct.pack(' Date: Mon, 17 Feb 2025 18:53:37 +0300 Subject: [PATCH 9/9] fix cpplint errors fix cpplint again --- controller/fw/embed/src/main.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index c7640d8..b342add 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -52,7 +52,7 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, driver->pwm_frequency = 20000; driver->voltage_power_supply = 24; driver->voltage_limit = 24; - driver->init(); + driver->init(); current_sense->linkDriver(driver); current_sense->init(); @@ -61,23 +61,23 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, motor->linkDriver(driver); motor->linkCurrentSense(current_sense); motor->useMonitoring(Serial); - motor->monitor_downsample = 5000; // default 0 + motor->monitor_downsample = 5000; // default 0 motor->controller = MotionControlType::angle; motor->torque_controller = TorqueControlType::voltage; motor->foc_modulation = FOCModulationType::SpaceVectorPWM; - //PID start + // PID start motor->PID_velocity.P = 0.75; motor->PID_velocity.I = 20; - motor->LPF_velocity.Tf = 0.005; - motor->P_angle.P = 0.5 ; - motor->LPF_angle.Tf = 0.001; - //PID end + motor->LPF_velocity.Tf = 0.005; + motor->P_angle.P = 0.5; + motor->LPF_angle.Tf = 0.001; + // PID end - motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm) + motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm) motor->voltage_limit = 24; motor->current_limit = 0.5; - + motor->sensor_direction = Direction::CCW; motor->init(); motor->initFOC(); @@ -176,7 +176,7 @@ void setup() { Can.setBaudRate(1000000); TIM_TypeDef *Instance = TIM2; HardwareTimer *SendTimer = new HardwareTimer(Instance); - SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz + SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz SendTimer->attachInterrupt(send_data); SendTimer->resume(); setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor);