CANopen with Ak80 Motor

ahmadelwaly · December 22, 2021, 12:39pm

hi

i have this motor AK80-80_AK Series Dynamical Modular_Exoskeleton Robot_CubeMars - Professional and Reliable manufacturer for robot actuators and frameless motors connected to mcp2515 Module Cs to pin 10
Si to pin 11
So to pin 12
Suck to pin 13

H CAN from the mcp2515 to H motor
L CAN from the mcp2515 to L motor

And I use termination 120 oham

// demo: CAN-BUS, send data

#include <mcp_can.h>
#include "mcp2515_can.h"
#include <SPI.h>

//SAMD core//

#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
#define SERIAL SerialUSB
#else
#define SERIAL Serial
#endif

//Value Limits
#define P_MIN -95.5f
#define P_MAX 95.5f
#define V_MIN -25.64f
#define V_MAX 25.64f
#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 = 20.0f;
float kp_in = 10.0f;
float kd_in = 1.0f;
float t_in = 12.0f;

//measured values
float p_out = 0.0f;
float v_out = 0.0f;
float t_out = 0.0f;

const int SPI_CS_PIN = 10;

mcp2515_can CAN(SPI_CS_PIN);


void setup()
{ Serial.begin(115200);
  Serial.println("setup");
  delay(1000);
  while (CAN_OK != CAN.begin(CAN_1000KBPS))
  {
    Serial.println("CAN BUS Shield init fail");
    Serial.println("Init CAN BUS Shield again");
    delay(100);
  }
  Serial.println("CAN BUS Shield init ok!");
  delay(1000);
  //Initialize pins as necessary
  pinMode(2, INPUT);

}

long previousMillis = 0;

void loop()
{
  //do something

  //Enable
  EnterMotorMode();
  Serial.println("Enabled");

  //UP
  float p_step = 0.1;
  Serial.println("UP");
  p_in = p_in + p_step;
  delay(1000);

  //Down
  Serial.println("DOWN");
  p_in = p_in - p_step;
  delay(1000);

  //Exit
  ExitMotorMode();
  Serial.println("Disabled");
  delay(1000);

  pack_cmd();

  if (CAN_MSGAVAIL == CAN.checkReceive())
  {
    Serial.println("received");
    unpack_reply();
  }

  Serial.print("\t");
  Serial.print(p_out);
  Serial.print("\t");
  Serial.print(v_out);
  Serial.print("\t");
  Serial.println(t_out);
  delay(10);

}

void EnterMotorMode()
{
  //Enter Motor Mode (enable)
  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;
  for (int i = 0; i < sizeof(buf); i++)
  {
    Serial.println(buf[i], HEX);
  }

  CAN.sendMsgBuf(0x01, 0, 8, buf);
  delay(50);
}
void ExitMotorMode()
{
  //Exit Motor Mode (enable)
  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;

  for (int i = 0; i < sizeof(buf); i++)
  {
    Serial.println(buf[i], HEX);
  }
  delay(50);
  CAN.sendMsgBuf(0x01, 0, 8, buf);
  delay(50);
  return 1;
}

void Zero()
{
  //Enter Motor Mode (enable)
  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);
  delay(50);
}

void pack_cmd()
{
  byte buf[8];

  /// CAN Command Packet Structure ///
  /// 16 bit position command, between -4*pi and 4*pi
  /// 12 bit kp, between 0 and 500 N-m/rad
  /// 12 bit kd, between 0 and 100 N-m*s/rad
  /// 12 bit feed forward torque, between -18 and 18 N-m
  /// CAN Packet is 8 8-bit words
  /// Formatted as follows. For each quantity, bit 0 is LSB
  /// 0: [position[15-8]]
  /// 1: [position[7-0]]
  /// 2: [velocity[11-4]]
  /// 3: [velocity[0-3], kp[11-8]]
  /// 4: [kp[7-0]]
  /// 5: [kd[11-4]]
  /// 6: [Kd[3-0], torque[11-8]]
  /// 7: [torque[7-0]]

  //Serial.println("packing");
  //delay(20);
  ///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);
  //convert floats to unsigned ints ///
  unsigned int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
  Serial.println(p_int);
  //delay(10);
  unsigned int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
  //Serial.println(v_int, HEX);
  //delay(10);
  unsigned int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
  //Serial.println(kp_int, HEX);
  //delay(10);
  unsigned int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
  //Serial.println(kd_int, HEX);
  //delay(10);
  unsigned int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
  //Serial.println(t_int, HEX);
  //delay(10);
  //pack ints into the 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;
  for (int i = 0; i < sizeof(buf); i++)
  {
    Serial.println(buf[i], HEX);
  }
  CAN.sendMsgBuf(0x01, 0, 8, buf);

  /* buf[0] = 0x01;
    buf[1] = p_int>>8;
    buf[2] = p_int&0xFF;
    buf[3] = v_int>>4;
    buf[4] = ((v_int&0xF)<<4)|(t_int>>8);
    buf[5] = t_int&0xFF;

    CAN.sendMsgBuf(0x01, 0, 6, buf);*/
  delay(10);
}

