robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

WIP: process_can #89

Draft
lulko wants to merge 4 commits from process_can into main
pull from: process_can
merge into: robossembler:main
Conversation 4 Commits 4 Files changed 8 +353 -115
8 changed files with 353 additions and 115 deletions
Download patch file Download diff file Expand all files Collapse all files

3
controller/fw/embed/include/flash.h
View file

@ -22,7 +22,8 @@ enum {
firmw,
foc_id,
angl,
vel
vel,
torq
};
/* for saved in FLASH float data*/

13
controller/fw/embed/include/process_can.h
View file

@ -4,13 +4,19 @@
#include "flash.h"
#include "reg_cah.h"
#define CAN_TIMEOUT 50
#define CAN_SILENCE_TIMEOUT 500
#define MAX_CAN_READS 2
extern FLASH_RECORD *flash_rec;
extern volatile uint16_t msg_id;
extern volatile uint16_t id_x;
extern volatile uint8_t msg_ch;
extern volatile uint8_t crc_h;
extern volatile uint8_t crc_l;
extern volatile bool send_blocked;
static volatile bool need_send_angle = false;
static volatile bool need_send_velocity = false;
void send_velocity();
void send_angle();
@ -19,9 +25,12 @@ void send_motor_enabled();
void send_id();
void firmware_update();
void send_pid_angle(uint8_t param_pid);
// void send_motor_torque();
void send_with_confirmation(void (*send_func)(void));
// void send_torque();
void send_pid(uint8_t param_pid);
void setup_id(uint8_t my_id);
void setup_angle(float target_angle);
void setup_pid_angle(uint8_t param_pid, float data);
void setup_velocity(float target_velocity);
void process_can_messages();
void listen_can(const CAN_message_t &msg);

17
controller/fw/embed/include/reg_cah.h
View file

@ -1,5 +1,11 @@
#ifndef REG_CAH_H_
#define REG_CAH_H_
#pragma once
enum{
error_foc = 0,
error_canRX,
error_canTX
};
#define APP_ADDR 0x0800400 // 16KB - Application
#define ADDR_VAR 0x8040000
@ -16,6 +22,10 @@
#define REG_ID 0x01
#define REG_BAUDRATE 0x02
#define DATA_TYPE_ANGLE 0x03
#define DATA_TYPE_VELOCITY 0x04
#define DATA_TYPE_TORQUE 0x05
#define REG_MOTOR_POSPID_Kp 0x30
#define REG_MOTOR_POSPID_Ki 0x31
#define REG_MOTOR_POSPID_Kd 0x32
@ -43,6 +53,3 @@
#define CAN_MSG_MAX_LEN 7
#define CRC_SIZE 2
#define ID_SIZE sizeof(uint8_t)
#endif // REG_CAH_H_

37
controller/fw/embed/src/main.cpp
View file

