Compare commits

...
Sign in to create a new pull request.

2 commits

6 changed files with 154 additions and 61 deletions

5
.gitignore vendored
View file

@ -38,3 +38,8 @@ fp-info-cache
.pio/ .pio/
# JetBrains CLion # JetBrains CLion
.idea/ .idea/
**/build/
**/install/
**/log/
**/.cache/

View file

@ -1,5 +1,5 @@
#ifndef REG_CAH_H_ #ifndef REG_CAN_H_
#define REG_CAH_H_ #define REG_CAN_H_
#define APP_ADDR 0x0800400 // 16KB - Application #define APP_ADDR 0x0800400 // 16KB - Application
#define ADDR_VAR 0x8040000 #define ADDR_VAR 0x8040000
@ -36,5 +36,6 @@
#define MOTOR_ENABLED 0x71 #define MOTOR_ENABLED 0x71
#define MOTOR_ANGLE 0x72 #define MOTOR_ANGLE 0x72
#define MOTOR_TORQUE 0x73 #define MOTOR_TORQUE 0x73
#define MOTOR_ID 0x74
#endif // REG_CAH_H_ #endif // REG_CAN_H_

View file

@ -1,18 +1,16 @@
// clang-format off // clang-format off
#include "Arduino.h" #include "Arduino.h"
#include "reg_can.h"
#include "stm32f446xx.h" #include "stm32f446xx.h"
#include <SimpleFOC.h> #include <SimpleFOC.h>
#include <STM32_CAN.h> #include <STM32_CAN.h>
#include <AS5045.h> #include <AS5045.h>
#include <DRV8313.h> #include <DRV8313.h>
#include <cstring> #include <cstring>
#include <iterator>
#include "common/base_classes/FOCMotor.h" #include "common/base_classes/FOCMotor.h"
#include "hal_conf_extra.h" #include "hal_conf_extra.h"
#include "wiring_analog.h"
#include "wiring_constants.h" #include "wiring_constants.h"
// clang-format on // clang-format on
#include "reg_cah.h"
#include "flash.h" #include "flash.h"
@ -145,7 +143,7 @@ void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, s
void send_velocity() { void send_velocity() {
float current_velocity = motor.shaftVelocity(); float current_velocity = motor.shaft_velocity;
flash_rec = load_params(); flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL if (flash_rec == nullptr) { // Проверка на NULL
// Обработка ошибки: запись в лог, сигнализация и т.д. // Обработка ошибки: запись в лог, сигнализация и т.д.
@ -153,11 +151,11 @@ void send_velocity() {
} }
uint8_t value = flash_rec[vel].value; uint8_t value = flash_rec[vel].value;
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'V',&value,sizeof(value)); send_can_with_id_crc(id, MOTOR_VELOCITY, &value,sizeof(value));
} }
void send_angle() { void send_angle() {
float current_angle = motor.shaftAngle(); float current_angle = motor.shaft_angle;
flash_rec = load_params(); flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL if (flash_rec == nullptr) { // Проверка на NULL
@ -166,13 +164,13 @@ void send_angle() {
} }
// uint8_t value = flash_rec[angl].value; // uint8_t value = flash_rec[angl].value;
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'A',&current_angle,sizeof(current_angle)); send_can_with_id_crc(id, MOTOR_ANGLE, &current_angle,sizeof(current_angle));
} }
void send_motor_enabled() { void send_motor_enabled() {
uint8_t id = *(volatile uint8_t*)ADDR_VAR; uint8_t id = *(volatile uint8_t*)ADDR_VAR;
CAN_TX_msg.id = id; CAN_TX_msg.id = id;
CAN_TX_msg.buf[0] = 'E'; CAN_TX_msg.buf[0] = MOTOR_ENABLED;
memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled, memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled,
sizeof(motor_control_inputs.motor_enabled)); sizeof(motor_control_inputs.motor_enabled));
Can.write(CAN_TX_msg); Can.write(CAN_TX_msg);
@ -188,7 +186,7 @@ void send_foc_state() {
uint8_t value = flash_rec[foc_id].value; uint8_t value = flash_rec[foc_id].value;
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'F',&value,sizeof(value)); send_can_with_id_crc(id, FOC_STATE, &value,sizeof(value));
} }
void send_id() { void send_id() {
@ -199,7 +197,7 @@ void send_id() {
return; return;
} }
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'I',&id,sizeof(id)); send_can_with_id_crc(id, MOTOR_ID, &id,sizeof(id));
__NOP(); __NOP();
} }
@ -209,7 +207,7 @@ void send_motor_torque() {
torque *= 100; torque *= 100;
flash_rec = load_params(); flash_rec = load_params();
CAN_TX_msg.id = flash_rec->value; CAN_TX_msg.id = flash_rec->value;
CAN_TX_msg.buf[0] = 'T'; CAN_TX_msg.buf[0] = MOTOR_TORQUE;
CAN_TX_msg.len = 5; CAN_TX_msg.len = 5;
memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque)); memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
Can.write(CAN_TX_msg); Can.write(CAN_TX_msg);
@ -247,9 +245,12 @@ void setup_id(uint8_t my_id) {
} }
void setup_angle(float target_angle) { void setup_angle(float target_angle) {
// float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы if (!motor_control_inputs.motor_enabled) {
motor.enable(); // Включаем мотор если он отключен motor.enable();
motor.controller = MotionControlType::angle; }
if (motor.controller != MotionControlType::angle) {
motor.controller = MotionControlType::angle;
}
motor.move(target_angle); motor.move(target_angle);
} }

