WIP: process_can #89
7 changed files with 205 additions and 42 deletions
|
@ -22,7 +22,8 @@ enum {
|
|||
firmw,
|
||||
foc_id,
|
||||
angl,
|
||||
vel
|
||||
vel,
|
||||
torq
|
||||
};
|
||||
|
||||
/* for saved in FLASH float data*/
|
||||
|
|
|
@ -19,9 +19,10 @@ void send_motor_enabled();
|
|||
void send_id();
|
||||
void firmware_update();
|
||||
void send_pid_angle(uint8_t param_pid);
|
||||
// void send_motor_torque();
|
||||
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 listen_can(const CAN_message_t &msg);
|
||||
void setup_velocity(float target_velocity);
|
||||
|
||||
void listen_can(const CAN_message_t &msg);
|
||||
|
|
|
@ -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_
|
||||
|
|
|
@ -14,10 +14,10 @@ build_flags =
|
|||
-D STM32F446xx
|
||||
-D HAL_CAN_MODULE_ENABLED
|
||||
-D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
|
||||
|
||||
|
||||
lib_deps =
|
||||
askuric/Simple FOC@^2.3.4
|
||||
pazi88/STM32_CAN@^1.1.2
|
||||
pazi88/STM32_CAN@^1.1.2
|
||||
|
||||
extra_scripts =
|
||||
pre:gen_compile_commands.py
|
||||
|
|
|
@ -18,6 +18,7 @@
|
|||
#include "config.h"
|
||||
#include "process_can.h"
|
||||
|
||||
|
||||
void SysTick_Handler(void) {
|
||||
HAL_IncTick();
|
||||
}
|
||||
|
@ -98,9 +99,13 @@ MotorControlInputs motor_control_inputs;
|
|||
|
||||
void loop() {
|
||||
__enable_irq();
|
||||
lulko marked this conversation as resolved
|
||||
send_angle();
|
||||
send_velocity();
|
||||
foc_step(&motor);
|
||||
CAN_message_t msg;
|
||||
|
||||
|
||||
|
||||
// Process incoming CAN messages
|
||||
while (Can.read(msg)) {
|
||||
solid-sinusoid
commented
Может, чтобы не было потенциальных залипаний цикла надо ограничить while цикл? Сделать ограничение на итерации или сделать ограничения на количество обрабатываемых сообщений... Может, чтобы не было потенциальных залипаний цикла надо ограничить while цикл? Сделать ограничение на итерации или сделать ограничения на количество обрабатываемых сообщений...
|
||||
listen_can(msg);
|
||||
|
|
|
@ -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',¤t_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
|
||||
lulko marked this conversation as resolved
solid-sinusoid
commented
Это откуда такая формула? Если функция пока не дописана пометь ее Это откуда такая формула? Если функция пока не дописана пометь ее
// TODO: something
Потом легко в коде ориентироваться
А можно пометить как
// TODO(#45): something
Типа сразу номер issue указываешь, если такой issue есть
|
||||
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,36 @@ 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;
|
||||
// }
|
||||
|
||||
// write_param(param_pid,data);
|
||||
// }
|
||||
/**
|
||||
* @brief Set the up velocity object
|
||||
*
|
||||
* @param target_velocity
|
||||
*/
|
||||
void setup_velocity(float target_velocity) {
|
||||
motor.enable();
|
||||
motor_control_inputs.target_velocity = target_velocity;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* @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
|
||||
|
||||
uint8_t data_type = 1; //type for work with foc
|
||||
/* 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 +179,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 +220,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 +254,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 +265,34 @@ 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();
|
||||
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;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
96
controller/fw/embed/test/test_simpleFOC.py
Normal file
96
controller/fw/embed/test/test_simpleFOC.py
Normal 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
Можно добавить тест на изменение режима управленя и отправку скорости. 2 раза подряд угол и 2 раза подряд скорость Можно добавить тест на изменение режима управленя и отправку скорости. 2 раза подряд угол и 2 раза подряд скорость
|
||||
bus.shutdown()
|
||||
|
||||
if __name__ == "__main__":
|
||||
test_simplefoc_else_block()
|
Loading…
Add table
Add a link
Reference in a new issue
А без этого не обойтись? Обязательно на каждом цикле включать прерывания? Это можно вынести в инициализацию? Если на каждом цикле включать прерывания, то это ведь может привести к неопределенному поведению?