@ -18,6 +18,7 @@
#include "config.h"
#include "process_can.h"
void SysTick_Handler(void) {
HAL_IncTick();
}
@ -29,7 +30,7 @@ uint8_t flag_can = 0;
uint32_t flash_error;
FLASH_EraseInitTypeDef pEraseInit;
uint32_t SectorError;
uint32_t timeout;
/* bool for test CAN */
volatile bool CAN_GET = false;
@ -94,22 +95,22 @@ MotorControlInputs motor_control_inputs;
}
void loop() {
__enable_irq();
foc_step(&motor);
CAN_message_t msg;
// Process incoming CAN messages
while (Can.read(msg)) {
listen_can(msg);
CAN_GET = true;
}
/* If receive data from CAN */
if(CAN_GET) {
CAN_GET = false;
}
bool is_can_busy() {
return (CAN2->TSR & (CAN_TSR_TME0 | CAN_TSR_TME1 | CAN_TSR_TME2)) != 0x07;
}
void loop() {
process_can_messages();
static uint32_t last_send = 0;
if(!send_blocked && !is_can_busy() && (HAL_GetTick() - last_send >= 50)) {
send_angle();
send_velocity();
last_send = HAL_GetTick();
}
foc_step(&motor);
HAL_Delay(1);
}

150
controller/fw/embed/src/process_can.cpp
View file

@ -3,8 +3,8 @@
static CAN_message_t CAN_TX_msg;
static CAN_message_t CAN_inMsg;
static uint8_t data_type = DATA_TYPE_ANGLE;
volatile bool send_blocked = false;
template <typename T>
void send_can_with_id_crc(uint8_t id, uint8_t message_type, T* data) {
// Create CAN message
@ -40,9 +40,9 @@ void send_velocity() {
// Error handling: logging, alerts, etc.
return;
}
float value = flash_rec[vel].value;
// float value = flash_rec[vel].value;
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'V',&value);
send_can_with_id_crc(id,'V',&current_velocity);
}
void send_angle() {
@ -67,6 +67,15 @@ void send_motor_enabled() {
send_can_with_id_crc(id,'M',&value);
}
// void send_torque() {
// float i_q = motor.current.q; // Q-axis current (A)
// float torque = 100 * i_q; // Torque calculation
// if (flash_rec == nullptr) return;
// uint8_t id = flash_rec[addr_id].value;
// send_can_with_id_crc(id, 'T', &torque);
// }
void send_id() {
/* Firmware data reading */
if (flash_rec == nullptr) { // Null check
@ -78,16 +87,10 @@ void send_id() {
send_can_with_id_crc(id,'I',&id);
}
// void send_motor_torque() {
// float i_q = motor.current.q; // Q-axis current (A)
// float torque = kt * i_q; // Torque calculation
// torque *= 100;
// CAN_TX_msg.id = flash_rec->value;
// CAN_TX_msg.buf[0] = 'T';
// CAN_TX_msg.len = 5;
// memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
// Can.write(CAN_TX_msg);
// }
void send_error(uint8_t error_code){
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'P',&error_code);
}
void send_pid_angle(uint8_t param_pid){
if (flash_rec == nullptr) { // Null check
@ -129,31 +132,68 @@ void setup_angle(float target_angle) {
// motor.move(target_angle);
}
// void setup_pid_angle(uint8_t param_pid, uint32_t data){
// conv_float_to_int.f = data;
// switch (param_pid) {
// case pid_p:
// motor.P_angle.P = conv_float_to_int.f;
// break;
// case pid_i:
// motor.P_angle.I = conv_float_to_int.f;
// break;
// case pid_d:
// motor.P_angle.D = conv_float_to_int.f;
// break;
// default:
// break;
// }
/**
* @brief Set the up velocity object
*
* @param target_velocity
*/
void setup_velocity(float target_velocity) {
motor.enable();
motor_control_inputs.target_velocity = target_velocity;
}
// write_param(param_pid,data);
// }
void send_data_type(uint8_t type_d){
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'D',&type_d);
}
// Вспомогательные функции
void send_with_confirmation(void (*send_func)(void)) {
uint32_t t_start = HAL_GetTick();
send_func();
// Ожидание подтверждения отправки
while (!(CAN2->TSR & (CAN_TSR_RQCP0 | CAN_TSR_RQCP1 | CAN_TSR_RQCP2))) {
if (HAL_GetTick() - t_start > CAN_TIMEOUT) break;
}
}
void process_can_messages() {
static uint32_t last_received = HAL_GetTick();
CAN_message_t msg;
uint8_t count = 0;
// block send while data cant
send_blocked = true;
// Чтение сообщений с ограничением
while(count < MAX_CAN_READS && Can.read(msg)) {
listen_can(msg);
last_received = HAL_GetTick();
count++;
}
// Проверка таймаута молчания
if(HAL_GetTick() - last_received > CAN_SILENCE_TIMEOUT) {
// Разблокируем отправку при отсутствии сообщений
send_blocked = false;
}
}
/**
* @brief Function for process data from CAN
* @details Function check your ID deviceю. Compare receive and calculated CRC.
* If ID && CRC == TRUE, then process message or dont send data.
* If if and crc = true:
* Check CAN_REG
* Gives your data type
* Write with data or send data
* @param msg
*/
void listen_can(const CAN_message_t &msg) {
msg_id = msg.id;
msg_ch = msg_id & 0xF; // Extract message channel
uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address
/* CRC Calculation */
uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
uint8_t data[10] = {0}; // Message buffer for CRC verification
@ -171,6 +211,7 @@ void listen_can(const CAN_message_t &msg) {
return; // Ignore message on CRC mismatch
}
flash_rec = load_params();
/* Message Structure: 0x691
69 - Device address
1 - Action code */
@ -211,6 +252,18 @@ void listen_can(const CAN_message_t &msg) {
write_param(pid_d,conv_float_to_int.i);
break;
case DATA_TYPE_ANGLE:
data_type = DATA_TYPE_ANGLE;
break;
case DATA_TYPE_VELOCITY:
data_type = DATA_TYPE_VELOCITY;
break;
case DATA_TYPE_TORQUE:
data_type = DATA_TYPE_TORQUE;
break;
case FIRMWARE_UPDATE:
firmware_update();
break;
@ -233,6 +286,9 @@ void listen_can(const CAN_message_t &msg) {
case MOTOR_VELOCITY: send_velocity(); break;
case MOTOR_ANGLE: send_angle(); break;
case MOTOR_ENABLED: send_motor_enabled(); break;
case DATA_TYPE_ANGLE: send_data_type(uint8_t(DATA_TYPE_ANGLE)); break;
case DATA_TYPE_VELOCITY: send_data_type(uint8_t(DATA_TYPE_VELOCITY)); break;
case DATA_TYPE_TORQUE: send_data_type(uint8_t(DATA_TYPE_TORQUE)); break;
// case MOTOR_TORQUE: send_motor_torque(); break;
// case FOC_STATE: send_foc_state(); break;
case REG_MOTOR_POSPID_Kp: send_pid_angle(pid_p); break;
@ -241,5 +297,35 @@ void listen_can(const CAN_message_t &msg) {
default: break;
}
}
/* If msg_ch != REG_WRITE or REG_READ, then SimpleFOC*/
else{
switch(data_type) {
/* Read after write*/
case DATA_TYPE_ANGLE:
send_angle();
delay(200);
memcpy(&motor_control_inputs.target_angle, &msg.buf[1], sizeof(float));
setup_angle(motor_control_inputs.target_angle);
break;
case DATA_TYPE_VELOCITY:{
send_velocity();
float vel = 0.0f;
memcpy(&vel, &msg.buf[1], sizeof(float));
setup_velocity(vel);
break;
}
case DATA_TYPE_TORQUE:
// send_torque();
break;
default:
send_error(error_foc);
break;
}
}
}
}

128
controller/fw/embed/test/read_id.py
View file

@ -1,12 +1,23 @@
import can
import time
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства (по умолчанию)
OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства
REG_READ = 0x7 # Код команды чтения
REG_ID = 0x01 # Адрес регистра с ID устройства
def flush_can_buffer(bus, duration=0.3):
"""Очистка входного буфера CAN"""
start_time = time.time()
flushed_count = 0
while time.time() - start_time < duration:
msg = bus.recv(timeout=0)
if msg:
flushed_count += 1
print(f"Очищено сообщений из буфера: {flushed_count}")
def send_can_message(bus, can_id, data):
"""Отправка CAN-сообщения"""
try:
@ -23,14 +34,14 @@ def send_can_message(bus, can_id, data):
return False
def receive_response(bus, timeout=1.0):
"""Ожидание ответа от устройства"""
"""Ожидание ответа от устройства (сохраняем вашу оригинальную логику)"""
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1)
if msg:
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
return msg
print("[Ошибка] Таймаут")
print("[Ошибка] Таймаут приема")
return None
def validate_crc16(data):
@ -45,64 +56,91 @@ def validate_crc16(data):
crc >>= 1
return crc
# Инициализация CAN-интерфейса
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
def main():
# Инициализация CAN-интерфейса
try:
bus = can.interface.Bus(
channel=CAN_INTERFACE,
bustype='socketcan',
bitrate=1000000 # Совпадает с устройством
)
except Exception as e:
print(f"Ошибка инициализации CAN: {e}")
sys.exit(1)
# ======= 1. Запрос текущего ID устройства =======
# ======= 1. Подготовка запроса =======
can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
data_read = [REG_ID, 0x00]
# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
# Формируем полные данные для расчета CRC:
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
# Данные для запроса: [регистр, резервный байт]
data_read = [REG_ID, 0x00]
# Рассчитываем CRC
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))
# Формируем полные данные для расчета CRC:
# - CAN ID разбивается на 2 байта (little-endian)
# - Добавляем данные запроса
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
# Собираем итоговый пакет
packet_read = data_read + crc_bytes
# Рассчитываем CRC и разбиваем на байты (little-endian)
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))
# ======= 2. Отправка запроса с повторами =======
max_retries = 3
response = None
# Собираем итоговый пакет: данные + CRC
packet_read = data_read + crc_bytes
for attempt in range(max_retries):
print(f"\nПопытка {attempt+1}/{max_retries}")
print("Запрос на чтение ID:", packet_read)
send_can_message(bus, can_id_read, packet_read)
# Очистка буфера перед отправкой
flush_can_buffer(bus, 0.3)
# Отправка запроса
print(f"Отправка запроса на чтение ID: {packet_read}")
if not send_can_message(bus, can_id_read, packet_read):
print("Ошибка отправки, повтор...")
time.sleep(0.2)
continue
# Ожидание ответа
response = receive_response(bus, timeout=0.5)
if response:
break
print("Ответ не получен, повтор...")
time.sleep(0.2)
# ======= 3. Обработка ответа =======
if not response:
print("Устройство не ответило после всех попыток")
bus.shutdown()
sys.exit(1)
# ======= 2. Получение и проверка ответа =======
response = receive_response(bus)
if response:
data = response.data
if len(data) < 4:
print("Слишком короткий ответ")
bus.shutdown()
sys.exit(1)
# Проверяем минимальную длину ответа (данные + CRC)
id_bytes = response.arbitration_id.to_bytes(1, byteorder='little')
full_data = list(id_bytes) + list(data[:-2])
print(f"Полные данные для CRC: {full_data}")
received_crc = int.from_bytes(data[-2:], byteorder='little')
calc_crc = validate_crc16(full_data)
print(f"Расчитанный CRC: 0x{calc_crc:04X}, Полученный CRC: 0x{received_crc:04X}")
if received_crc == calc_crc:
print(f"Текущий ID устройства: 0x{data[1]:02X}")
else:
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
#buff with id and data without CRC
full_data = list(id_bytes) + list(data[:-2])
print(f"Received full_data: {list(full_data)}")
received_crc = int.from_bytes(data[-2:], byteorder='little')
#calc CRC
calc_crc = validate_crc16(full_data)
print("Ошибка: CRC не совпадает")
print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}")
if received_crc == calc_crc:
# Если CRC совпадает, проверяем структуру ответа:
print(f"Текущий ID устройства: 0x{data[1]:02X}")
else:
print("Ошибка: CRC не совпадает")
else:
print("Устройство не ответило")
# Завершаем работу с шиной
bus.shutdown()
# Завершаем работу с шиной
bus.shutdown()
if __name__ == "__main__":
import sys
if len(sys.argv) != 2:
print("Использование: python3 can_flasher.py address")
print("Использование: python3 can_flasher.py <адрес_устройства>")
print("Пример: python3 can_flasher.py 1")
sys.exit(1)
main()

