Hello guys , I have recently bought GIM 8115-9 actuator from Steadywin, it has a built in motor driver based on Ben Katz documentation ( MIT Mini Cheetah Creator) , I have already updated its firmware according to my needs and currently trying to run it via Arduino Mega interfaced with a MCP2515 CAN module. I have been unsuccessful so far and the worst part is that there is little to no information about these motors online.
I searched on youtube and on forums for hours and the closest thing I got was Skyentific's videos regarding a previous version of the motors which he ran via a Sparkfun CAN Bus Module (its unavailable currently in the market) . Even he used the built-in joystick of the module to move the motor, however, I want to do it autonomously.
I configured the code provided by him based off Ben Katz original CAN master test code but the motor just wont run. Its not even going into motor mode via arduino. I have to manually put it into the motor mode again and again via its firmware settings through Tera Term software. Despite clearly using the initializing commands provided for it to run in normal state. Please help me out, I have already double checked the connections, everything is fine.
The Code I am using is this:
#include <MCP_CAN.h>
#include <SPI.h>
// 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 -1.0f
#define T_MAX 1.0f
// Set Values
float p_in = 0.0f;
float v_in = 0.0f;
float kp_in = 100.0f;
float kd_in = 1.0f;
float t_in = 0.0f;
// Measured values
float p_out = 0.0f;
float v_out = 0.0f;
float t_out = 0.0f;
// cs pin
const int SPI_CS_PIN = 53;
MCP_CAN CAN(SPI_CS_PIN);
void setup() {
Serial.begin(115200);
delay(1000);
while(CAN_OK != CAN.begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ)) {
Serial.println("CAN BUS Shield init fail");
Serial.println("Init CAN BUS Shield again");
delay(100);
}
Serial.println("CAN BUS Shield init ok");
}
long previousMillis = 0;
int cycle = 0;
void loop() {
unsigned long currentMillis = millis();
if (cycle < 2) {
if (currentMillis - previousMillis >= 1000) { // Change position every second
previousMillis = currentMillis;
if (p_in < P_MAX) {
p_in += 1.0f;
} else {
p_in = P_MIN;
cycle++;
}
p_in = constrain(p_in, P_MIN, P_MAX);
EnterMotorMode();
pack_cmd();
}
} else {
ExitMotorMode();
}
// Receive CAN
if(CAN_MSGAVAIL == CAN.checkReceive()) {
unpack_reply();
}
// Print data
Serial.print(millis() - previousMillis);
Serial.print(" ");
Serial.print(p_in);
Serial.print(" ");
Serial.print(p_out);
Serial.print(" ");
Serial.print(v_out);
Serial.print(" ");
Serial.print(t_out);
Serial.println();
}
void EnterMotorMode() {
// Enter Motor Mode (enable)
byte buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};
CAN.sendMsgBuf(0x01, 0, 8, buf);
}
void ExitMotorMode() {
// Disable motor
byte buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};
CAN.sendMsgBuf(0x01, 0, 8, buf);
}
void Zero() {
// Zero
byte buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 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);
// 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);
}
void unpack_reply() {
byte len = 0;
byte buf[8];
long unsigned int id;
CAN.readMsgBuf(&id, &len, buf);
// Unpack ints from CAN buffer
unsigned int p_int = (buf[0] << 8) | buf[1];
unsigned int v_int = (buf[2] << 4) | (buf[3] >> 4);
unsigned int i_int = ((buf[3] & 0xF) << 8) | buf[4];
// Convert uint 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);
}
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;
return (unsigned int) ((x - offset) * ((float)((1 << bits) - 1) / span));
}
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;
return ((float)x_int * span / (float)((1 << bits) - 1)) + offset;
}
Please help me out, its very urgent, and I cant replace the motor or its driver anymore due to lack of funds as it was a pretty hefty investment for me.
