SHOP News & Articles Contact Impressum

Testing Cubemars AK60-6 motor using arduino

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 ).

  1. Plug the CANBus shield onto Arduino Uno. Connect motor to the shield as shown in the image.

Processing: arduino_test.zip…

  1. Upload provided cubemars_test sketch to Arduino.
  2. Open serial terminal and input commands for motors as following
    image

image

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 :wink:

// 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

1 Like

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

1 Like

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

Footer With Address And Phones