View file

@ -2,29 +2,68 @@ import can
import struct import struct
import time 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) REG_READ = 0x7
flag = chr(msg.data[0]) DEVICE_ID = 0x00 # Старый ID устройства
REG_POS = 0x72 # Предположим, что команда записи позиции — 0x2
if flag == 'A': # Angle CAN_INTERFACE = 'can0'
angle_bytes = msg.data[1:5]
angle = struct.unpack('<f', bytes(angle_bytes))[0]
print(f"Angle: {angle} degrees") def validate_crc16(data):
elif flag == 'V': # Velocity """Функция расчета CRC16 (MODBUS)"""
velocity_bytes = msg.data[1:5] crc = 0xFFFF
velocity = struct.unpack('<f', bytes(velocity_bytes))[0] for byte in data:
print(f"Velocity: {velocity} rad/s") crc ^= byte
elif flag == 'E' and msg.dlc >= 2: # Enable/Disable for _ in range(8):
enabled = msg.data[1] # Expecting 1 byte (0 or 1) if crc & 0x0001:
print(f"Enabled: {bool(enabled)}") crc = (crc >> 1) ^ 0xA001
else: else:
print(f"Unknown flag: {flag}") crc >>= 1
else: return crc
print(f"Received message with unexpected length: {msg.dlc}")
def process_can_message(bus):
# ID и команда
arbitration_id = (DEVICE_ID << 4) | REG_READ
id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
# Команда и параметры
data_read = [REG_POS] + list(struct.pack('<f', 0))
# Для CRC — весь пакет: id + команда + параметры
full_data_for_crc = id_bytes + data_read
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
# Итоговый пакет
packet = data_read + crc_bytes
# Создание и отправка сообщения
msg = can.Message(
arbitration_id=arbitration_id,
is_extended_id=False,
data=packet
)
try:
bus.send(msg)
except can.CanError:
print("Ошибка отправки сообщения")
def receive_response(bus, timeout=2.0):
"""Ожидание ответа"""
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=timeout)
data_bytes = msg.data[1:5] # берём первые 4 байта
print(data_bytes)
angle = struct.unpack('<f', bytes(data_bytes))[0] # '<f' = little-endian float
# angle = int.from_bytes(data_bytes, byteorder='little', signed=True) # или 'big'
if msg:
print(f"[Прием] Angle: 0x{msg.arbitration_id:03X}, Данные: {angle}, RAW: {bytes(msg.data)}")
return msg
print("[Ошибка] Таймаут")
return None
def receive_can_messages(): def receive_can_messages():
try: try:
@ -34,9 +73,26 @@ def receive_can_messages():
print("Waiting for messages on the CAN bus...") print("Waiting for messages on the CAN bus...")
while True: while True:
msg = bus.recv() process_can_message(bus)
if msg: response = receive_response(bus)
process_can_message(msg) if response:
data = response.data
if len(data) < 4:
print("Ответ слишком короткий")
else:
received_crc = int.from_bytes(data[-2:], byteorder='little')
print("Полученный CRC: ", received_crc)
# Расчет CRC по всему пакету без CRC
calc_crc = validate_crc16(data[:-2])
if received_crc == calc_crc:
if data[0] == REG_POS:
print("\nУСПЕХ!")
else:
print(f"Некорректный ответ: {list(data)}")
else:
print("CRC не совпадает, данные повреждены.")
else:
print("Нет ответа от устройства.")
except KeyboardInterrupt: except KeyboardInterrupt:
print("\nExiting program...") print("\nExiting program...")

