AK 80-64 actuation

Hi, I am trying to actuate AK 80-64 with the use of Arduino + Sparkfun CAN bus shield, hwoever the motor doesn’t want to start.

Do you have any suggestions or advice on what part needs to be fixed?

Please see my code and setup used

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

/* SAMD core*/
#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
  #define SERIAL SerialUSB
#else
  #define SERIAL 0x00
#endif

//Define Joystick connection pins
#define UP A1
#define DOWN A3
#define RIGHT A5
#define CLICK A4
#define LEFT A2

//Define LED pins
#define LED2 8
#define LED3 7

//VALUE LIMIS

#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -5.0f
#define V_MAX 5.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN 0.0f
#define T_MAX 5.0f

// SET VALUES

float p_in = 0.0f;
float v_in = 3.0f;
float kp_in = 0.0f;
float kd_in = 3.0f;
float t_in = 0.0f;
//measured values

float p_out = 0.0f;
float v_out = 0.0f;
float t_out = 0.0f;

//the cs pin of the version after v1.1 is default to D9
//v0.9 and v1.0 is default D10
const int SPI_CS_PIN = 10;

MCP_CAN CAN (SPI_CS_PIN);   //Set CS pin

void setup() {
  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(100);
  }
Serial.println("CAN BUS Shield is ok!");

//Intialize pins as necessary

pinMode(UP, INPUT);
pinMode(DOWN, INPUT);
pinMode(LEFT, INPUT);
pinMode(RIGHT, INPUT);
pinMode(CLICK, INPUT);
pinMode(LED2, OUTPUT);
pinMode(LED3, OUTPUT);

//Pull analog pins high to enable reading of joystick movements
digitalWrite(UP, HIGH);
digitalWrite(DOWN, HIGH);
digitalWrite(LEFT, HIGH);
digitalWrite(RIGHT, HIGH);
digitalWrite(CLICK, HIGH);

//Write LED pins low to trun them off by default

digitalWrite(LED2, LOW);
digitalWrite(LED3, LOW);

}

long previousMillis = 0;

void loop() {
  //do something

  float p_step=0.001;
  //delay(200);
  if (digitalRead(UP) ==LOW)
  {
    //move motor forward
    p_in = p_in + p_step;
  }
  if (digitalRead(DOWN) ==LOW)
  {
    //move motor forward
    p_in = p_in - p_step;
  }
  p_in = constrain(p_in, P_MIN, P_MAX);
  if (digitalRead(RIGHT) ==LOW)
  {
    //Enable
    EnterMotorMode();
    digitalWrite(LED2, HIGH);
  }
  if (digitalRead(LEFT)==LOW)
  {
    ExitMotorMode();
    digitalWrite(LED2,LOW);
  }
  //send CAN
  pack_cmd();

  //receive CAN
  if(CAN_MSGAVAIL == CAN.checkReceive())
  {
   
    unpack_reply();
  }
  //print data
  Serial.print(millis()-previousMillis);
  previousMillis = millis();
  Serial.print(" ");
  Serial.print(p_in);
  Serial.print(" ");
  Serial.print(p_out);
  Serial.print(" ");
  Serial.print(v_out);
  Serial.print(" ");
  Serial.println(t_out);
}
void unpack_reply() {
/// CAB reply Oacjet 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: [position[7-0]]
/// 2: [velocity [11-4]]
/// 3: [velocity [3-0], current [11-8]]
/// 4: [current [7-0]]

  byte len = 0;
  byte buf[8];
  unsigned long canId;

  CAN.readMsgBuf(&canId, &len, buf); 
  //read data
  //MCP_CAN::readMsgBuf(*len,  buf[])

  
  /// 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_MAX, T_MAX, 12);
}
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] = 0xFF;
  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] = 0xFF;
  CAN.sendMsgBuf(0x01, 0, 8, buf);
}
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] = 0xFF;
  CAN.sendMsgBuf(0x01, 0, 8, buf);

}
void pack_cmd(){
  byte buf[8];
  ///CAN Command Packet Structure///
  ///16 bit position command, between -4*pi and 4*pi
  ///12 bit velocity command, between -30 and +30rad/s
  //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 toque, between -18 and 18 N-m
  ///CAN packet is 8 8-bit words
  ///Formatted as follows. For each quantity, bit i0 is LSB
  /// 0: [position[15-8[[
  ///1: [position [7-0]]
  ///2: [velocity[11-4]]
  ///3: [velocity[3-0], kp[11-8]]
  ///4: [kp[7-0]]
  ///5: [kd[11-4]]
  ///6:[kd[3-0], torque [11-8]]
  ///7: [torque [7-0]]

  ///limit data to be withing bounds///

  float p_des = constrain(p_in, P_MIN, P_MAX); ///fminf(fmaxf(P_MIN, p_in(, P_MAX);
  float v_des = constrain(v_in, V_MIN, V_MAX); ///fminf(fmaxf(V_MIN, v_in(, V_MAX);
  float kp = constrain(kp_in, KP_MIN, KP_MAX); ///fminf(fmaxf(KP_MIN, kp_in(, KP_MAX);
  float kd = constrain(kd_in, KD_MIN, KD_MAX); ///fminf(fmaxf(KD_MIN, kd_in(, KD_MAX);
  float t_ff = constrain(t_in, T_MIN, T_MAX); ///fminf(fmaxf(T_MIN, t_in(, V_MAX);

 ///convert floats to unsigned ints///
  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 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;
  CAN.sendMsgBuf(0x01, 0, 8, buf);
}


unsigned int float_to_uint(float x, float x_min, float x_max, int bits)
{
  ///Converts 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 unsigned int to float, given range and number of bits///
  float span = x_max-x_min;
  float offset = x_min;
  float pgg = 0;
  if (bits==12){
    pgg = ((float) x_int)*span/4095 + offset;
  }
  if (bits==16){
    pgg = ((float) x_int)*span/65535.0 + offset;
  }
  return pgg;

}

@Zainroid @WidosFTW @MYBOTSHOP_Support @Darrell @Joy @jorgensao @Nate @phribeiro @Rick @SubashHSA

Hello guys, have you faced similar problems, do you have any ideas on how to solve it?

Basically,my code and hardware setup is from the following video
Gravity compensation and Spherical robot joint with AK60-6 actuator - YouTube

@axel hi, do you have any suggestions on this one?

Hi @Akim , does a blue LED comes on when you plug the power in? ALso, have you tried running the motor with R-Link / CubemarsTool?
If you’re sure your hardware is good, try the code sent by @Darell : Velocity_Demo.ino - Google Drive, it worked for me. After that, you can adapt it to your project.

axel

Thanks for your help. Bro
You description is totally correct.

Hi Axel, thanks very much, appreciate. Yes, the blue light is on, can you tell me about the connection for this code? does it use different can bus module?

Yes, it works with Cubemars software

at some point of time I thought I figured out the error in my code, now the power just shuts down after I enter motor mode with Arduino, but with Cubemars it works fine… very capricious motor. So exhausted with dealing with it.