96
controller/fw/embed/test/test_simpleFOC.py Normal file
View file

@ -0,0 +1,96 @@
import can
import time
import sys
import struct
# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x69 # Текущий ID устройства
REG_READ = 0x7
REG_WRITE = 0x8
DATA_TYPE_ANGLE = 0x03
DATA_TYPE_VELOCITY = 0x04
DATA_TYPE_TORQUE = 0x05
# CRC функция (аналогичная устройству)
def validate_crc16(data):
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_can_message(bus, can_id, data):
try:
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
bus.send(msg)
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
return True
except can.CanError as e:
print(f"Ошибка CAN: {e}")
return False
def receive_response(bus, timeout=1.0):
start_time = time.time()
while time.time() - start_time < timeout:
msg = bus.recv(timeout=0.1)
if msg:
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
return msg
print("[Ошибка] Таймаут")
return None
def test_simplefoc_else_block():
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# 1. Установка типа данных (DATA_TYPE_ANGLE)
can_id_write = (DEVICE_ID << 4) | REG_WRITE
data_set_type = [DATA_TYPE_ANGLE, 0x00]
full_data = list(can_id_write.to_bytes(2, 'little')) + data_set_type
crc = validate_crc16(full_data)
crc_bytes = list(crc.to_bytes(2, 'little'))
packet = data_set_type + crc_bytes
send_can_message(bus, can_id_write, packet)
time.sleep(0.1) # Ожидание обработки
# 2. Отправка SimpleFOC сообщения (угол)
target_angle = 45.0
angle_bytes = struct.pack('<f', target_angle)
can_id_simplefoc = (DEVICE_ID << 4) | 0x01 # Не REG_READ/REG_WRITE
payload = [0x00] + list(angle_bytes) + [0x00] # [type placeholder, angle, padding]
# Расчет CRC
full_data_sf = list(can_id_simplefoc.to_bytes(2, 'little')) + payload
crc_sf = validate_crc16(full_data_sf)
payload += list(crc_sf.to_bytes(2, 'little'))
# Отправка
print("\nТест SimpleFOC (блок else):")
send_can_message(bus, can_id_simplefoc, payload)
# 3. Проверка ответа (отправка угла + установка нового угла)
response = receive_response(bus)
if response:
# Проверка структуры ответа
if response.data[0] == ord('A'):
print("Успех: Отправлен текущий угол")
else:
print("Ошибка: Неверный тип ответа")
else:
print("Ошибка: Нет ответа от устройства")
# 4. Проверка установки нового угла (интеграционно)
# ... (может требовать дополнительной проверки на устройстве)
solid-sinusoid commented 2025-06-08 14:05:55 +03:00
Copy link

Можно добавить тест на изменение режима управленя и отправку скорости. 2 раза подряд угол и 2 раза подряд скорость

Можно добавить тест на изменение режима управленя и отправку скорости. 2 раза подряд угол и 2 раза подряд скорость
bus.shutdown()
if __name__ == "__main__":
test_simplefoc_else_block()