fix: typo in header name and change buf0 flags and fix tests
This commit is contained in:
parent
ffd778ab69
commit
54c8296622
5 changed files with 149 additions and 61 deletions
|
|
@ -1,5 +1,5 @@
|
|||
#ifndef REG_CAH_H_
|
||||
#define REG_CAH_H_
|
||||
#ifndef REG_CAN_H_
|
||||
#define REG_CAN_H_
|
||||
|
||||
#define APP_ADDR 0x0800400 // 16KB - Application
|
||||
#define ADDR_VAR 0x8040000
|
||||
|
|
@ -36,5 +36,6 @@
|
|||
#define MOTOR_ENABLED 0x71
|
||||
#define MOTOR_ANGLE 0x72
|
||||
#define MOTOR_TORQUE 0x73
|
||||
#define MOTOR_ID 0x74
|
||||
|
||||
#endif // REG_CAH_H_
|
||||
#endif // REG_CAN_H_
|
||||
|
|
@ -1,18 +1,16 @@
|
|||
// clang-format off
|
||||
#include "Arduino.h"
|
||||
#include "reg_can.h"
|
||||
#include "stm32f446xx.h"
|
||||
#include <SimpleFOC.h>
|
||||
#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
|
||||
#include "reg_cah.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() {
|
||||
float current_velocity = motor.shaftVelocity();
|
||||
float current_velocity = motor.shaft_velocity;
|
||||
flash_rec = load_params();
|
||||
if (flash_rec == nullptr) { // Проверка на NULL
|
||||
// Обработка ошибки: запись в лог, сигнализация и т.д.
|
||||
|
|
@ -153,11 +151,11 @@ void send_velocity() {
|
|||
}
|
||||
uint8_t value = flash_rec[vel].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() {
|
||||
float current_angle = motor.shaftAngle();
|
||||
float current_angle = motor.shaft_angle;
|
||||
|
||||
flash_rec = load_params();
|
||||
if (flash_rec == nullptr) { // Проверка на NULL
|
||||
|
|
@ -166,13 +164,13 @@ void send_angle() {
|
|||
}
|
||||
// uint8_t value = flash_rec[angl].value;
|
||||
uint8_t id = flash_rec[addr_id].value;
|
||||
send_can_with_id_crc(id,'A',¤t_angle,sizeof(current_angle));
|
||||
send_can_with_id_crc(id, MOTOR_ANGLE, ¤t_angle,sizeof(current_angle));
|
||||
}
|
||||
|
||||
void send_motor_enabled() {
|
||||
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
||||
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,
|
||||
sizeof(motor_control_inputs.motor_enabled));
|
||||
Can.write(CAN_TX_msg);
|
||||
|
|
@ -188,7 +186,7 @@ void send_foc_state() {
|
|||
|
||||
uint8_t value = flash_rec[foc_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() {
|
||||
|
|
@ -199,7 +197,7 @@ void send_id() {
|
|||
return;
|
||||
}
|
||||
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();
|
||||
}
|
||||
|
||||
|
|
@ -209,7 +207,7 @@ void send_motor_torque() {
|
|||
torque *= 100;
|
||||
flash_rec = load_params();
|
||||
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;
|
||||
memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
|
||||
Can.write(CAN_TX_msg);
|
||||
|
|
@ -247,9 +245,12 @@ void setup_id(uint8_t my_id) {
|
|||
}
|
||||
|
||||
void setup_angle(float target_angle) {
|
||||
// float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы
|
||||
motor.enable(); // Включаем мотор если он отключен
|
||||
motor.controller = MotionControlType::angle;
|
||||
if (!motor_control_inputs.motor_enabled) {
|
||||
motor.enable();
|
||||
}
|
||||
if (motor.controller != MotionControlType::angle) {
|
||||
motor.controller = MotionControlType::angle;
|
||||
}
|
||||
motor.move(target_angle);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -2,29 +2,68 @@ 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])
|
||||
REG_READ = 0x7
|
||||
DEVICE_ID = 0x00 # Старый ID устройства
|
||||
REG_POS = 0x72 # Предположим, что команда записи позиции — 0x2
|
||||
CAN_INTERFACE = 'can0'
|
||||
|
||||
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 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 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():
|
||||
try:
|
||||
|
|
@ -34,9 +73,26 @@ def receive_can_messages():
|
|||
print("Waiting for messages on the CAN bus...")
|
||||
|
||||
while True:
|
||||
msg = bus.recv()
|
||||
if msg:
|
||||
process_can_message(msg)
|
||||
process_can_message(bus)
|
||||
response = receive_response(bus)
|
||||
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:
|
||||
print("\nExiting program...")
|
||||
|
|
|
|||
|
|
@ -3,35 +3,65 @@ import struct
|
|||
import time
|
||||
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):
|
||||
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
|
||||
# ID и команда
|
||||
arbitration_id = (DEVICE_ID << 4) | REG_WRITE
|
||||
id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
|
||||
|
||||
# Команда и параметры
|
||||
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:
|
||||
bus.send(msg)
|
||||
print(f"Sent message with target angle: {target_angle} degrees")
|
||||
print(f"Message data: {msg.data}")
|
||||
print(f"[Отправка] CAN ID: 0x{arbitration_id:03X}, Угол: {target_angle} rad, Данные: {list(msg.data)}")
|
||||
except can.CanError:
|
||||
print("Message failed to send")
|
||||
print("Ошибка отправки сообщения")
|
||||
|
||||
# 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")
|
||||
parser = argparse.ArgumentParser(description="Отправка угла позиции по CAN.")
|
||||
parser.add_argument("--angle", type=float, required=True, help="Угол (в градусах)")
|
||||
args = parser.parse_args()
|
||||
|
||||
target_angle = args.angle
|
||||
# Инициализация CAN
|
||||
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
||||
print("CAN шина инициализирована.")
|
||||
|
||||
# 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...")
|
||||
send_target_angle(bus, args.angle)
|
||||
|
||||
# Loop to send messages
|
||||
send_target_angle(bus, target_angle)
|
||||
bus.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ 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
|
||||
bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
|
||||
print("CAN bus initialized.")
|
||||
|
||||
while True:
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue