Power cycles when doing initFOC

hardware support
1

Hey everyone, I’m still very new to working with SimpleFOC and I’m currently using the MKS XDrive Mini, which runs on the STM32F405RGT6. I’m using Arduino IDE with the Generic F405RGTx variant selected. I’m running the board with a 20V laptop power supply and setting the driver limit to 12v. Unfortunately every time it gets to initFOC and trying to align the sensor it power cycles. I recently linked a LowsideCurrentSense in hopes of that being the issue but to no avail. Here’s the full code.

#include <SimpleFOC.h>

// #define RS485_RO PA3
#define RS485_RE PA0
#define RS485_DE PA1
// #define RS485_DI PA2

#define RE_PINNAME digitalPinToPinName(RS485_RE)
#define DE_PINNAME digitalPinToPinName(RS485_DE)

#define AR49_READ     0x02
#define AR49_ZERO_RST 0xC2
#define AR49_COUNTING_ERROR 0x10
#define AR49_PARITY_ERROR   0x40
#define AR49_END_BIT_ERROR  0x80

#define RESOLUTION 24
#define RADIAN_PER_TICK TWO_PI / pow(2, RESOLUTION)

#define PWM_AH PA8
#define PWM_AL PB13

#define PWM_BH PA9
#define PWM_BL PB14

#define PWM_CH PA10
#define PWM_CL PB15

#define MO_EN  PB12

#define SENSE_SO1 PC0
#define SENSE_SO2 PC1

