Hi everyone,
I’m working on a project where I’m trying to control a Cubemars AK80-9 motor using an Arduino Uno and a SeeedStudio CAN Bus Shield V2 (the one that mounts directly on the Uno and uses the MCP2515).
I’m powering the motor with 24V, and I’ve added a 120-ohm termination resistor at the motor end of the CAN line. The CANH and CANL lines go directly from the shield to the motor.
I'm using the following code to control the motor:
// This skecth is for testing the cubemars Ak80-9 motor using arduino CAN shield
/// use serial monitor for input the commands to motor
/// o -- turns on the motor mode
/// f -- turns off the motor mode
/// u -- steps up the position increment according to fix offset, can be adjusted in loop
/// d -- steps down the position increment according to fix offset, can be adjusted in loop
#include <mcp_can.h>
#include <SPI.h>
// the cs pin of the version v1.4 is default to D10
// Set CS pin
#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
#define SERIAL SerialUSB
#else
#define SERIAL Serial
#endif
// Value Limits--Standard Values, shouldn't be changed
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -65.0f
#define V_MAX 65.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
// Set Values
unsigned int id = 0;
float p_in = 0.0f;
float v_in = 0.0f;
float kp_in = 20.0f;
float kd_in = 1.0f;
float t_in = 0.0f;
//measured values - responses from the motor
float p_out = 0.0f; // actual position
float v_out = 0.0f; // actual velocity
float t_out = 0.0f; // actual torque
const int SPI_CS_PIN = 9;// change it to 9
MCP_CAN CAN(SPI_CS_PIN); //Set CS pin
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
delay(1000);
while (CAN_OK != CAN.begin(MCP_ANY, CAN_1000KBPS, MCP_16MHZ)) {
Serial.println("CAN BUS Shield init fail");
Serial.println("Init CAN BUS Shield again");
delay(1000);
}
Serial.println("CAN BUS Shield init ok!");
}
long previousMillis = 0;
boolean newData = false;
void loop() {
char rc;
float p_step = 0.1;
//delay(200);
while (Serial.available() > 0 && newData == false) {
rc = Serial.read();
Serial.println(rc);
if (rc == 's') {
Serial.println("True");
}
if (rc == 'u') {
p_in = p_in + p_step;
}
if (rc == 'd') {
p_in = p_in - p_step;
}
p_in = constrain(p_in, P_MIN, P_MAX);//
if (rc == 'o') {
EnterMotorMode();
}
if (rc == 'f') {
ExitMotorMode();
}
// send CAN
pack_cmd();
}
// receive CAN
if (CAN_MSGAVAIL == CAN.checkReceive()) // check if data coming
{
unpack_reply();
}
Serial.print(millis() - previousMillis);
previousMillis = millis();
Serial.print("ID ");
Serial.print(id);
Serial.print("position ");
Serial.print(p_in);
Serial.print(" ");
Serial.print("position output ");
Serial.print(p_out);
Serial.print(" ");
Serial.print("velocity output ");
Serial.print(v_out);
Serial.print(" ");
Serial.print("torque output ");
Serial.println(t_out);
}
void EnterMotorMode() {// standard values
byte buf[8];
buf[0] = 0xFF;
buf[1] = 0xFF;
buf[2] = 0xFF;
buf[3] = 0xFF;
buf[4] = 0xFF;
buf[5] = 0xFF;
buf[6] = 0xFF;
buf[7] = 0xFC;
CAN.sendMsgBuf(0x01, 0, 8, buf);
}
void ExitMotorMode() {
byte buf[8];
buf[0] = 0xFF;
buf[1] = 0xFF;
buf[2] = 0xFF;
buf[3] = 0xFF;
buf[4] = 0xFF;
buf[5] = 0xFF;
buf[6] = 0xFF;
buf[7] = 0xFD;
CAN.sendMsgBuf(0x01, 0, 8, buf);
}
void Zero() {
byte buf[8];
buf[0] = 0xFF;
buf[1] = 0xFF;
buf[2] = 0xFF;
buf[3] = 0xFF;
buf[4] = 0xFF;
buf[5] = 0xFF;
buf[6] = 0xFF;
buf[7] = 0xFE;
CAN.sendMsgBuf(0x01, 0, 8, buf);
}
void pack_cmd() {
byte buf[8];
// limit data to be within bounds ///
float p_des = constrain(p_in, P_MIN, P_MAX);
float v_des = constrain(v_in, V_MIN, V_MAX);
float kp = constrain(kp_in, KP_MIN, KP_MAX);
float kd = constrain(kd_in, KD_MIN, KD_MAX);
float t_ff = constrain(t_in, T_MIN, T_MAX);
unsigned int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
unsigned int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
unsigned int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
unsigned int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
unsigned int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
//pack ints into can buffer ///
buf[0] = p_int >> 8;
buf[1] = p_int & 0xFF;
buf[2] = v_int >> 4;
buf[3] = ((v_int & 0xF) << 4) | (kp_int >> 8);
buf[4] = kp_int & 0xFF;
buf[5] = kd_int >> 4;
buf[6] = ((kd_int & 0xF) << 4) | (t_int >> 8);
buf[7] = t_int & 0xFF;
CAN.sendMsgBuf(0x01, 0, 8, buf);// canid
}
void unpack_reply() {
unsigned long int canId;
byte len = 0;
byte buf[8];
CAN.readMsgBuf(&canId, &len, buf);
//unsigned long canId = CAN.getCanId();
///unpack ints from can buffer///
unsigned int id = buf[0];
unsigned int p_int = (buf[1] << 8) | buf[2];
unsigned int v_int = (buf[3] << 4) | (buf[4] >> 4);
unsigned int i_int = ((buf[4] & 0xF) << 8) | buf[5];
/// convert uints to floats ///
p_out = uint_to_float(p_int, P_MIN, P_MAX, 16);
v_out = uint_to_float(v_int, V_MIN, V_MAX, 12);
t_out = uint_to_float(i_int, -T_MIN, T_MAX, 12);
}
unsigned int float_to_uint(float x, float x_min, float x_max, float bits) {
//convert a float to an unsigned int, given range and number of bits ///
float span = x_max - x_min;
float offset = x_min;
unsigned int pgg = 0;
if (bits == 12) {
pgg = (unsigned int)((x - offset) * 4095.0 / span);
}
if (bits == 16) {
pgg = (unsigned int)((x - offset) * 65535.0 / span);
}
return pgg;
}
float uint_to_float(unsigned int x_int, float x_min, float x_max, int bits) {
/// converts unsined int to float, given range and numbers of bits///
float span = x_max - x_min;
float offset = x_min;
float pgg = 0;
if (bits == 12) {
pgg = ((float)x_int) * span / 4095.0 + offset;
}
if (bits == 16) {
pgg = ((float)x_int) * span / 65535.0 + offset;
}
return pgg;
}
When I type o in the Serial Monitor (to send the “enter motor mode” command), the INT LED on the CAN shield blinks, but the motor doesn’t do anything. No sound, no movement, nothing.
Other commands like u, d, and f also print as expected — so the Arduino and CAN library seem to be running.
Any help would really be appreciated.
Thanks!
since the formatting is still terrible. If you format it properly, you will notice that every time you send something from your PC to the program, it will Always call