Difficulties with Torque Control

developement
, ,
1

Hello FOC Community,

I’m working on a project using the SparkFun IoT Brushless Motor Driver with an integrated ESP32 and the TMC6300 motor driver. I’m utilizing FOC (Field Oriented Control) along with Torque Control for precise BLDC motor management. My goal is to simulate fictitious positions at different angles by turning the BLDC motor, where each designated position requires an increased torque value compared to a constant preset torque value for all other positions.

Problem Description:

I am encountering an issue where my code does not seem to adjust the torque values according to my requirements. Specifically, I wish to set higher torque values at specific target angles while maintaining a constant preset torque value at all other positions. However, my current implementation does not achieve this differentiation in torque settings effectively.

Objective:

  1. Primary Goal: To achieve precise torque control at specific target positions (angles) while maintaining a constant torque elsewhere.
  2. Desired Outcome: For the motor to exhibit increased torque at designated positions and a preset torque value at all other angles. (see attached graph)

Current Approach:

I’ve implemented the FOC and Torque Control algorithm as per the guidance available in the SimpleFOC library documentation. My setup involves using the TMAG5273 hall-effect sensor for position feedback and the INA240A1 for current sensing, facilitating the torque control loop.

Challenges Faced:

  • Inability to consistently apply different torque settings at specific angles.
  • Difficulty in integrating the positional feedback with the torque control to achieve the desired behavior.

Request for Assistance:

Could anyone guide me on how to correctly implement variable torque settings at designated positions using FOC and Torque Control? I believe my challenge lies in effectively integrating positional feedback to adjust torque values dynamically.

Hardware and Software Details:

  • Motor Driver: SparkFun IoT Brushless Motor Driver (TMC6300)
  • MCU: ESP32 WROOM
  • Sensors: TMAG5273 (Position), INA240A1 (Current Sensing)
  • Development Environment: PlatformIO

Thank you in advance for your time and assistance. I’m looking forward to your valuable insights and suggestions to overcome this challenge.

Best regards,
Chin

Expected Torque vs Angle Graph
Expected Torque vs Angle Graph886×528 57.6 KB

#include <Arduino.h>
#include <SimpleFOC.h>
#include <BLDCMotor.h>
#include <SparkFun_TMAG5273_Arduino_Library.h>
#include "../lib/Hall_Effect_Sensor.h"

// Global Constants
const float stallCurrent = 0.8; // Stall current in A
const float maxTorque = 0.0032; // Max torque in Nm (320 g.cm)
const float Kt = 0.004; // Torque constant in Nm/A
const float increasedTorquePercentage = 0.50; // 50% for increased torque
const float normalTorquePercentage = 0.05; // 5% for normal torque

const float correctionFactor = 8;
float lastAngleDegrees = 0; 
float currentAngleDegrees = 0;
bool isFirstLoop = true;
String lastDirection = "Unknown";

float initialAngleOffset = 0;
float targetCurrent = 0;

// Predefined positions for increased torque and tolerance
const int fictitiousPositions[] = {90, 180, 270};
const int numberOfPositions = sizeof(fictitiousPositions) / sizeof(fictitiousPositions[0]);
const float angleTolerance = 5.0; // Degrees in tolerance for detecting increased torque positions
const float rampWidth = 2.0; // Width of the ramp in degrees

// Motor setup
const int polePairs = 4;
BLDCMotor motor = BLDCMotor(polePairs);
BLDCDriver6PWM driver = BLDCDriver6PWM(16, 17, 18, 23, 19, 33);

// Hall sensor object
TMAG5273Adapter Hall_Sensor;

// Current sensor setup
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 20, 35, 36, 39);

// Function Prototypes........................................................................................//
String detectRotationDirection(float& lastAngleDegrees, float currentAngleDegrees, bool& isFirstLoop);
float angleMeasurement();
float calculateMotorTarget(float angle);
float calculateTorqueNm(float angle);
void displaySerialInfo(String direction, float angle, float torqueNm);
float interpolate(float startValue, float angle, float gradient);
void testHallSensorReading();
void testTorqueCalculation();
void testTargetCurrentApplication();
void testHallSensorReadingWithUpdate();

void setup() {
  Serial.begin(115200);
  Wire.begin();
  delay(100);
  
  // Motor driver setup
  driver.voltage_power_supply = 5;
  driver.voltage_limit = 7.8;
  driver.init();
  motor.linkDriver(&driver);
  
  // Hall sensor initialization
  Hall_Sensor.init();
  motor.linkSensor(&Hall_Sensor);
  delay(100);
  
  // Current sensor setup
  current_sense.init();
  motor.linkCurrentSense(&current_sense);
  current_sense.linkDriver(&driver);

  // FOC algorithm setup
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.controller = MotionControlType::torque;
  motor.torque_controller = TorqueControlType::foc_current;

  // PID setup for current control
  motor.PID_current_q.P = 0.1;
  motor.PID_current_q.I = 10;
  motor.PID_current_q.D = 0.001;
  motor.PID_current_q.limit = 1;
  motor.LPF_current_q.Tf = 0.01;
  
  // Initialize motor
  motor.init();
  motor.initFOC();

  // Hall Sensor Alignment to point zero
  initialAngleOffset = Hall_Sensor.getAngle();

}


