sensor.getAngle() works correctly but motor.shaft_angle is always zero. 'MOT: Init FOC failed' as a result

developement
1

I’m having difficulties getting my 2208 90KV Gimbal motor working using the SimpleFOC library. I am using an STM32 Nucleo446RE board with the SimpleFOC Shield V2.0.4. I am using an AS5600 magnetic encoder to measure the position of the motor. As the title suggests, I can call the getAngle() method of the encoder to get the correct position measurement, however the value of motor.shaft_angle is always exactly zero. My motor fails to initialize during startup; I am shown a “MOT: Failed to notice movement” error, despite being able to see the motor rotate briefly.

I am using I2C communication to read the magnetic encoder. The encoder is connected to the SDA & SCL pins on the motor shield; the SDA/SCL lines are connected to the 3.3 V pin of the shield using 3.7k pullup resistors.

I have setup the SimpleFOCDebug to get debugging information on my PC. The output of SimpleFOCDebug can be seen below. You can see that the value for the ‘Angle’ field is non-zero and changing - I rotated the motor while this text was captured; ‘Angle’ is the value I get from reading the encoder directly. ‘The Motor Angle’ field remains zero.

TIM1-CH1 TIM1-CH2 TIM1-CH3 score: 1
STM32-DRV: best: TIM1-CH1 TIM1-CH2 TIM1-CH3 score: 1
STM32-DRV: Configuring high timer 1
STM32-DRV: Configuring high channel 1
STM32-DRV: Configuring high timer 1
STM32-DRV: Configuring high channel 2
STM32-DRV: Configuring high timer 1
STM32-DRV: Configuring high channel 3
STM32-DRV: Syncronising timers! Timer no. 1
STM32-DRV: Restarting timer 1
Successfully initialized driverMOT: Init
MOT: Enable driver.
STM32-DRV: Stopping timer 1
STM32-DRV: Stopping timer 1
STM32-DRV: Stopping timer 1
STM32-DRV: Syncronising timers! Timer no. 1
STM32-DRV: Restarting timer 1
MOT: Align sensor.
MOT: Failed to notice movement
MOT: Init FOC failed.
Angle: 68.73
Motor Angle: 0.00
Angle: 78.66
Motor Angle: 0.00
Angle: 106.26
Motor Angle: 0.00
Angle: 131.84

The code I am running is shown below.

#include <Wire.h>
#include <SimpleFOC.h>

#define AS5600_ADDR 0x36  // Default I2C address for AS5600

// Register addresses
#define AS5600_RAW_ANGLE_MSB 0x0C
#define AS5600_RAW_ANGLE_LSB 0x0D

HardwareSerial Serial2(PA3, PA2);

//Hardware used for the motor setup
MagneticSensorI2C encoder(AS5600_I2C);
BLDCMotor motor = BLDCMotor(14);
BLDCDriver3PWM driver = BLDCDriver3PWM(PA8, PA9, PA10);
LowsideCurrentSense current_sense = LowsideCurrentSense(0.01, 20, A0, A1, _NC);

void setup() {

  //Setup SimpleFOC monitoring
  SimpleFOCDebug::enable(&Serial2);

  //Setup Serial Comms
  Serial2.begin(9600);

  //Configure Motor Controller
  motor.torque_controller = TorqueControlType::voltage;//Basic type for now
  // motor.controller = MotionControlType::torque;//Torque control has not worked either
  motor.controller = MotionControlType::angle;

  //Initialize magnetic encoder
  encoder.init();

  //Link Sensor
  motor.linkSensor(&encoder);

  //Configure driver
  driver.pwm_frequency = 20000;
  driver.voltage_power_supply = 6.8;
  driver.voltage_limit = 6.8;
  // motor.voltage_sensor_align = 1; //Have not had success with values 1, 3 or by not setting this value
  const auto success = driver.init();
  if(success){
    Serial2.print("Successfully initialized driver");
  }else{
    Serial2.print("Failed to initialize driver");
  }
  
  //Link driver to current_sense
  current_sense.linkDriver(&driver);
  current_sense.gain_a *= -1;
  current_sense.gain_b *= -1;
  current_sense.gain_c *= -1;
  // skip alignment
  current_sense.skip_align = true;

  //Link Driver to motor
  motor.linkDriver(&driver);

  //Link current sense to motor
  motor.linkCurrentSense(&current_sense);

  motor.init();
  motor.enable();


  current_sense.init(); //Supposed to call after motor.init()? https://community.simplefoc.com/t/mot-init-foc-failed/3398/10
  
  motor.initFOC();

  delay(5000);
}

void loop() {
  
  //Read value from encoder and print it
  encoder.update();
  float angle = encoder.getAngle()*180.0/M_PI;
  Serial2.print("Angle: ");
  Serial2.println(angle);
  motor.loopFOC();
  float motor_angle = motor.shaft_angle *180.0/M_PI;
  Serial2.print("Motor Angle: ");
  Serial2.println(motor_angle);


  //Run motor controller
  motor.move(1.0);


  delay(500);  // Adjust delay as needed
}

Any advice would be greatly appreciated.
-Chris

2

Hi @chris2 , welcome to SimpleFOC!

Looking at your code, first thing you need to change is to remove all the delays in the main loop:

  • remove the delay(500) !!
  • remove all the prints - you can print at most a few times per second. Change the code to use a timestamp to print only 1x per second
  • remove the encoder.update() call - it is called for you in loopFOC()

The main loop needs to run as fast as possible. Your I2C communication with the sensor will already slow it down a lot. Avoid any other delays.

Before trying angle control, make sure the simpler control modes like torque - voltage work, tune your velocity PIDs, tune your current PIDs, etc. Angle control is the most complicated mode, don’t start with this…