I am currently working on a foot robot and using RMD X15-400 motor from my actuator using arduino mega. The motor uses CAN communication , and has a VESC driver. the thing is while I was testing the motor via VESC tool , the torque outputs I received were really good , especially its static torque , I tried my best to push down using a moment arm but was unable to do so, however, I am trying to apply the same thing using arduino mega now using the motors' Force-position hybrid mode , but there is way too much damping and not enough torque , regardless of whatever changes I make to the KP and KD , I tried including feedforward torque in the code too , but that just overshoots the position , its really confusing and concerning, please someone help me out, I am sharing my code below:
#include <SPI.h>
#include <mcp_can.h>
// Value Limits for Parameters
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -18.0f
#define V_MAX 18.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -150.0f
#define T_MAX 150.0f
float kp_in = 250.0f;
float kd_in = 3.75f;
float v_in = 0.0f;
float t_ff = 0.0f;
// CS Pin for MCP2515
const int spiCSPin = 49;
MCP_CAN CAN(spiCSPin);
// CAN ID
const long CAN_ID = 0x02; // Motor ID
// Global variable to remember last commanded position
float lastPosition = 0.0f;
void setup() {
Serial.begin(115200);
if (CAN.begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ) == CAN_OK) {
Serial.println("MCP2515 Initialized Successfully!");
} else {
Serial.println("Error Initializing MCP2515...");
while (1);
}
CAN.setMode(MCP_NORMAL);
Serial.println("CAN Module set to NORMAL mode.");
}
void loop() {
// Always check and print CAN feedback continuously
if (CAN_MSGAVAIL == CAN.checkReceive()) {
unpack_reply();
}
printFeedback();
if (Serial.available() > 0) {
char command = Serial.read();
if (command == 'e') {
// "e": Ramp to 0 slowly, wait 2 s, then ramp to 90° (1.5708 rad) and hold
delay(1500);
if (rampToPosition(lastPosition, 0.0f)) {
delay(1500); // 2-second delay at 0 rad
if (rampToPosition(0.0f, 0.19198621f)) {
holdPosition(0.19198621f);
}
}
}
else if (command == 'g') {
// "g": Ramp to 0 slowly and hold there
if (rampToPosition(lastPosition, 0.0f)) {
holdPosition(0.0f);
}
}
else if (command == 'f') {
// "f": Immediately stop the motor
stopMotor();
}
}
}
//----------------------------------------------------
// Helper: Ramp the motor from startPos to targetPos slowly.
// Returns false if aborted because 'f' was pressed.
bool rampToPosition(float startPos, float targetPos) {
const int steps = 25; // number of incremental steps
float stepSize = (targetPos - startPos) / steps;
for (int i = 1; i <= steps; i++) {
// immediate stop even during ramping:
if (Serial.available() > 0) {
char newCommand = Serial.read();
if (newCommand == 'f') {
stopMotor();
return false; // abort ramping
}
}
float pos = startPos + i * stepSize;
sendCmd(CAN_ID, pos, v_in, 12.0, 0.5, t_ff);
lastPosition = pos;
delay(100);
printFeedback();
}
return true;
}
// Helper: Hold the motor at the specified position until a new command (f, e, or g) is received.
void holdPosition(float pos) {
lastPosition = pos;
while (true) {
// Check if a new command has been pressed:
if (Serial.available() > 0) {
char newCommand = Serial.read();
if (newCommand == 'f') {
stopMotor();
return;
}
else if (newCommand == 'g') {
// If "g" is pressed while holding, ramp to 0 and then hold at 0.
if (rampToPosition(lastPosition, 0.0f)) {
holdPosition(0.0f);
}
return;
}
else if (newCommand == 'e') {
// If "e" is pressed while holding, restart the e sequence.
if (rampToPosition(lastPosition, 0.0f)) {
delay(2000);
if (rampToPosition(0.0f, 0.19198621f)) {
holdPosition(0.19198621f);
}
}
return;
}
}
// Continually command the current holding position:
sendCmd(CAN_ID, pos, v_in, kp_in, kd_in, t_ff);
delay(100);
printFeedback();
}
}
// Stop the motor immediately.
void stopMotor() {
sendCmd(CAN_ID, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
lastPosition = 0.0f;
Serial.println("Motor stopped.");
}
// Function to send motor command via CAN
void sendCmd(long canId, float pos, float vel, float kp, float kd, float torque) {
byte buf[8];
unsigned int p_int = float_to_uint(pos, P_MIN, P_MAX, 16);
unsigned int v_int = float_to_uint(vel, 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, 9);
unsigned int t_int = float_to_uint(torque, T_MIN, T_MAX, 12);
buf[0] = 0x00 | (kp_int >> 7);
buf[1] = ((kp_int & 0x7F) << 1) | ((kd_int & 0x100) >> 8);
buf[2] = kd_int & 0xFF;
buf[3] = p_int >> 8;
buf[4] = p_int & 0xFF;
buf[5] = v_int >> 4;
buf[6] = ((v_int & 0x0F) << 4) | (t_int >> 8);
buf[7] = t_int & 0xFF;
CAN.sendMsgBuf(canId, 0, 8, buf);
}
// Function to unpack and process feedback from CAN bus
void unpack_reply() {
long unsigned int rxId;
byte len = 0;
byte buf[8];
CAN.readMsgBuf(&rxId, &len, buf);
// Extract data from buffer
uint8_t message_type = (buf[0] >> 5) & 0x07; // First 3 bits of buf[0]
uint8_t error_msg = buf[0] & 0x1F; // Last 5 bits of buf[0]
unsigned int output_pos = (buf[1] << 8) | buf[2]; // Motor output position (16 bits)
unsigned int output_speed = ((buf[3] << 4) | (buf[4] >> 4)) & 0x0FFF; // Motor output speed (12 bits)
unsigned int phase_curr = ((buf[4] & 0x0F) << 8) | buf[5]; // Motor phase current (12 bits)
uint8_t motor_temp = buf[6]; // Motor temperature (8 bits)
uint8_t mos_temp = buf[7]; // MOS temperature (8 bits)
// Convert motor temperature and MOS temperature to actual values
float actual_motor_temp = (motor_temp - 50.0) / 2.0;
float actual_mos_temp = (mos_temp - 50.0) / 2.0;
// Convert unsigned ints to float values for position and speed
float actual_position = uint_to_float(output_pos, P_MIN, P_MAX, 16);
float actual_speed = uint_to_float(output_speed, V_MIN, V_MAX, 12);
float actual_phase_current = uint_to_float(phase_curr, -200.0f, 200.0f, 12);
float torque = (actual_phase_current * 2.32 ); //for torque
Serial.print("Message Type: ");
Serial.print(message_type);
Serial.print(", Error Message: ");
Serial.print(error_msg);
Serial.print(" Output Position: ");
Serial.print(actual_position, 16);
Serial.print(" rad, Speed: ");
Serial.print(actual_speed, 2);
Serial.print(" rad/s, Current= ");
Serial.print(actual_phase_current, 2);
Serial.print(" A, Torque: ");
Serial.print(torque, 2);
Serial.print(" Nm/A, Motor Temp: ");
Serial.print(actual_motor_temp, 1);
Serial.print(" °C, MOS Temp: ");
Serial.print(actual_mos_temp, 1);
Serial.println(" °C");
Serial.println();
}
// Helper function to print motor feedback
void printFeedback() {
if (CAN_MSGAVAIL == CAN.checkReceive()) {
unpack_reply();
}
}
// Conversion from float to uint
unsigned int float_to_uint(float x, float x_min, float x_max, int bits) {
float span = x_max - x_min;
unsigned int result = 0;
if (bits == 9) {
result = (unsigned int)((x - x_min) * 511.0 / span);
} else if (bits == 12) {
result = (unsigned int)((x - x_min) * 4095.0 / span);
} else if (bits == 16) {
result = (unsigned int)((x - x_min) * 65535.0 / span);
}
return result;
}
// Conversion from uint to float
float uint_to_float(unsigned int x_int, float x_min, float x_max, int bits) {
float span = x_max - x_min;
float result = 0;
if (bits == 9) {
result = ((float)x_int) * span / 511.0 + x_min;
} else if (bits == 12) {
result = ((float)x_int) * span / 4095.0 + x_min;
} else if (bits == 16) {
result = ((float)x_int) * span / 65535.0 + x_min;
}
return result;
}