void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.LSIState = RCC_LSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 7;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
                                | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
    Error_Handler();
  }

    /**Configure the Systick interrupt time 
  */
  HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);

    /**Configure the Systick 
    */
  HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  /* SysTick_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(SysTick_IRQn, 15, 0);
}

void PrintBits(char c)
{
  for(int i = 7; i >= 0; i--)
  {
    Serial.print(bitRead(c,i));
  }
}

HardwareSerial Serial2(USART2);

char buffer[10];

uint32_t encoderPosition;

uint delayBeforeWrite = 2;
uint delayBeforeLow = 4;
uint delayBeforeRead = 30;
char messageToSend = AR49_READ;

bool shouldLog = false;

float OnReadEncoder()
{
  digitalWriteFast(RE_PINNAME, HIGH);
  digitalWriteFast(DE_PINNAME, HIGH);
  delayMicroseconds(delayBeforeWrite);

  Serial2.write(messageToSend);
  Serial2.flush();

  // delayMicroseconds(delayBeforeLow);

  digitalWriteFast(RE_PINNAME, LOW);
  digitalWriteFast(DE_PINNAME, LOW);

  delayMicroseconds(delayBeforeRead);

  if (shouldLog) Serial.print("Response from encoder:");

  int bytesReceived = Serial2.available();
  if (bytesReceived = 6)
  {
    Serial2.readBytes(buffer, bytesReceived);

    if (shouldLog)
    {
      bool readConfirmed = false;
      for (int i = 0; i < bytesReceived; i++)
      {
        Serial.print(' ');
        PrintBits(buffer[i]);

        if (buffer[i] == AR49_READ)
        {
          readConfirmed = true;
        }
      }

      if (readConfirmed)
      {
        Serial.print(" (Read confirmed!) ");
      }
    }

    if ((buffer[1] & AR49_COUNTING_ERROR) > 0)
    {
      if (shouldLog) Serial.print(" !!Counting error!!");
    }
    else
    {
      encoderPosition = buffer[2];
      encoderPosition |= buffer[3] << 8;
      encoderPosition |= buffer[4] << 16;

      // Serial.println(calcCRC8((uint8_t*)encoderPosition, 3), HEX);
      // TODO Need to check CRC for validity

      if (shouldLog)
      {
        Serial.print(" encoder position is at: ");
        Serial.print(encoderPosition, DEC);

        Serial.print(" Encoder angle: ");
        Serial.print(encoderPosition * RADIAN_PER_TICK, 10);
      }
    }
  }
  else
  {
    if (shouldLog) Serial.println(" Bad read!");
  }

  if (shouldLog)
  {
    Serial.println("");

    if (bytesReceived > 0)
    {
      Serial.print("                     ");

      for (int i = 0; i < bytesReceived; i++)
      {
        Serial.print("       ");
        Serial.print(buffer[i], HEX);
      }

      Serial.println("");
    }
  }

  while (Serial2.available())
  {
    if (shouldLog)
    {
      Serial.print("Extra data was present: ");
      PrintBits(Serial2.read());
      Serial.println("");
    }
    else Serial2.read();
  }

  return encoderPosition * RADIAN_PER_TICK;
}

void OnInitEncoder()
{

}

GenericSensor encoder = GenericSensor(OnReadEncoder, OnInitEncoder);
BLDCMotor motor = BLDCMotor(14, NOT_SET, 105);
BLDCDriver6PWM driver = BLDCDriver6PWM(PWM_AH, PWM_AL, PWM_BH, PWM_BL, PWM_CH, PWM_CL, MO_EN);
LowsideCurrentSense current_sense = LowsideCurrentSense(0.005, 50, SENSE_SO1, SENSE_SO2, NOT_SET);

// instantiate the commander
Commander command = Commander(Serial);
void doMotor(char* cmd) { command.motor(&motor, cmd); }

void setup()
{
  pinMode(RS485_RE, OUTPUT);
  pinMode(RS485_DE, OUTPUT);

  Serial2.begin(2500000, SERIAL_8N1);

  Serial.begin(115200);

  delay(3000);

  SimpleFOCDebug::enable();

  encoder.init();

  motor.linkSensor(&encoder);

  driver.voltage_power_supply = 20;
  driver.voltage_limit = 12;

  if (!driver.init())
  {
    Serial.println("Driver init failed!");
    return;
  }

  current_sense.linkDriver(&driver);

  if (!current_sense.init())
  {
    Serial.println("Current sense init failed");
    return;
  }

  motor.linkDriver(&driver);
  motor.linkCurrentSense(&current_sense);

  motor.voltage_sensor_align = 5;

  motor.controller = MotionControlType::angle;

  motor.useMonitoring(Serial);

  // initialize motor
  if (!motor.init())
  {
    Serial.println("Motor init failed!");
    return;
  }
  // align sensor and start FOC
  if (!motor.initFOC())
  {
    Serial.println("FOC init failed!");
    return;
  }

  // set the initial motor target
  motor.target = 2; // Volts 

  // add target command M
  command.add('M', doMotor, "Motor");

  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target using serial terminal and command M:"));

  shouldLog = false;

  delay(1000);
}

void loop()
{
  // main FOC algorithm function
  motor.loopFOC();

  // Motion control function
  motor.move();

  // user communication
  command.run();
}

Any help is greatly appreciated. I’m all out of ideas.

2

Hey @Metastable,

Could it be that your power-supply is to blame?
What motor do you have?
Try reducing the motor.voltage_sensor_align and motor.volatge_limit to 1V or even less if you motor is a low-resistance one.

I’am also a bit curious about the line BLDCMotor motor = BLDCMotor(14, NOT_SET, 105);
I have never tried to supply the KV rating and not the phase resistance. This might cause some issues. I think that in the code we consider that if the KV is provided that the phase resistance is too. I’d have to look into it to be sure.

I’d suggest you to try first without the both
BLDCMotor motor = BLDCMotor(14); and then if it works go further.

Good luck!

3

Thanks for the suggestions! Yes it could certainly be the power-supply. I assumed setting the voltage limits to 12 and 5 would be low enough but that appears to have been incorrect. I set the limits to 1V as you suggested and its no longer power cycling!

This is the motor I’m using https://hobbyking.com/en_us/turnigy-ld9017-brushless-gimbal-motor.html?srsltid=AfmBOoqdW4r_Q6gaAU84rmr24oSYF0cvbRlnBDzOO4Kk_JgFsuq2plK1 I also set the phase resistance to 20 as outlined in the specs (I haven’t verified this myself, which I know I should). I did this after testing with the 1V limits.

I’ll have to test the limits of the power supply (even if this is temporary).

Thanks again for the help!

1 Like