Hii all, I am having problem with AK80-9(24v 25Amp) motor when I switch in servo mode through R-link it giving scary vibration until switch off the power supply
and in MIT mode also giving sinusoidal actuation
please me for firmware update. I don’t have copy of firmware
even not able to control through mcp2515 can module.
I am using Arduino mega
connection is : 
INT – to-- D2
CS --to-- D53
SCK --to-- D52
MOSI–to–D51
MISO --to–D50
VCC to 5v VCC
GND to GND
CAN H of module to CAN H of MOTOR
CAN L of module to CAN L of MOTOR
POWER supply 24V 25A to motor
and also did 120 ohms resistor termination in between CAN H and CAN L.
#include <SPI.h>
#include <mcp_can.h>
/*
Author: Mihai Moldovanu
Youtube: https://www.youtube.com/channel/UC1CS67BvP8PaVJRK6wxhbCg
Mail: mihaim@tfm.ro
*/
// Mini cheetah actuator testing code
//
// Hardware needed: Wemos d1 mini + mcp215 can
// Last Tested : 24 ian 2021
// 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 = 5.0f;
float kp_in = 10.0f;
float kd_in = 0.20f;
float t_in = 0.0f;
// measured values
float p_out = 0.0f;
float v_out = 0.0f;
float t_out = 0.0f;
long previousMillis = 0;
// Wemos D1 mini CS pin is D8 ( translated to pin 15 )
const int spiCSPin = 53; // 53 for arduino mega
MCP_CAN CAN(spiCSPin);
// Motor Mode on command
byte MotorModeOn[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc};
// Motor Mode off command
byte MotorModeOff[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfd};
void setup()
{
Serial.begin(115200);
Serial.println("CANBUS firmware 1.0");
while (CAN_OK != CAN.begin(MCP_ANY,CAN_1000KBPS,MCP_16MHZ)) {
Serial.println("CAN BUS init Failed");
delay(100);
}
Serial.println("CAN BUS Shield Init OK!");
// Enable motor mode
while ( CAN_OK != CAN.sendMsgBuf(0x01, 0, 8, MotorModeOn))
{
Serial.println("Error enabling motor mode");
delay(1000);
}
}
int a = 0;
void loop()
{
// Moving faster to 6.0 and slow back to 0
if ( a == 0 )
p_in = p_in + 0.5;
else
p_in = p_in - 0.1;
if ( p_in > 6 )
a = 1;
if ( p_in < 0.2 )
a = 0;
pack_cmd();
// receive CAN
if(CAN_MSGAVAIL == CAN.checkReceive()) {
unpack_reply();
//print data
Serial.print(p_in);
Serial.print(" ");
Serial.print(p_out);
Serial.print(" ");
Serial.print(v_out);
Serial.print(" ");
Serial.println(t_out);
}
// during testing don't issue commands to fast
delay(200);
}
/*
Pack command and send it to canbus
*/
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 + 30 rad/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 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[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 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);
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);
}
/*
Unpack message received on canbus
*/
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: [position[7-0]]
// 2: [velocity[11-4]]
// 3: [velocity[3-0], current[11-8]]
// 4: [current[7-0]]
long unsigned int rxId;
byte len = 0;
byte buf[8];
CAN.readMsgBuf(&rxId, &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);
}
/*
Converts a float to an unsigned int, given range and number of bits
*/
unsigned int float_to_uint(float x, float x_min, float x_max, int 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;
}
/*
Converts unsigned int to float, given range and number of bits
*/
float uint_to_float(unsigned int x_int, float x_min, float x_max, int 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;
}