diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index c22f247..572cc52 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -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',¤t_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: diff --git a/controller/fw/embed/test/python_test_id.py b/controller/fw/embed/test/python_test_id.py index 433cd88..4788dc1 100644 --- a/controller/fw/embed/test/python_test_id.py +++ b/controller/fw/embed/test/python_test_id.py @@ -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