Here i am attaching a basic sketch for testing the Cubemars AK60-6 motor using Arduino and CAN-Bus shield ( ELE Can-Bus Shield-V1.5 ).
Hi Zainroid,
Thank you very much for sharing these instructions, I cannot access the arduino sketch cubemars_test.
COuld you please share it with me or grant an access?
Kind regards,
Akim
@Akim, please find below 
// This skecth is for testing the cubemars Ak60-6 motor using arduino CAN shield
/// use serial monitor for input the commands to motor
/// o -- turns on the motor mode
/// f -- turns of the motor mode
/// u -- steps up the position increment according to fix offset, can be adjusted in loop
/// u -- 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
#define LED 8
#define LED 7
// Value Limits
#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
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 =10;
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(CAN_1000KBPS))
{
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("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(){
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);
}
void unpack_reply(){
byte len=0;
byte buf[8];
CAN.readMsgBuf(&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);
p_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;
}
Hi, I am trying to adapt this code for AK 80-64 motor, however serial monitor prints the following
15:02:33.325 → Entering Configuration Mode Failure…
15:02:33.325 → CAN BUS Shield init fail
15:02:33.325 → Init CAN BUS Shield again
Do you have any suggestions on that?
Hi Zainroid,
Thank you for the code, did you make any changes using the CubeMars upper computer software? I’m thinking about maybe the Baudrate? I’m also using a AK60-6 but the V1.1 version, do you know if I have to modify the code or if I can just flash your code and it will work?
Thank you for your help,
Axel
Hi @Zainroid
Thanks for your tutorial.
I did try it with an Arduino Nano and a AZ Delivery MCP2515 CAN
Your code is working between two Arduinos with two MCP2515 Can Modules but I can’t pilot the motor.
When I try to send ‘o’, I received data on the second arduino but the motor is always off…
Are there any other settings to set?
Thanks for your help!
Best regards,
Jacques
Hi @Jacques,
Did you try sending ‘u’ after sending ‘o’. Motor doesn’t responds only by just sending ‘o’ , it only enables the motion mode. Try sending ‘u’ afterwards.
In some cases CAN interface is also broken.
Regards,
Zain
Hi Zainroid,
Thank you so much for your information! Now I am trying to run my AK80-6 motor with your code, but found that I can read through the serial plotter, sending o or u to make differences on the plot, but the motor never run. Could you please give me some suggestions over it? Than you very much!
Sincerely,
Boer
Hi @boerc , can you detect your motor on upper program and check is it in torque mode or not.
Regards,
Zain
