Hi everyone, i have some problem to control a motor AK10-9 in servo Mode.
At the moment, i want to control the motor with arduino Uno with and CAN moduler MCP2515. But the problem is i can get the feedback of the motor at momento sending the comand with 0 values, but when i want to send another comand for example with comandos for the position and velocity, the motor does not do anything.
Here i show you the code:
#include <SPI.h>
#include <mcp_can.h>
/SAMD core/
#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
#define SERIAL SerialUSB
#else
#define SERIAL Serial
#endif
// CS pin
MCP_CAN CAN(10);
int CAN_INT = 2;
// Some constants
const float MAX_PWM = 100000.0f;
const float MAX_CURRENT = 60000.0f;
const float MAX_VELOCITY = 100000.0f;
const float MAX_POSITION = 360000000.0f;
const float MAX_POSITION_VELOCITY = 32767.0f;
const float MIN_POSITION_VELOCITY = -32768.0f;
const float MAX_ACCELERATION = 400000.0f;
const int avanzar = 7; // Pin for avanzar button
const int prender = 6; // Pin for prender button
const int apagar = 5; // Pin for apagar button
const int retroceder = 4;
enum AKMode {
AK_PWM = 0,
AK_CURRENT,
AK_CURRENT_BRAKE,
AK_VELOCITY,
AK_POSITION,
AK_ORIGIN,
AK_POSITION_VELOCITY,
};
uint32_t canId(int id, AKMode Mode_set) {
uint32_t mode;
mode = Mode_set;
return uint32_t(id | mode << 8);
}
void comm_can_transmit_eid(uint32_t id, const uint8_t* data, uint8_t len) {
if (len > 8) {
len = 8;
}
uint8_t buf[len];
for (uint8_t i = 0; i < len; i++) {
buf[i] = data[i];
}
// Imprimir el ID y el buffer antes de enviar
Serial.print("Enviando CAN ID: “);
Serial.print(id, HEX);
Serial.print(” Data: “);
for (uint8_t i = 0; i < len; i++) {
if (buf[i] < 0x10) {
Serial.print(“0”);
}
Serial.print(buf[i], HEX);
Serial.print(” ");
}
Serial.println(" “);
Serial.println(” ");
// Enviar el mensaje CAN
if (CAN.sendMsgBuf(id, 1, len, buf) != CAN_OK) {
Serial.println(“Error enviando el mensaje CAN”);
} else {
Serial.println(“Mensaje CAN enviado con éxito”);
}
}
void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t* index) {
buffer[(*index)++] = number >> 24;
buffer[(*index)++] = number >> 16;
buffer[(*index)++] = number >> 8;
buffer[(*index)++] = number;
}
void buffer_append_int16(uint8_t* buffer, int16_t number, int16_t* index) {
buffer[(*index)++] = number >> 8;
buffer[(*index)++] = number;
}
void comm_can_set_duty(uint8_t controller_id, float duty) {
int32_t send_index = 0;
uint8_t buffer[4];
buffer_append_int32(buffer, (int32_t)(duty * 100000.0), &send_index);
comm_can_transmit_eid(canId(controller_id, AKMode::AK_PWM), buffer, send_index);
}
void comm_can_set_current(uint8_t controller_id, float current) {
int32_t send_index = 0;
uint8_t buffer[4];
buffer_append_int32(buffer, (int32_t)(current * 1000.0), &send_index);
comm_can_transmit_eid(canId(controller_id, AKMode::AK_CURRENT), buffer, send_index);
}
void comm_can_set_cb(uint8_t controller_id, float current) {
int32_t send_index = 0;
uint8_t buffer[4];
buffer_append_int32(buffer, (int32_t)(current * 1000.0), &send_index);
comm_can_transmit_eid(canId(controller_id, AKMode::AK_CURRENT_BRAKE), buffer, send_index);
}
void comm_can_set_rpm(uint8_t controller_id, float rpm) {
int32_t send_index = 0;
uint8_t buffer[4];
buffer_append_int32(buffer, (int32_t)rpm, &send_index);
comm_can_transmit_eid(canId(controller_id, AKMode::AK_VELOCITY), buffer, send_index);
}
void comm_can_set_pos(uint8_t controller_id, float pos) {
int32_t send_index = 0;
uint8_t buffer[4];
buffer_append_int32(buffer, (int32_t)(pos * 1000000.0), &send_index);
comm_can_transmit_eid(canId(controller_id, AKMode::AK_POSITION), buffer, send_index);
}
void comm_can_set_origin(uint8_t controller_id, uint8_t set_origin_mode) {
int32_t send_index = 0;
uint8_t buffer[4];
comm_can_transmit_eid(canId(controller_id, AKMode::AK_ORIGIN), buffer, send_index);
}
void comm_can_set_pos_spd(uint8_t controller_id, float pos, int16_t spd, int16_t RPA) {
int32_t send_index = 0;
uint8_t buffer[8];
buffer_append_int32(buffer, (int32_t)(pos * 10000.0), &send_index);
// Directly append int16 values at the correct positions
buffer[4] = (spd >> 8) & 0xFF;
buffer[5] = spd & 0xFF;
buffer[6] = (RPA >> 8) & 0xFF;
buffer[7] = RPA & 0xFF;
comm_can_transmit_eid(canId(controller_id, AKMode::AK_POSITION_VELOCITY), buffer, 8);
}
void motor_receive(float* motor_pos, float* motor_spd, float* motor_cur, int8_t* motor_temp, int8_t* motor_error) {
byte len = 0;
byte buf[8];
unsigned long canId;
CAN.readMsgBuf(&canId, &len, buf);
int16_t pos_int = buf[0] << 8 | buf[1];
int16_t spd_int = buf[2] << 8 | buf[3];
int16_t cur_int = buf[4] << 8 | buf[5];
*motor_pos = (float)(pos_int * 1.0f);
*motor_spd = (float)(spd_int * 10.0f);
*motor_cur = (float)(cur_int * 0.01f);
}
void setup() {
Serial.begin(115200);
SPI.begin();
while (!Serial) {};
while (CAN_OK != CAN.begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ)) { // init can bus : baudrate = 1000k
Serial.println(“CAN BUS Shield init fail”);
Serial.println(" Init CAN BUS Shield again");
delay(100);
}
Serial.println(“CAN init ok!”);
pinMode(CAN_INT, INPUT);
pinMode(avanzar, INPUT_PULLUP);
pinMode(prender, INPUT_PULLUP);
pinMode(apagar, INPUT_PULLUP);
pinMode(retroceder, INPUT_PULLUP);
CAN.setMode(MCP_NORMAL);
}
float pos = 0.0;
int16_t spd = 0;
int16_t RPA = 10;
float motor_pos = 0.0;
float motor_spd = 0.0;
float motor_cur = 0.0;
int8_t motor_temp = 0;
int8_t motor_error = 0;
void loop() {
if (digitalRead(avanzar) == LOW) {
pos += 10000.0; // Incrementa la posición deseada
}
if (digitalRead(retroceder) == LOW) {
pos -= 10000.0; // Decrementa la posición deseada
}
if (digitalRead(prender) == LOW) {
spd += 1000; // Velocidad deseada
}
if (digitalRead(apagar) == LOW) {
spd -= 1000; // Para el motor
}
comm_can_set_pos_spd(0x01, pos, spd, RPA);
// Recibir datos del motor
motor_receive(&motor_pos, &motor_spd, &motor_cur, &motor_temp, &motor_error);
// Imprimir datos del motor
Serial.print(“Deseada: “);
Serial.print(pos);
Serial.print(” Vel_des: “);
Serial.print(spd);
Serial.print(” Posición: “);
Serial.print(motor_pos);
Serial.print(” °, Velocidad: “);
Serial.print(motor_spd/10);
Serial.print(” RPM, Corriente: “);
Serial.print(motor_cur*10);
Serial.println(” A”);
delay(100); // Intervalo entre comandos
}