void unpack_reply()
{
  ///CAN Reply Packet Structure ///
  /// 16 bit position, between -4*pi and 4*pi
  /// 12 bit velocity, between -30 and +30 rad/s
  /// 12 bit current, between -40 and +40;
  /// CAN Packet is 5 8-bit words
  /// Formatted as follows. For each quantity, bit 0 is LSB
  /// 0: [position[15-8]]
  /// 1: [postion[7-0]
  /// 2: [velocity[11-4]]
  /// 3: [velocity[3-0], current[11-8]]
  /// 4: [current[7-0]]
  Serial.println("unpacking");

  byte len = 0;
  byte buf[8];
  CAN.readMsgBuf(&len, buf);

  unsigned long canId = CAN.getCanId();

  /// unpacks ints from CAN buffer ///
  unsigned int id = buf[0];
  unsigned int p_int = (buf[1] << 8) | buf[2];
  //Serial.println(p_int);
  //delay(10);
  unsigned int v_int = (buf[3] << 4) | (buf[4] >> 4);
  //Serial.println(v_int);
  //delay(10);
  unsigned int i_int = ((buf[4] & 0xF) << 8) | buf[5];
  //Serial.println(i_int);
  //delay(10);
  /// converts 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, int bit)
{ /// converts a float to an unsigned int, given range and number of bits ///
  //Serial.println("conv2");
  //Serial.println(x);
  //delay(20);
  float span = x_max - x_min;
  float offset = x_min;
  //Serial.println(offset);
  unsigned int pgg = 0;
  if (bit == 12)
  {
    unsigned int pgg = (x - offset) * 4095 / span;
    //Serial.println(pgg);
    return pgg;
  }
  if (bit == 16)
  {
    unsigned int pgg = (x - offset) * 65535 / span;
    //Serial.println(pgg);
    return pgg;
  }

}

unsigned int uint_to_float(unsigned int x_int, float x_min, float x_max, int bit)
{ /// converts an unsigned int to a float, given range and number of bits ///
  //Serial.println("conv1");
  //Serial.println(x_int);
  //Serial.print(x_min);
  //delay(20);
  float span = x_max - x_min;
  float offset = x_min;
  //Serial.println(offset);
  unsigned int pgg = 0;
  if (bit == 12)
  {
    float pgg = span * (x_int / 4095);
    //Serial.println(pgg);
    pgg = pgg + offset;
    //delay(50);
  }
  if (bit == 16)
  {
    float pgg = span * (x_int / 65535);
    //Serial.println(pgg);
    pgg = pgg + offset;
    //delay(50);
  }
  return pgg;
}



// END FILE

this my code and try to make the motor move step forward and other backward , but nothing happen for motor and no movement at all , anyone have any idea why ?

Thank you in advance

blh64 · December 22, 2021, 12:51pm

You increment your variable 'p_in with the comment "UP"
You decrement your variable p_in with the comment "DOWN"
You then call ExitMotorMode() which turn the motor off? (I don't know)
You then call the pack_cmd() function which takes that variable and puts it on the CAN bus.

You are sending the same value every time through loop() so how to you expect the motor to move?

ahmadelwaly · December 22, 2021, 1:41pm

// demo: CAN-BUS, send data

#include <mcp_can.h>
#include "mcp2515_can.h"
#include <SPI.h>

//SAMD core//

#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
#define SERIAL SerialUSB
#else
#define SERIAL Serial
#endif

//Value Limits
#define P_MIN -95.5f
#define P_MAX 95.5f
#define V_MIN -25.64f
#define V_MAX 25.64f
#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 = 20.0f;
float kp_in = 10.0f;
float kd_in = 1.0f;
float t_in = 12.0f;

//measured values
float p_out = 0.0f;
float v_out = 0.0f;
float t_out = 0.0f;

const int SPI_CS_PIN = 10;

mcp2515_can CAN(SPI_CS_PIN);


void setup()
{ Serial.begin(115200);
  Serial.println("setup");
  delay(1000);
  while (CAN_OK != CAN.begin(CAN_1000KBPS))
  {
    Serial.println("CAN BUS Shield init fail");
    Serial.println("Init CAN BUS Shield again");
    delay(100);
  }
  Serial.println("CAN BUS Shield init ok!");
  delay(1000);
  //Initialize pins as necessary
  pinMode(2, INPUT);

}

long previousMillis = 0;

void loop()
{
  //do something

  //Enable
  EnterMotorMode();
  Serial.println("Enabled");

  //UP
  float p_step = 0.1;
  Serial.println("UP");
  p_in = p_in + p_step;
  delay(1000);

  //Down
  Serial.println("DOWN");
  p_in = p_in - p_step;
  delay(1000);

 /* //Exit
  ExitMotorMode();
  Serial.println("Disabled");
  delay(1000);*/

  pack_cmd();

  if (CAN_MSGAVAIL == CAN.checkReceive())
  {
    Serial.println("received");
    unpack_reply();
  }

  Serial.print("\t");
  Serial.print(p_out);
  Serial.print("\t");
  Serial.print(v_out);
  Serial.print("\t");
  Serial.println(t_out);
  delay(10);

}

void EnterMotorMode()
{
  //Enter Motor Mode (enable)
  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;
  for (int i = 0; i < sizeof(buf); i++)
  {
    Serial.println(buf[i], HEX);
  }

  CAN.sendMsgBuf(0x01, 0, 8, buf);
  delay(50);
}
void ExitMotorMode()
{
  //Exit Motor Mode (enable)
  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;

  for (int i = 0; i < sizeof(buf); i++)
  {
    Serial.println(buf[i], HEX);
  }
  delay(50);
  CAN.sendMsgBuf(0x01, 0, 8, buf);
  delay(50);
  return 1;
}

void Zero()
{
  //Enter Motor Mode (enable)
  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);
  delay(50);
}

void pack_cmd()
{
  byte buf[8];

  /// CAN Command Packet Structure ///
  /// 16 bit position command, between -4*pi and 4*pi
  /// 12 bit kp, between 0 and 500 N-m/rad
  /// 12 bit kd, between 0 and 100 N-m*s/rad
  /// 12 bit feed forward torque, between -18 and 18 N-m
  /// CAN Packet is 8 8-bit words
  /// Formatted as follows. For each quantity, bit 0 is LSB
  /// 0: [position[15-8]]
  /// 1: [position[7-0]]
  /// 2: [velocity[11-4]]
  /// 3: [velocity[0-3], kp[11-8]]
  /// 4: [kp[7-0]]
  /// 5: [kd[11-4]]
  /// 6: [Kd[3-0], torque[11-8]]
  /// 7: [torque[7-0]]

  //Serial.println("packing");
  //delay(20);
  ///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);
  //convert floats to unsigned ints ///
  unsigned int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
  Serial.println(p_int);
  //delay(10);
  unsigned int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
  //Serial.println(v_int, HEX);
  //delay(10);
  unsigned int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
  //Serial.println(kp_int, HEX);
  //delay(10);
  unsigned int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
  //Serial.println(kd_int, HEX);
  //delay(10);
  unsigned int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
  //Serial.println(t_int, HEX);
  //delay(10);
  //pack ints into the 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;
  for (int i = 0; i < sizeof(buf); i++)
  {
    Serial.println(buf[i], HEX);
  }
  CAN.sendMsgBuf(0x01, 0, 8, buf);

  /* buf[0] = 0x01;
    buf[1] = p_int>>8;
    buf[2] = p_int&0xFF;
    buf[3] = v_int>>4;
    buf[4] = ((v_int&0xF)<<4)|(t_int>>8);
    buf[5] = t_int&0xFF;

    CAN.sendMsgBuf(0x01, 0, 6, buf);*/
  delay(10);
}

void unpack_reply()
{
  ///CAN Reply Packet Structure ///
  /// 16 bit position, between -4*pi and 4*pi
  /// 12 bit velocity, between -30 and +30 rad/s
  /// 12 bit current, between -40 and +40;
  /// CAN Packet is 5 8-bit words
  /// Formatted as follows. For each quantity, bit 0 is LSB
  /// 0: [position[15-8]]
  /// 1: [postion[7-0]
  /// 2: [velocity[11-4]]
  /// 3: [velocity[3-0], current[11-8]]
  /// 4: [current[7-0]]
  Serial.println("unpacking");

  byte len = 0;
  byte buf[8];
  CAN.readMsgBuf(&len, buf);

  unsigned long canId = CAN.getCanId();

  /// unpacks ints from CAN buffer ///
  unsigned int id = buf[0];
  unsigned int p_int = (buf[1] << 8) | buf[2];
  //Serial.println(p_int);
  //delay(10);
  unsigned int v_int = (buf[3] << 4) | (buf[4] >> 4);
  //Serial.println(v_int);
  //delay(10);
  unsigned int i_int = ((buf[4] & 0xF) << 8) | buf[5];
  //Serial.println(i_int);
  //delay(10);
  /// converts 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, int bit)
{ /// converts a float to an unsigned int, given range and number of bits ///
  //Serial.println("conv2");
  //Serial.println(x);
  //delay(20);
  float span = x_max - x_min;
  float offset = x_min;
  //Serial.println(offset);
  unsigned int pgg = 0;
  if (bit == 12)
  {
    unsigned int pgg = (x - offset) * 4095 / span;
    //Serial.println(pgg);
    return pgg;
  }
  if (bit == 16)
  {
    unsigned int pgg = (x - offset) * 65535 / span;
    //Serial.println(pgg);
    return pgg;
  }

}

unsigned int uint_to_float(unsigned int x_int, float x_min, float x_max, int bit)
{ /// converts an unsigned int to a float, given range and number of bits ///
  //Serial.println("conv1");
  //Serial.println(x_int);
  //Serial.print(x_min);
  //delay(20);
  float span = x_max - x_min;
  float offset = x_min;
  //Serial.println(offset);
  unsigned int pgg = 0;
  if (bit == 12)
  {
    float pgg = span * (x_int / 4095);
    //Serial.println(pgg);
    pgg = pgg + offset;
    //delay(50);
  }
  if (bit == 16)
  {
    float pgg = span * (x_int / 65535);
    //Serial.println(pgg);
    pgg = pgg + offset;
    //delay(50);
  }
  return pgg;
}



// END FILE

i made edit for the code , i was think i will enable the motor and make a forward move and backward move then stop.

but i comment now the exit mode like you see but no change.

TomGeorge · December 22, 2021, 1:58pm

Hi, @ahmadelwaly

Can you please post a circuit diagram of your project?

You do have the gnd of the motor connected to gnd of the controller?

What model Arduino controller are you using?

Thanks.. Tom...

blh64 · December 22, 2021, 3:01pm

ahmadelwaly:

  //UP
  float p_step = 0.1;
  Serial.println("UP");
  p_in = p_in + p_step;
  delay(1000);

  //Down
  Serial.println("DOWN");
  p_in = p_in - p_step;
  delay(1000);

 /* //Exit
  ExitMotorMode();
  Serial.println("Disabled");
  delay(1000);*/

  pack_cmd();

This is still not correct. You have to increment your variable, send the value to the motor and then decrement the value and send the value to the motor. You are still sending the same value to the motor. I am not sure if you would be able to see a 0.1 "step" by the motor?

  //UP
  float p_step = 0.1;
  Serial.println("UP");
  p_in = p_in + p_step;
  pack_cmd();
  delay(1000);

  //Down
  Serial.println("DOWN");
  p_in = p_in - p_step;
  pack_cmd();
  delay(1000);

 /* //Exit
  ExitMotorMode();
  Serial.println("Disabled");
  delay(1000);

  pack_cmd();
*/

ahmadelwaly · December 23, 2021, 5:07am

Hi tom , thanks for replay .

i have Arduino Uno connected to mcp2515 Module
Cs to pin 10
Si to pin 11
So to pin 12
Suck to pin 13

H CAN from the mcp2515 to H motor
L CAN from the mcp2515 to L motor

And I use termination 120 oham

ahmadelwaly · December 23, 2021, 5:20am

thanks for help

i'm tried that too , but i don't feel any movement. and i try 10 steps btw

TomGeorge · December 23, 2021, 5:27am

Hi,
Suck == SCK

You do have the gnd of the motor connected to gnd of the controller?

Please a circuit diagram, words can be misunderstood.

What are you powering the motor with?

Tom...

ahmadelwaly · December 23, 2021, 5:50am

yes i mean sck i'm sorry .

no i don't have gnd between the controller and motor. i just connect the CAN H , CAN L of the motor to mcp2515 module

i'm powering the motor with power supply 48v and 20A , i test the motor with the software from cubemares and it's working , But the problem when i connect it to arduino.

system · June 21, 2022, 5:50am

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
T-motor Cubemars AK 80-64 arduino Spakfun CAN bus communication Programming Questions	30	6147	September 26, 2024
Interfacing my GIM 8115-9 actuator with Arduino via MCP2515 CAN module Motors, Mechanics, Power and CNC	20	226	September 20, 2024
Controlling MY ACTUATOR motor with CAN-BUS Shield Motors, Mechanics, Power and CNC	28	4931	August 7, 2024
Troubleshooting CAN Bus to Control Motor Motors, Mechanics, Power and CNC	13	126	July 4, 2024
CAN bus MCP2515 Programming Questions	10	2873	May 5, 2021

CANopen with Ak80 Motor

Related Topics