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

fix work with float type of angle

Browse source
This commit is contained in:
lulko 2025-04-17 15:53:03 +03:00
parent 317a4c48ea
commit 06aae3981e
2 changed files with 20 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -117,32 +117,33 @@ void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, s
// Создаем сообщение // Создаем сообщение
CAN_message_t msg; CAN_message_t msg;
msg.id = id; msg.id = id;
msg.len = 8; // или как у вас в протоколе msg.len = 8; // или как в протоколе
msg.buf[0] = message_type; msg.buf[0] = message_type;
memcpy(&msg.buf[1], data, data_length); memcpy(&msg.buf[1], data, data_length);
// Формируем массив для CRC: ID + 2 байта данных (например, 'I' + value) // Формируем массив для CRC, включающий ID и все данные
size_t crc_data_size = sizeof(msg.id) + 2; size_t crc_data_size = sizeof(msg.id) + data_length;
uint8_t crc_data[crc_data_size]; uint8_t crc_data[crc_data_size];
// Копируем ID // Копируем ID
memcpy(crc_data, &msg.id, sizeof(msg.id)); memcpy(crc_data, &msg.id, sizeof(msg.id));
// Копируем 2 байта данных (например, msg.buf[0..1], где msg.buf[0] — message_type, а дальше значение) // Копируем все байты data
memcpy(crc_data + sizeof(msg.id), &msg.buf[0], 2); memcpy(crc_data + sizeof(msg.id), data, data_length);
// Расчет CRC // Расчет CRC
uint16_t crc_value = validate_crc16(crc_data, crc_data_size); uint16_t crc_value = validate_crc16(crc_data, crc_data_size);
// Вставляем CRC в буфер (можно в конец, или в специальные байты) // Вставляем CRC в буфер
msg.buf[6] = crc_value & 0xFF; // младший байт msg.buf[6] = crc_value & 0xFF;
msg.buf[7] = (crc_value >> 8) & 0xFF; // старший байт msg.buf[7] = (crc_value >> 8) & 0xFF;
// Отправка // Отправляем
Can.write(msg); Can.write(msg);
__NOP(); __NOP();
} }
void send_velocity() { void send_velocity() {
float current_velocity = motor.shaftVelocity(); float current_velocity = motor.shaftVelocity();
flash_rec = load_params(); flash_rec = load_params();
@ -163,9 +164,9 @@ void send_angle() {
// Обработка ошибки: запись в лог, сигнализация и т.д. // Обработка ошибки: запись в лог, сигнализация и т.д.
return; return;
} }
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',&value,sizeof(value)); send_can_with_id_crc(id,'A',&current_angle,sizeof(current_angle));
} }
void send_motor_enabled() { void send_motor_enabled() {
@ -219,8 +220,9 @@ void setup_id(uint8_t my_id) {
// send_id(); // send_id();
} }
void setup_angle(int target_angle_rad) { void setup_angle(float target_angle) {
float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы // float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы
motor.enable(); // Включаем мотор если он отключен
motor.controller = MotionControlType::angle; motor.controller = MotionControlType::angle;
motor.move(target_angle); motor.move(target_angle);
} }
@ -273,7 +275,9 @@ void listen_can(const CAN_message_t &msg) {
break; break;
case MOTOR_ANGLE: case MOTOR_ANGLE:
setup_angle(msg.buf[1]); memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
sizeof(motor_control_inputs.target_angle));
setup_angle(motor_control_inputs.target_angle);
break; break;
case MOTOR_ENABLED: case MOTOR_ENABLED:

4
controller/fw/embed/test/python_test_id.py
View file

@ -3,8 +3,8 @@ import time
# Конфигурация # Конфигурация
CAN_INTERFACE = 'can0' CAN_INTERFACE = 'can0'
OLD_DEVICE_ID = 0x69 OLD_DEVICE_ID = 0x00
NEW_DEVICE_ID = 0x00 NEW_DEVICE_ID = 0x69
REG_WRITE = 0x8 REG_WRITE = 0x8
REG_READ = 0x7 REG_READ = 0x7
REG_ID = 0x1 REG_ID = 0x1