Hi everyone, I’m very new to motors, motor controllers, robotics, etc., so I would really appreciate any help.
I am creating an adaptive bicep curl machine that senses what the user is pulling (using a load cell + load cell amplifier) and slightly underperforms that weight so that the user is pulling at their max weight for each rep. I am using an ODrive S1 with a 1.4Nm 440W BLDC Motor + 20:1 Gearbox. I am powering these with the Meanwell LRS-600-48. For the code, I am using an ESP32, powered using a buck converter. I also have set up the ODrive with the brake resistor and other necessary options in the GUI.
Right now, I am running into a few problems, all of which I think are coding issues, but I might be wrong. Firstly, when pulling the cable, if I pull too fast, the ODrive flashes red and blue, the cable goes limp, and then goes back to flashing green and working as intended. Secondly, when I stop applying force on the cable, the retract force is (I think) matching the applied force instead of measuring around 0 lbs. What I wanted to do was let the cable reel back in slowly when the user is not applying any force.
This is my code. I would appreciate any help. Thank you!
#include <ODriveUART.h>
#include <HardwareSerial.h>
#include <HX711_ADC.h>
#include <LiquidCrystal_I2C.h>
#include <math.h>
// Pin Definitions
#define ESP_RX 26
#define ESP_TX 27
#define HX711_DAT 18
#define HX711_CLK 19
#define LCD_SDA 21
#define LCD_SCL 22
// Constants
#define BAUD_ODRIVE 115200
#define CAL_FACTOR 976.56f
#define MAX_TORQUE_NM 6.75f
#define SPOOL_RADIUS 0.0635f
#define RESISTANCE_PCT 0.07f
#define MOTOR_CURRENT_LIMIT 20.0f
#define MOTOR_VEL_LIMIT 200.0f
#define RETRACT_SPEED 2.0f
#define TORQUE_RAMP_RATE 0.0055f
#define RETRACT_THRESHOLD 5.0f
// Objects
HardwareSerial odrive_serial(2);
ODriveUART odrive(odrive_serial);
HX711_ADC loadCell(HX711_DAT, HX711_CLK);
LiquidCrystal_I2C lcd(0x27,16,2);
// Variables
unsigned long startup_time = 0;
bool motorEnabled = false;
bool lifting = false;
float lbs = 0;
float filteredTorque = 0;
// ODrive Fault Check
void checkODriveFault()
{
if(odrive.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL)
{
Serial.println("ODrive left closed loop!");
Serial.print("Axis Error: ");
Serial.println(odrive.getParameterAsFloat("axis0.error"));
Serial.print("Motor Error: ");
Serial.println(odrive.getParameterAsFloat("axis0.motor.error"));
Serial.print("Controller Error: ");
Serial.println(odrive.getParameterAsFloat("axis0.controller.error"));
odrive.clearErrors();
delay(100);
odrive.setState(AXIS_STATE_CLOSED_LOOP_CONTROL);
delay(200);
}
}
// Setup
void setup()
{
Serial.begin(115200);
delay(10);
lcd.init();
lcd.backlight();
lcd.setCursor(0,0);
lcd.print("Initializing");
loadCell.begin();
loadCell.start(2000,true);
if(loadCell.getTareTimeoutFlag())
{
Serial.println("Load cell error");
while(1)
{
delay(10);
}
}
loadCell.setCalFactor(CAL_FACTOR);
Serial.println("Load cell initialized");
odrive_serial.begin(BAUD_ODRIVE, SERIAL_8N1, ESP_RX, ESP_TX);
delay(100);
unsigned long timeout = millis();
bool connected = false;
while(millis()-timeout < 3000)
{
if(odrive.getState() != AXIS_STATE_UNDEFINED)
{
connected = true;
break;
}
Serial.println("Waiting for ODrive");
delay(250);
}
if(!connected)
{
Serial.println("ODrive communication failed");
while(1)
{
delay(10);
}
}
Serial.println("ODrive connected");
odrive.clearErrors();
odrive.setParameter("axis0.motor.config.current_lim", MOTOR_CURRENT_LIMIT);
odrive.setParameter("axis0.controller.config.vel_limit", MOTOR_VEL_LIMIT);
startup_time = millis();
lcd.clear();
lcd.print("Wait 3 sec");
}
// Loop
void loop()
{
if(loadCell.update())
{
float rawLbs = fabs(loadCell.getData());
lbs = lbs * 0.85f + rawLbs * 0.15f;
if(lbs < RETRACT_THRESHOLD)
{
lbs = 0;
}
}
if(millis()-startup_time < 3000)
{
return;
}
if(!motorEnabled)
{
Serial.println("Enabling motor");
odrive.setState(AXIS_STATE_CLOSED_LOOP_CONTROL);
delay(500);
motorEnabled = true;
Serial.println("Motor enabled");
}
checkODriveFault();
if(lbs > RETRACT_THRESHOLD)
{
if(!lifting)
{
odrive.setParameter("axis0.controller.config.control_mode", 1.0f);
odrive.setParameter("axis0.controller.config.input_mode", 1.0f);
lifting = true;
filteredTorque = 0;
}
float newtons = lbs * 4.448f;
float torque_command = (newtons * SPOOL_RADIUS) * RESISTANCE_PCT;
torque_command = constrain(torque_command, 0.0f, MAX_TORQUE_NM);
// Adaptive ramping
float torque_diff = torque_command - filteredTorque;
float adaptive_ramp = TORQUE_RAMP_RATE;
// If user pulls hard and fast, ramp faster
if(fabs(torque_diff) > 1.0f)
{
adaptive_ramp = TORQUE_RAMP_RATE * 3.0f;
}
if(filteredTorque < torque_command)
{
filteredTorque += adaptive_ramp;
}
else
{
filteredTorque -= adaptive_ramp;
}
filteredTorque = constrain(filteredTorque, 0.0f, torque_command);
odrive.setTorque(
filteredTorque
);
}
else
{
if(lifting)
{
filteredTorque = 0;
lifting = false;
delay(100);
}
odrive.setParameter("axis0.controller.config.control_mode", 2.0f);
odrive.setParameter("axis0.controller.config.input_mode", 1.0f);
odrive.setParameter("axis0.controller.input_vel", RETRACT_SPEED);
}
lcd.setCursor(0,0);
lcd.print("Force:");
lcd.setCursor(8,0);
lcd.print(" ");
lcd.setCursor(8,0);
lcd.print(lbs,1);
lcd.print(" lb");
lcd.setCursor(0,1);
lcd.print("Torque:");
lcd.setCursor(8,1);
lcd.print(" ");
lcd.setCursor(8,1);
lcd.print(filteredTorque,2);
lcd.print(" Nm");
Serial.print("Force: ");
Serial.print(lbs);
Serial.print(" lb Torque: ");
Serial.print(filteredTorque);
Serial.print(" Nm Mode: ");
Serial.println(lifting ? "Lift" : "Retract");
}