controller/fw/embed/.clang-tidy

@@ -0,0 +1 @@
+Checks: '-*, -misc-definitions-in-headers' 

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,
+    ]

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

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}")

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")

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

controller/fw/embed/src/main.cpp

@@ -4,7 +4,12 @@
 #include <STM32_CAN.h>
 #include <AS5045.h>
 #include <DRV8313.h>
+#include <cstring>
+#include <iterator>
+#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], &current_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], &current_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, &current_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, &current_sense, &command, doMotor);
 }
 
-float current_angle = 0;
 void loop() {
-    run_foc(&motor, &command);
-    current_angle = sensor.getAngle();
-    memcpy(&CAN_TX_msg.buf[1], &current_angle, sizeof(current_angle));
-    while (Can.read(CAN_inMsg)) {
-        read_can_message();
-    }
+  foc_step(&motor, &command);
+  while (Can.read(CAN_inMsg)) {
+    read_can_step();
+  }
 }

controller/fw/embed/test/README

@@ -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

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
+     ```

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('<f', bytes(angle_bytes))[0]
+            print(f"Angle: {angle} degrees")
+        elif flag == 'V':  # Velocity
+            velocity_bytes = msg.data[1:5]
+            velocity = struct.unpack('<f', bytes(velocity_bytes))[0]
+            print(f"Velocity: {velocity} rad/s")
+        elif flag == 'E' and msg.dlc >= 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()

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()

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('<f', target_angle))  # 'A' for the command identifier, followed by the angle in float format
+
+    try:
+        bus.send(msg)
+        print(f"Sent message with target angle: {target_angle} degrees")
+        print(f"Message data: {msg.data}")
+    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...")
+
+    # Loop to send messages
+    send_target_angle(bus, target_angle)
+
+if __name__ == '__main__':
+    main()

controller/fw/embed/test/python_send_velocity.py

@@ -0,0 +1,76 @@
+import can
+import struct
+import time
+import sys
+
+# 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 = bytearray([ord('V')] + list(struct.pack('<f', target_speed)))  # 'V' for the command identifier, followed by the speed in float format
+
+    try:
+        bus.send(msg)
+        print(f"Sent message with target speed: {target_speed} rad/s")
+    except can.CanError:
+        print("Message failed to send")
+
+# 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 = bytearray([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")
+    except can.CanError as e:
+        print(f"Message failed to send: {e}")
+        sys.exit(1)  # Exit the program on failure
+
+    send_target_speed(bus,0.0)
+
+def main():
+    # CAN interface setup
+    bus = None  # Define outside the try block for proper shutdown
+    try:
+        bus = can.interface.Bus(channel='COM4', bustype='slcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
+        print("CAN bus initialized.")
+
+        while True:
+            user_input = input("Enter target speed: ")
+            if user_input.lower() == 'exit':
+                print("Exiting...")
+                break
+            try:
+                target_speed = float(user_input)
+                send_target_speed(bus, target_speed)
+            except ValueError:
+                print("Invalid input. Please enter a valid number.")
+
+        # Disable motor before exiting
+        send_motor_enable(bus, 0)
+        print("Motor disabled.")
+    
+    except Exception as e:
+        print(f"Error initializing1 CAN bus: {e}")
+        sys.exit(1)
+
+    finally:
+        if bus is not None:
+            bus.shutdown()
+            print("CAN bus shut down.")
+
+if __name__ == '__main__':
+    main()

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('<f', target_speed))  # 'V' for the command identifier, followed by the speed in float format
+
+    try:
+        bus.send(msg)
+        print(f"Sent message with target speed: {target_speed} m/s")
+        print(f"Message data: {msg.data}")
+    except can.CanError:
+        print("Message failed to send")
+
+# Main function
+def main():
+    # 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 speed impulses...")
+
+    # Send impulses of target speed from -2 to 2 m/s
+    target_speeds = [-1, 1]
+
+    while True:
+        for speed in target_speeds:
+            send_target_speed(bus, speed)
+            time.sleep(1)  # 1-second delay between messages
+
+if __name__ == '__main__':
+    main()

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 <netinet/in.h>
+#include <rclcpp/logger.hpp>
 #include <sys/socket.h>
 // System libs
 #include <memory>
@@ -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_;
 };
 

ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp

@@ -7,17 +7,19 @@
 #include <cmath>
 #include <limits>
 #include <memory>
+#include <rclcpp_lifecycle/state.hpp>
 #include <vector>
 // 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 <rclcpp/logger.hpp>
 #include <linux/can.h>
 #include <linux/can/raw.h>
-#include <sys/socket.h>
-#include <sys/ioctl.h>
 #include <net/if.h>
+#include <rclcpp/logger.hpp>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
 #include <unistd.h>
 
 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<double>::quiet_NaN();
+  hw_joint_target_angle_ = std::numeric_limits<double>::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<double>(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<double>(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<double>(hw_joint_command_);
+  frame.can_id = can_node_id_;
+  float angle = static_cast<float>(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;

ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf

@@ -41,6 +41,8 @@
   <ros2_control name="simble_robot_hardware_interface" type="actuator">
     <hardware>
       <plugin>rbs_servo_hardware/RbsServoActuator</plugin>
+      <param name="can_interface">can0</param>
+      <param name="can_node_id">1</param>
     </hardware>
     <joint name="joint1">
       <command_interface name="position" />