void loop() {
  float angle_deg = angleMeasurement();
  String direction = detectRotationDirection(lastAngleDegrees, angle_deg, isFirstLoop);
  float torqueNm = calculateTorqueNm(angle_deg);

  motor.target = driver.voltage_power_supply * calculateMotorTarget(angle_deg);
  motor.loopFOC();
  motor.move(motor.target);
  
  displaySerialInfo(direction, angle_deg, torqueNm); 
  delay(100);
}


String detectRotationDirection(float& lastAngleDegrees, float currentAngleDegrees, bool& isFirstLoop) {
    if (!isFirstLoop) {
        float deltaAngle = currentAngleDegrees - lastAngleDegrees;
        if (deltaAngle < -180) deltaAngle += 360;
        else if (deltaAngle > 180) deltaAngle -= 360;

        if (deltaAngle > 0) lastDirection = "CW";
        else if (deltaAngle < 0) lastDirection = "CCW";
    } else {
        isFirstLoop = false;
    }
    lastAngleDegrees = currentAngleDegrees;
    return lastDirection;
}

float angleMeasurement() {
    float angleRadians = (Hall_Sensor.getAngle() - initialAngleOffset) / (motor.pole_pairs * correctionFactor);
    float angleDegrees = fmod(angleRadians * (180.0 / PI), 360.0);
    if (angleDegrees < 0) {
        angleDegrees += 360;
    }
    return angleDegrees;
}


float calculateMotorTarget(float angle) {
    float targetCurrent = stallCurrent * normalTorquePercentage;
    for (int i = 0; i < numberOfPositions; i++) {
        float pos = fictitiousPositions[i];
        float rampUpBegin = pos - angleTolerance;
        float rampUpEnd = rampUpBegin + rampWidth;
        float rampDownBegin = pos + rampWidth;
        float rampDownEnd = rampDownBegin + angleTolerance;

        if (angle >= rampUpBegin && angle <= rampDownEnd) {
            if (angle >= rampUpBegin && angle < rampUpEnd) {
                float progress = (angle - rampUpBegin) / (rampUpEnd - rampUpBegin);
                targetCurrent = stallCurrent * (normalTorquePercentage + (increasedTorquePercentage - normalTorquePercentage) * progress);
                Serial.println("Rising Torque");
            } else if (angle > rampDownBegin && angle <= rampDownEnd) {
                float progress = (angle - rampDownBegin) / (rampDownEnd - rampDownBegin);
                targetCurrent = stallCurrent * (increasedTorquePercentage - (increasedTorquePercentage - normalTorquePercentage) * progress);
                Serial.println("Falling Torque");
            } else if (angle > rampUpEnd && angle < rampDownBegin) {
                targetCurrent = stallCurrent * increasedTorquePercentage;
                Serial.println("Increased Torque");
            }
            break;
        }
    }    
    return targetCurrent;
}


float calculateTorqueNm(float angle) {
    float current = calculateMotorTarget(angle);
    return current * Kt * 10197.16; //torque in gcm
}

void displaySerialInfo(String direction, float angle, float torqueNm) {
    Serial.print(direction);
    Serial.print("\t");
    Serial.print(angle);
    Serial.print("\t");
    Serial.println(torqueNm, 5);
}


void scanI2CDevices() {
  byte error, address;
  int nDevices;

  Serial.println("Scanning...");

  nDevices = 0;
  for (address = 1; address < 127; address++ ) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address < 16) Serial.print("0");
      Serial.print(address, HEX);
      Serial.println("  !");
      nDevices++;
    } else if (error == 4) {
      Serial.print("Unknown error at address 0x");
      if (address < 16) Serial.print("0");
      Serial.println(address, HEX);
    }
  }
  if (nDevices == 0) Serial.println("No I2C devices found\n");
  else Serial.println("done\n");
}

void testHallSensorReading() {
    float angle_deg = angleMeasurement();
    Serial.print("Hall Sensor Angle: ");
    Serial.println(angle_deg);
    delay(500); 
}

void testTorqueCalculation() {
    float angle_deg = angleMeasurement();
    float targetCurrent = calculateMotorTarget(angle_deg);
    float torqueGcm = calculateTorqueNm(angle_deg);
    Serial.print("Angle: "); Serial.println(angle_deg);
    Serial.print("Target Current: "); Serial.println(targetCurrent);
    Serial.print("Calculated Torque (Nm): "); Serial.println(torqueGcm);
    delay(500); // Adjust the delay as needed for readability
}

void testTargetCurrentApplication() {
    float angle_deg = angleMeasurement();
    motor.target = calculateMotorTarget(angle_deg);
    Serial.print("Angle: "); Serial.println(angle_deg);
    Serial.print("Motor Target (Current): "); Serial.println(motor.target);
    motor.loopFOC(); 
    delay(500); 
}