How to avoid motor go to zero position on reset?

Hi, I successfully made the angle_control example working with my motor and simplefoc shield. But my motor always goes back to zero position when I power the system up. In my scenario, I want to get rid of this behavior and I hope the motor stays at where it was before reset. What should I do?

Hi, and welcome, David!

This is something you will have to solve in your programming.
For example, you could read the current position, and set this as the target position before activating the motor.
In this way the system would hold the current position, whatever it is, when powered on.

Hey David,
I agree with Richard. There is also a feature fairly new to the v2.1 release to add the sensor offset. So basically you have two options. After motor.initFOC() you can add either:

motor.sensor_offset = motor.shaft_angle;

Or as Richard said:

motor.target = motor.shaft_angle;

the difference is that the first method your initial position will be declared as zero. And for the second code your target will be set to the initial position, but your zero position will still be sensors absolute zero.

Hi, I added motor.target = motor.shaft_angle; after motor.initFOC() but, disappointedly, nothing changed. The motor still goes back to absolute zero of the sensor on reset, and after that response of command ‘T’ in serial monitor is 0.000.

I am running Arduino IDE 1.8.13 and SimpleFoc 2.1.0, and here is my current code:

/**
 * 
 * Position/angle motion control example
 * Steps:
 * 1) Configure the motor and magnetic sensor  
 * 2) Run the code
 * 3) Set the target angle (in radians) from serial terminal
 * 
 */
#include <SimpleFOC.h>

// magnetic sensor instance - SPI
MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
// magnetic sensor instance - MagneticSensorI2C
//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
// magnetic sensor instance - analog output
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
// Stepper motor & driver instance
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);

// angle set point variable
float target_angle = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }

void setup() {

  // initialise magnetic sensor hardware
  sensor.init();
  // link the motor to the sensor
  motor.linkSensor(&sensor);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);
  
  // choose FOC modulation (optional)
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;

  // set motion control loop to be used
  motor.controller = MotionControlType::angle;

  // contoller configuration 
  // default parameters in defaults.h

  // velocity PI controller parameters
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 20;
  motor.PID_velocity.D = 0;
  // maximal voltage to be set to the motor
  motor.voltage_limit = 6;
  
  // velocity low pass filtering time constant
  // the lower the less filtered
  motor.LPF_velocity.Tf = 0.01;

  // angle P controller 
  motor.P_angle.P = 20;
  // maximal velocity of the position control
  motor.velocity_limit = 20;

  // use monitoring with serial 
  Serial.begin(115200);
  // comment out if not needed
  motor.useMonitoring(Serial);

  
  // initialize motor
  motor.init();
  // align sensor and start FOC
  motor.initFOC(0.13, Direction::CCW);
  motor.target = motor.shaft_angle;
  // add target command T
  command.add('T', doTarget, "target angle");

  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target angle using serial terminal:"));
  _delay(1000);
}


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_angle);


  // function intended to be used with serial plotter to monitor motor variables
  // significantly slowing the execution down!!!!
  // motor.monitor();
  
  // user communication
  command.run();
}

You are setting your target angle using target_angle variable. Not the motor.target. So use then:

target_angle = motor.shaft_angle;

I hate to say but after doing this my motor still goes back to sensor’s absolute zero. I have also tried to move target_angle = motor.shaft_angle; after command.add('T', doTarget, "target angle"); but it still not working.

I tried to print target_angle in Serial Monitor by adding Serial.println(target_angle); to very beginning of void loop(). The output is:

MOT: Monitor enabled!
MOT: Init
MOT: Enable driver.
MOT: Align sensor.
MOT: Skip dir calib.
MOT: Skip offset calib.
MOT: No current sense.
MOT: Ready.
Motor ready.
Set the target angle using serial terminal:
0.00
0.00
0.00
0.00
0.00

That’s confusing…

Haha :smiley:
So you removed the motor.target and used the target_angle and it still goes to 0. I have no idea why, this should work. It is possible that I’ve forgotten to update the motor.shaft_angle in the motor.initFOC() :slight_smile:

Does it go the the old location of it just jumps a bit but stays on the same spot?

What you can try is:
target_angle=motor.shaftAngle();

I hope this one will work :slight_smile:

Have you tried the motor.sensor_offset

Yeah, target_angle=motor.shaftAngle(); works :smiley:. But strangely, after powering up, the motor always turn a bit clockwise and then return to the init angle. Is it possible to get rid of this behavior?

I have not tried motor.sensor_offset because I do not want change sensor’s zero position in my current project. Maybe I will find it useful in later projects.

And lastly, I am sorry to say I do not really understand your question:

Does it go the the old location of it just jumps a bit but stays on the same spot?

Does it happen during or after the init?

If it’s after it might be due to the velocity calculatio or due to the pid initial jump on start.
What you could do maybe is just call the motor.move(motor.shaftAngle()) (several times, like 10-15) in the setup after motor.initFOC() to denounce the initial conditions. I’m not 100% sure this will work though.
If this does not work you could set the pid controller parameters very low on the beginning and the raise them slowly for a second or so. :slight_smile:

Hi, I had this issue too - thanks for the tips. Repeatedly calling motor.move does not correct it, but starting with 0 PIDs does work.

I set my PIDs to 0, then after 10 milliseconds of running the main loop, I set them to their desired values. No need to slowly raise them.

Avesome @Jason_Kirsons ! I’ll look into the PID to see if I can remove this behavior. :smiley:

Hi Community,
Is that topic was solved? Because I was confronted to the same problem.
First thanks for this wonderful FOC library. My goal is to build a small robot with brushless engines. So I tested in FOC angle control loop with 13.5 ohm Gimbal brushless; L6234 Breakout Board and AS5600 in I2C with Arduino nano and found it works very well except for the switching on. For most applications it is not an issue but making a robot that starts with uncontrolled movement when switch on is not the best. I tested many things that does not correct the problem except the one suggested by @Jason_Kirsons and modified the setup this way: Set PIDs at 0 and replaced the 1 second delay at the end by a 500 iteration loop running motor.loopFOC() and motor.move (target_angle); followed by the expected PIDs and the setting of target_angle to the motor.shaft_angle for don’t move once in the main loop. It works well but I suppose that’s not the most elegant way

Hey @CREGIS21,
Thanks for the heads-up, I’ve created a github issue for this problem and I hope to remove it for the next releasse :smiley:

BTW, this might have something to do with the velocity not been calculated well for the first time.
https://community.simplefoc.com/t/weird-jump-after-initialisation/1114/5

1 Like