Compare commits
2 commits
main
...
updated-ha
Author | SHA1 | Date | |
---|---|---|---|
54c8296622 | |||
ffd778ab69 |
6 changed files with 154 additions and 61 deletions
5
.gitignore
vendored
5
.gitignore
vendored
|
@ -38,3 +38,8 @@ fp-info-cache
|
||||||
.pio/
|
.pio/
|
||||||
# JetBrains CLion
|
# JetBrains CLion
|
||||||
.idea/
|
.idea/
|
||||||
|
|
||||||
|
**/build/
|
||||||
|
**/install/
|
||||||
|
**/log/
|
||||||
|
**/.cache/
|
||||||
|
|
|
@ -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_
|
|
@ -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',¤t_angle,sizeof(current_angle));
|
send_can_with_id_crc(id, MOTOR_ANGLE, ¤t_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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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...")
|
||||||
|
|
|
@ -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()
|
||||||
|
|
|
@ -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:
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue