Pp number very high

Hi.
My setup: 1.8 deg stepper motor 42STH60-1206A (nema 17), l298n, encoder Autonics E40H12-5000-6-L-5, 5000 ppr.
I’ve tried running example “find_pole_pairs_number”. The results are shown in the figure. After finding the poles, the motor starts to vibrate slightly and make noise. I tried to start the engine in angle_openloop mode, everything works fine. I also tested the encoder with the “encoder example”. Works fine too. What could be the problem?Снимок экрана 2021-06-11 104436

Code
   #include <SimpleFOC.h>

// Stepper motor instance
// its important to put pole pairs number as 1!!!
StepperMotor motor = StepperMotor(1);
StepperDriver4PWM driver = StepperDriver4PWM(PE9, PE11, PE13, PE14, PD0, PD1);

//  Encoder(int encA, int encB , int cpr, int index)
Encoder encoder = Encoder(PA0, PA1, 5000);
// interrupt routine intialisation
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

void setup() {
  encoder.quadrature = Quadrature::OFF;
  // initialise encoder hardware
  encoder.init();
  // hardware interrupt enable
  encoder.enableInterrupts(doA, doB);
  // link the motor to the sensor
  motor.linkSensor(&encoder);

  // power supply voltage
  // default 12V
  driver.voltage_power_supply = 12;
  driver.init();
  motor.linkDriver(&driver);


  // initialize motor
  motor.init();
  // monitoring port
  Serial.begin(115200);

  // pole pairs calculation routine
  Serial.println("Pole pairs (PP) estimator");
  Serial.println("-\n");

  float pp_search_voltage = 6; // maximum power_supply_voltage/2
  float pp_search_angle = 6*M_PI; // search electrical angle to turn
  
  // move motor to the electrical angle 0
  motor.controller = MotionControlType::angle_openloop;
  motor.voltage_limit=pp_search_voltage;
  motor.move(0);
  _delay(1000);
  // read the encoder angle 
  float angle_begin = encoder.getAngle();
  Serial.println(angle_begin);
  _delay(50);
  
  // move the motor slowly to the electrical angle pp_search_angle
  float motor_angle = 0;
  while(motor_angle <= pp_search_angle){
    motor_angle += 0.01;
    motor.move(motor_angle);
  }
  _delay(1000);
  // read the encoder value for 180
  float angle_end = encoder.getAngle();
  Serial.println(angle_end);
  _delay(50);
  // turn off the motor
  motor.move(0);
  _delay(1000);

  // calculate the pole pair number
  int pp = round((pp_search_angle)/(angle_end-angle_begin));

  Serial.print(F("Estimated PP : "));
  Serial.println(pp);
  Serial.println(F("PP = Electrical angle / Encoder angle "));
  Serial.print(pp_search_angle*180/M_PI);
  Serial.print("/");
  Serial.print((angle_end-angle_begin)*180/M_PI);
  Serial.print(" = ");
  Serial.println((pp_search_angle)/(angle_end-angle_begin));
  Serial.println();
   

  // a bit of monitoring the result
  if(pp <= 0 ){
    Serial.println(F("PP number cannot be negative"));
    Serial.println(F(" - Try changing the search_voltage value or motor/encoder configuration."));
    return;
  }else if(pp > 30){
    Serial.println(F("PP number very high, possible error."));
  }else{
    Serial.println(F("If PP is estimated well your motor should turn now!"));
    Serial.println(F(" - If it is not moving try to relaunch the program!"));
    Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!"));
  }

  
  // set FOC loop to be used
  motor.controller = MotionControlType::torque;
  // set the pole pair number to the motor
  motor.pole_pairs = pp;
  //align encoder and start FOC
  motor.initFOC();
  _delay(1000);

  Serial.println(F("\n Motor ready."));
  Serial.println(F("Set the target voltage using serial terminal:"));
}

// uq voltage
float target_voltage = 2;

void loop() {

  // main FOC algorithm function
  // the faster you run this function the better
  // Arduino UNO loop  ~1kHz
  // Bluepill loop ~10kHz 
  motor.loopFOC();

  // Motion control function
  // velocity, position or voltage (defined in motor.controller)
  // this function can be run at much lower frequency than loopFOC() function
  // You can also use motor.move() and set the motor.target in the code
  motor.move(target_voltage);
  
  // communicate with the user
  serialReceiveUserCommand();
}


// utility function enabling serial communication with the user to set the target values
// this function can be implemented in serialEvent function as well
void serialReceiveUserCommand() {
  
  // a string to hold incoming data
  static String received_chars;
  
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the string buffer:
    received_chars += inChar;
    // end of user input
    if (inChar == '\n') {
      
      // change the motor target
      target_voltage = received_chars.toFloat();
      Serial.print("Target voltage: ");
      Serial.println(target_voltage);
      
      // reset the command buffer 
      received_chars = "";
    }
  }
}

Hey @Pavel_Sannikov,

In the terminal it seems that the sensor has measured only 3 degrees change?
Does your motor move when you run this example?

Can you change the pp_search_angle to something like:

 float pp_search_angle = 20*M_PI; // search electrical angle to turn

You can also play with the voltage limit:

 float pp_search_voltage = 6; // rise or lower to see the different effects

When you say that the encoder works well, does that mean that one turn of the motor is results in exactly 6.28 radian angle?

Also, your pole pair number for this nema17 motor will be 50, so you can use it as a debugging value. Once you get 50 that means that everything works well. :smiley:

Thank you very much for your reply. Didn’t expect you to respond so quickly. I will definitely try your tips on July 14th and report the results. Now there is no access to the motor. Thanks a lot again.

It turns quite a bit to both sides with a nasty sound.