View file

@ -3,35 +3,65 @@ import struct
import time import time
import argparse import argparse
# Function to send the target angle # Константы
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x00 # Старый ID устройства
REG_WRITE = 0x8
REG_POS = 0x72 # Предположим, что команда записи позиции — 0x2
def validate_crc16(data):
"""Функция расчета CRC16 (MODBUS)"""
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_target_angle(bus, target_angle): def send_target_angle(bus, target_angle):
msg = can.Message() # ID и команда
msg.arbitration_id = 1 # Message ID arbitration_id = (DEVICE_ID << 4) | REG_WRITE
msg.is_extended_id = False id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
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 # Команда и параметры
data_write = [REG_POS] + list(struct.pack('<f', target_angle)) # Позиция как float (4 байта)
# Для CRC — весь пакет: id + команда + параметры
full_data_for_crc = id_bytes + data_write
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
# Итоговый пакет
packet = data_write + crc_bytes
# Создание и отправка сообщения
msg = can.Message(
arbitration_id=arbitration_id,
is_extended_id=False,
data=packet
)
try: try:
bus.send(msg) bus.send(msg)
print(f"Sent message with target angle: {target_angle} degrees") print(f"[Отправка] CAN ID: 0x{arbitration_id:03X}, Угол: {target_angle} rad, Данные: {list(msg.data)}")
print(f"Message data: {msg.data}")
except can.CanError: except can.CanError:
print("Message failed to send") print("Ошибка отправки сообщения")
# Main function
def main(): def main():
parser = argparse.ArgumentParser(description="Send target angles over CAN bus.") parser = argparse.ArgumentParser(description="Отправка угла позиции по CAN.")
parser.add_argument("--angle", type=float, required=True, help="Target angle to send over the CAN bus") parser.add_argument("--angle", type=float, required=True, help="Угол (в градусах)")
args = parser.parse_args() args = parser.parse_args()
target_angle = args.angle # Инициализация CAN
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
print("CAN шина инициализирована.")
# CAN interface setup send_target_angle(bus, args.angle)
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 bus.shutdown()
send_target_angle(bus, target_angle)
if __name__ == '__main__': if __name__ == '__main__':
main() main()

View file

@ -45,7 +45,7 @@ def main():
# CAN interface setup # CAN interface setup
bus = None # Define outside the try block for proper shutdown bus = None # Define outside the try block for proper shutdown
try: try:
bus = can.interface.Bus(channel='COM4', bustype='slcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
print("CAN bus initialized.") print("CAN bus initialized.")
while True: while True: