Closed Loop to Desired Angle

applications
, ,
1

Hello All!

I’m new to implementing SimpleFOC into projects and I would like some help :slight_smile: . I have a BLDC motor that I am controlling with MotionControlType::angle; I also have two encoders (input and output). I am using an absolute encoder on the output to give absolute position data to the incremental input encoder at startup. I am also using genericSensor(); for the encoders that I have tested independent of motor linking within SimpleFOC. So all seemed to be going well until I tried giving the commands to the motor because the motor is not moving to the position command.

From my understanding the code (using MotionControlType::angle) should take an angle and the motor will move in the correct direction until the encoder reads that position. Is this correct? Upon further inspection, the behavior that it is exhibiting is one where the motor follows the encoders movement rather than trying to move to the specified angle.

Am I misunderstanding the use of the MotionControlType()? Or am I implementing it wrong? Any advice on the direction to go or how to approach this would be much appreciated.

This is my code: (I commented put the output encoder code to trouble shoot the input encoder).

#include <SimpleFOC.h>
#include "HEDS9140.h"
// #include "MA732_OffAxis.h" // Comment out the MA732 encoder include

// Motor Parameters
float pp = 11;  // number of pole pairs
float R = 11.8; // [Ohm]
int gear_ratio = 100;


// RP GPIO Pins
int PWM_A = 5;
int PWM_B = 4;
int PWM_C = 3;
int EN = 2;



// MA732 Output Encoder
// MA732_OffAxis encoder1; // Create instance of the encoder
// void initMySensorCallback1() {
//   encoder1.begin();
// }

// float readMySensorCallback1() {
//   return encoder1.calculateAngle();
// }

// create instance of MA732 encoder
// GenericSensor output_encoder = GenericSensor(readMySensorCallback1, initMySensorCallback1);

// Quadrature Input Encoder Pins
int CH_A = 14;
int CH_B = 15;
int Index = 13; // HEDS Input Encoder
HEDS9140 encoder0(CH_A, CH_B, Index);

void initMySensorCallback0() {
  encoder0.begin(); 
}

float readMySensorCallback0() {
  encoder0.update(); // Update the encoder position
  return encoder0.calculateAngle(); // unbounded angle in radians

}

// create the sensor
GenericSensor input_encoder = GenericSensor(readMySensorCallback0, initMySensorCallback0);

// // BLDC MOTOR & driver instance
BLDCMotor motor = BLDCMotor(pp,R);
BLDCDriver3PWM driver = BLDCDriver3PWM(PWM_A, PWM_B, PWM_C, EN);


// angle set point variable
float target_angle = 0; // Target Angle in Radians

// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { 
  float angle_degrees = atof(cmd);  // Convert string to float
  // Calculate the corresponding input angle based on the gear ratio
  float input_angle = angle_degrees * gear_ratio; // Multiply by gear ratio

  // Convert degrees to radians for the motor
  target_angle = input_angle * PI / 180.0f;  // Convert to radians

  Serial.print("Moving output to ");
  Serial.print(angle_degrees);
  Serial.println(" degrees");
}

void initializePosition() {
  // Read the absolute position from the MA732 encoder
  float absolute_position = 0.0;
  // float absolute_position = output_encoder.getAngle(); // unbounded angle in radians
  // Also if you are bounding it in HEDS.cpp you could just bound it here with getMechanicalAngle()

  // Call the initializePosition method on the HEDS encoder instance
  encoder0.initializePosition(absolute_position, gear_ratio); // Pass the absolute position, gear ratio
}

void setup() {

  Serial.begin(115200);
  SimpleFOCDebug::enable(&Serial);  // Debugging Mode

  // initialize Input Encoder (HEDS)
  input_encoder.init();
  motor.linkSensor(&input_encoder);

  // Initialize the position based on the absolute encoder
  initializePosition();

  // initalize Output Encoder
  // output_encoder.init(); //initalize output but do not link to motor


  // driver config
  driver.voltage_power_supply = 20;  // Set to motor's voltage limit
  driver.init();
  motor.linkDriver(&driver);

  // Motion Control
  motor.controller = MotionControlType::angle;

  // velocity PI controller parameters
  motor.PID_velocity.P = 0.2f;
  motor.PID_velocity.I = 20;
  motor.PID_velocity.D = 0;

  // angle P controller parameters
  motor.P_angle.P = 20;

  // default voltage_power_supply
  motor.voltage_limit = 15;
  motor.PID_velocity.output_ramp = 1000; // jerk control using voltage voltage ramp & default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.velocity_limit = 4;  // angle loop velocity limit
  // Add sensor alignment voltage
  motor.voltage_sensor_align = 5;  // Increased alignment voltage
  // motor.current_limit = ; // Effectively the same limitation as velocity_limiy (V=IR)
  motor.useMonitoring(Serial);  // comment out if not needed
  // Initialize motor & FOC
  motor.init();
  motor.initFOC();


  // Target Angle
  command.add('T', doTarget, "target angle (degrees)");
  // Add homing command
  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target angle in degrees using serial terminal:"));
  Serial.println(F("Commands:"));
  Serial.println(F(" T<angle> - Move to angle in degrees"));
  _delay(1000);

}

void loop() {

  input_encoder.update();
  
  Serial.print("Input getAngle: ");
  Serial.print(input_encoder.getAngle()*180/PI, 2);
  Serial.println("°");
  // Serial.print("\t");
  // Serial.print("Output getAngle: ");
  // Serial.println(output_encoder.getAngle()*180/PI,2);

  
  motor.loopFOC();
  motor.move(target_angle);
  command.run();
}