Утро вечера мудренее.
Создал задачу, поместил в нее блокирующую функцию чтения алертов.
Все заработало.
Скорость шины подняли до 1 мбит. При инициализации настроек моторов новые значения почему-то не всегда записывались. Пока не понятна причина. Возможно помехи на высокой скорости. Диагностику нужно добавлять, что бы анализировать.
Но в целом можно считать успешно. Получаю скорость по запросу, положение энкодера по запросу.
Так же есть команда, которая задает периодическую отсылку сообщений с моторов. Задать номер параметра и частоту отправки. Не проверял пока.
Код прилагаю. Мало ли кто похожим вопросом занимается.
#include <Arduino.h>
#include "can.h"
#include "mksservocmd.h"
#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
#include "driver/twai.h"
TaskHandle_t h_Can;
void parsing(twai_message_t msg);
void setup() {
Serial.begin(115200);
while (!canInit()) {
Serial.println("Error init CAN BUS");
delay(5000);
};
xTaskCreatePinnedToCore(task_Can, "Can", 4096, NULL, 10, &h_Can, 1);
mksInit();
};
int sp = 50;
int dir = 1;
void loop() {
//Serial.printf("SPEED: %d | ", sp);
//mksRequestSpeed(1);
mksRequestPosition(1);
//mksSetSpeed(1, sp);
if (sp == 0) {
delay(20000);
}
if (sp > 1500) {
dir = -1;
}
if (sp < -1500) {
dir = 1;
}
sp += 50 * dir;
delay(200);
};
void task_Can(void *pvParameters) { // This is a task.
(void)pvParameters;
uint32_t alerts;
twai_message_t rx_msg;
for (;;) {
if (twai_read_alerts(&alerts, portMAX_DELAY) == ESP_OK) {
if (alerts & TWAI_ALERT_RX_DATA) {
if (twai_receive(&rx_msg, 0) == ESP_OK){
// Serial.print("Received CMD: 0x");
// Serial.println(rx_msg.data[0], HEX);
parsing(rx_msg);
}
}
else if (alerts & TWAI_ALERT_BUS_OFF) {
Serial.println("BUS OFF - restarting...");
twai_stop();
vTaskDelay(pdMS_TO_TICKS(100));
twai_start();
}
}
}
};
void parsing(twai_message_t msg){
int16_t speed;
int32_t carry;
uint16_t value;
switch (msg.data[0]) {
case 0x32: // speed
Serial.printf("code: %X ", msg.data[0]);
speed = (msg.data[1] << 8) | msg.data[2];
Serial.printf("speed: %d\n", speed);
break;
case 0x30: // position
Serial.printf("code: %X ", msg.data[0]);
carry = (msg.data[1] << 24) | (msg.data[2] << 16) | (msg.data[3] << 8) | msg.data[4];
Serial.printf("carry: %d ", carry);
value = (msg.data[5] << 8) | msg.data[6];
Serial.printf("value: %d ", value);
Serial.printf("pos: %d\n", carry * 0x4000 + value);
break;
default:
break;
}
}
can.h
#ifndef CAN_H
#define CAN_H
#include "driver/twai.h"
// Pins used to connect to CAN bus transceiver:
#define RX_PIN 26
#define TX_PIN 25
// Interval:
#define POLLING_RATE_MS 1000
// инициализация can шины
bool canInit(void);
// расчет контрольной суммы пакета
uint8_t CalcCRC(twai_message_t msg);
//отправка сообщения
bool sendMessage(twai_message_t msg);
// прием сообщения
esp_err_t reciveMessage(void);
//Команды для установки начальных значений в мотор MKS
void mksservoinit(void);
#endif
can.cpp
#include <Arduino.h>
#include "can.h"
#include "mksservocmd.h"
/*************************************************************
Инициализация CAN шины
Включаем минимально необходимые алерты для приема и отправки
сообщений, ошибка шины
При приеме сообщения генерируется прерывание и выставляет флаг.
В отдельной функции проверить флаг и принять сообщение
**************************************************************/
//uint8_t masiv[10] = {1,2,3,4,5,6,7,8,9,10};
//int arr[3][2] = {{23, 34}, {12, 5}, {7, 9}};
// // Send message
// // Configure message to transmit
// twai_message_t message;
// message.identifier = 0x01; //slave address
// message.data_length_code = 5; //DLC
// message.data[0] = 0xF6; //function code
// message.data[1] = ( (speed >> 8) & 0x0F); //High 4 bits for direction and speed
// message.data[2] = speed & 0x00FF; //8 bits lower
// message.data[3] = 0; //0xF5; // acc
// message.data[4] = CalcCRC(message);
// sendMessage(message);
bool canInit(void) {
// Initialize configuration structures using macro initializers
twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)TX_PIN, (gpio_num_t)RX_PIN, TWAI_MODE_NORMAL);
//twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS(); //Look in the api-reference for other speed sets.
twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS(); //Look in the api-reference for other speed sets.
twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
// Install TWAI driver
esp_err_t err = twai_driver_install(&g_config, &t_config, &f_config);
if ( err != ESP_OK) {
Serial.printf("Failed to install driver. err: 0x%x\n", err);
return false;
}
Serial.printf("Driver installed\n");
// Start TWAI driver
err = twai_start();
if ( err != ESP_OK) {
Serial.printf("Failed to start driver err: 0x%x\n", err);
return false;
Serial.printf("Driver started\n");
}
// Настройка минимально необходимых алертов
uint32_t alerts_to_enable = TWAI_ALERT_TX_SUCCESS |
TWAI_ALERT_RX_DATA |
TWAI_ALERT_BUS_ERROR;
err = twai_reconfigure_alerts(alerts_to_enable, NULL);
if ( err != ESP_OK) {
Serial.printf("Failed to reconfigure alerts. err: 0x%x\n", err);
return false;
}
Serial.printf("CAN Alerts reconfigured\n");
return true;
}
/*********************************************************
прием сообщений
**********************************************************/
// esp_err_t reciveMessage(void) {
// return twai_receive(&rxMsg, 0);
// };
bool sendMessage(twai_message_t msg) {
// Queue message for transmission
esp_err_t err = twai_transmit(&msg, pdMS_TO_TICKS(10));
if (err != ESP_OK) {
Serial.printf("Failed message transmission. err: 0x%x\n", err);
return false;
}
//printf("Message transmission ok\n");
return true;
}
/*****************************************************
вычисляем контрольную сумму
typedef struct {
uint32_t identifier; включаем в расчет
uint8_t data_length_code; не включаем
uint8_t data[TWAI_FRAME_MAX_DLC]; включаем кроме последнего байта
} twai_message_t;
В последнем байте будет рассчитанный CRC
******************************************************/
uint8_t CalcCRC(twai_message_t msg) {
uint16_t sum = msg.identifier;
for(uint8_t i = 0; i < msg.data_length_code - 1; i++)
{
sum += msg.data[i]; //Calculate accumulated value
}
return(sum & 0xFF); //return checksum
}
mksservocmd.h
/*****************************************************
Команды для установки начальных значений в мотор MKS
0x82 установка режима FOC
0x83 установка максимального тока 1600 ма (0x640)
0x9B установка тока удержания 10%
0x84 установка микро шага 16
0x8C установка режима ответа slave
******************************************************/
#ifndef MKSSERVOCMD_H
#define MKSSERVOCMD_H
#include "driver/twai.h"
void mksSetModeFoc(uint8_t id);
void mksSetMaxCurrent(uint8_t id);
void mksSetCurrentHold(uint8_t id);
void mksSetMicroStep(uint8_t id);
void mksSetModeRequestSlave(uint8_t id);
void mksInit(void);
void mksSetSpeed(uint8_t id, int16_t speed);
void mksRequestSpeed(uint8_t id);
void mksRequestPosition(uint8_t id);
#endif
mksservocmd.cpp
#include "mksservocmd.h"
#include "can.h"
void mksSetModeFoc(uint8_t id) {
twai_message_t msg;
msg.identifier = id;
msg.data_length_code = 0x03;
msg.data[0] = 0x82; // установка режима V_FOC
msg.data[1] = 0x05; // foc
msg.data[2] = CalcCRC(msg);
sendMessage(msg);
};
void mksSetMaxCurrent(uint8_t id) {
twai_message_t msg;
msg.identifier = id;
msg.data_length_code = 0x04;
msg.data[0] = 0x83; // Установка максимального тока 1600 ма (0x640)
msg.data[1] = 0x06;
msg.data[2] = 0x40;
msg.data[3] = CalcCRC(msg);
sendMessage(msg);
};
void mksSetCurrentHold(uint8_t id) {
twai_message_t msg;
msg.identifier = id;
msg.data_length_code = 0x03;
msg.data[0] = 0x9B; // Установка тока удержания на 10%
msg.data[1] = 0x00;
msg.data[2] = CalcCRC(msg);
sendMessage(msg);
};
void mksSetMicroStep(uint8_t id) {
twai_message_t msg;
msg.identifier = id;
msg.data_length_code = 0x03;
msg.data[0] = 0x84; // установка микрошага 16
msg.data[1] = 0x10;
msg.data[2] = CalcCRC(msg);
sendMessage(msg);
};
void mksSetModeRequestSlave(uint8_t id){
twai_message_t msg;
msg.identifier = id;
msg.data_length_code = 0x04;
msg.data[0] = 0x8C; // установка режима ответа slave
msg.data[1] = 0x01;
msg.data[2] = 0x01;
msg.data[3] = CalcCRC(msg);
sendMessage(msg);
};
void mksInit(void) {
for (uint8_t i = 1; i <= 2; i++){
mksSetModeFoc(i);
mksSetMaxCurrent(i);
mksSetCurrentHold(i);
mksSetMicroStep(i);
mksSetModeRequestSlave(i);
}
}
void mksSetSpeed(uint8_t id, int16_t speed) {
// Send message
// Configure message to transmit
uint8_t dir = (speed < 0 ? 1 : 0);
speed = abs(speed);
twai_message_t msg;
msg.identifier = id; //slave address
msg.data_length_code = 0x5; //DLC
msg.data[0] = 0xF6; //function code
msg.data[1] = (dir<<7) | ( (speed >> 8) & 0x0F); //High 4 bits for direction and speed
msg.data[2] = speed & 0x00FF; //8 bits lower
msg.data[3] = 0; //0xF5; // acc
msg.data[4] = CalcCRC(msg);
sendMessage(msg);
}
//запрос скорости в реальном времени
void mksRequestSpeed(uint8_t id){
twai_message_t msg;
msg.identifier = id; //slave address
msg.data_length_code = 0x02; //DLC
msg.data[0] = 0x32; //function code
msg.data[1] = CalcCRC(msg);
sendMessage(msg);
}
//запрос позиции в реальном времени
void mksRequestPosition(uint8_t id){
twai_message_t msg;
msg.identifier = id; //slave address
msg.data_length_code = 0x02; //DLC
msg.data[0] = 0x30; //function code
msg.data[1] = CalcCRC(msg);
sendMessage(msg);
}
