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..a759816 100644 --- a/controller/fw/embed/platformio.ini +++ b/controller/fw/embed/platformio.ini @@ -16,12 +16,13 @@ board = genericSTM32F446RE framework = arduino upload_protocol = stlink debug_tool = stlink -monitor_speed = 115200 +monitor_speed = 19200 monitor_parity = N build_flags = -DSTM32F446xx -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 diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index d446a0d..b342add 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,173 @@ 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 = 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); + 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); + + driver->pwm_frequency = 20000; + 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); + motor->useMonitoring(Serial); + motor->monitor_downsample = 5000; // default 0 + motor->controller = MotionControlType::angle; + motor->torque_controller = TorqueControlType::voltage; + 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; + 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)); + 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') { + 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') { + 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(); + } + } + 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); } -float current_angle = 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); + while (Can.read(CAN_inMsg)) { + read_can_step(); + } } 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 + ``` 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..c491eb7 --- /dev/null +++ b/controller/fw/embed/test/python_enable_motor.py @@ -0,0 +1,54 @@ +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 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.") + + # 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) + + 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 new file mode 100644 index 0000000..dc23d9b --- /dev/null +++ b/controller/fw/embed/test/python_send_angle.py @@ -0,0 +1,37 @@ +import can +import struct +import time +import argparse + +# 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(' +#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