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

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

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