Hi, I’m trying to use the step / dir communication to move my motor.
But I have noticed that there is something wrong with the encoder interrupt routine.
When I use an encoder type sensor, the step / dir communication does not work (the program does not read the pulses).
But if I use a magnetic sensor or remove the encoder interrupts from the code.
The program does read the step pulses and it works correctly.
Thank you very much for your time.
This is my code:
#include <SimpleFOC.h>
const float VBus= PA1;
const float Vcc=3.3;
const int Button =PC13;
const int LED =PA5;
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(2);
BLDCDriver3PWM driver = BLDCDriver3PWM(PA8, PA9, PA10, PC10, PC11, PC12);
//encoder instance
Encoder encoder = Encoder(PA15,PB3, 1000);
// channel A and B callbacks
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}
LowsideCurrentSense current_sense = LowsideCurrentSense(0.33, 1.53, PA0, PC1,PC0);
// inline current sensor instance
//InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
// StepDirListener( step_pin, dir_pin, counter_to_value)
StepDirListener step_dir = StepDirListener(PB5, PB4, 360/200);
void onStep() {step_dir.handle();}
// commander communication instance
Commander command = Commander(Serial);
void doMotor(char* cmd){ command.motor(&motor, cmd); }
//angle
float received_angle = 0;
float VB=0;
void setup() {
pinMode(Button, INPUT);
pinMode(LED, OUTPUT);
VB = analogRead(VBus);
VB = ((Vcc/ 1024)*VB) ;
VB=VB*19.17;
step_dir.init();
// enable interrupts
step_dir.enableInterrupt(onStep);
step_dir.attach(&motor.target);
// Serial.println(F("Step/Dir listenning."));
// initialize encoder sensor hardware
encoder.init();
encoder.enableInterrupts(doA, doB);
//link the motor to the sensor
motor.linkSensor(&encoder);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 15;
driver.init();
// link driver
motor.linkDriver(&driver);
// set control loop type to be used
motor.controller = MotionControlType::angle;
motor.torque_controller = TorqueControlType::foc_current;
motor.foc_modulation = FOCModulationType::SinePWM;
// contoller configuration based on the controll type
motor.PID_velocity.P = 0.05;
motor.PID_velocity.I = 1;
motor.PID_velocity.D = 0;
// default voltage_power_supply
motor.voltage_limit = 14; //24V
motor.current_limit = 1.2;
// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01;
// angle loop controller
motor.P_angle.P = 20;
// angle loop velocity limit
motor.velocity_limit = 30;
// use monitoring with serial for motor init
// monitoring port
Serial.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial);
motor.monitor_downsample = 0; // disable intially
motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_CURR_Q | _MON_VOLT_Q; // monitor target velocity and angle
// current sense init and linking/////////////////////////////////////////
current_sense.init();
motor.linkCurrentSense(¤t_sense);
// initialise motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// subscribe motor to the commander
command.add('M', doMotor, "motor");
_delay(1000);
}
void loop() {
// float current_magnitude = current_sense.getDCCurrent();
// iterative setting FOC phase voltage
motor.loopFOC();
// iterative function setting the outter loop target
motor.move(motor.target);
// motor monitoring
motor.monitor();
// user communication
command.run();
}
runger
November 20, 2021, 11:05am
2
Hi
I can’t at first glance see an error in your program, nor in the Encoder/StepDir classes - not sure what’s going on here.
May I ask which MCU you are using?
And is it possible you could share a schematic of your setup/driver board?
hello Thank you very much for answering.
I am using a STM32F446RE board alongside the IHM07M1 board.
NUCLEO-F446RE - STM32 Nucleo-64 development board with STM32F446RE MCU, supports Arduino and ST morpho connectivity, NUCLEO-F446RE, STMicroelectronics
-driver board.
thanks
runger
November 20, 2021, 2:57pm
4
Ok, this looks like a reasonable configuration…
One thing I think I see is that pin PA0 (used on low-side current sense) and A0 (used in inline current sense) are the same pin - PA0. Maybe you wanted A1 for the inline?
But this wouldn’t affect the interrupts…
Do you have pull-up resistors on your encoder and step-dir lines? How are you driving the step-dir? Could it be an electrical problem?
Hi, friend.
If I have pull up resistors in the encoder.
But not on the step / dir lines.
To generate the step / dir pulse I am using an arduino UNO.
Everything works fine for me separately but when using the encoder and the step / dir inputs it doesn’t.
Thanks for your interest. I hope I am not too annoying.
Sorry. I only use the low side current sense.
Fabian
November 21, 2021, 9:38pm
7
Hi, i am currently experimenting with the step/dir communication as well. Kind of the same setup. I am using an Arduino to provide Step/Dir signals to my Nucleo. Only difference is that i am using a stepper motor. But that should not be relevant.
Maybe my code helps you. It works fine for me in closed and open loop
#include <Arduino.h>
#include <SimpleFOC.h>
#include <PciManager.h>
#include <PciListener.h>
#include <config.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// MOTOR INSTANCE
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
StepDirListener step_dir = StepDirListener(INPUT_STEP, INPUT_DIR, 360.0/STEPS_PER_ROTATION/MICROSTEPS);
StepperMotor motor = StepperMotor(STEPS_PER_ROTATION/4);
StepperDriver4PWM driver = StepperDriver4PWM(OUTPUT_PHASE_1A, OUTPUT_PHASE_1B, OUTPUT_PHASE_2A, OUTPUT_PHASE_2B, OUTPUT_ENABLE_1, OUTPUT_ENABLE_2);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ENCODER INSTANCE
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Encoder encoder = Encoder(ENC_A, ENC_B, PPR_VALUE);
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// STEP FUNCTION
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void onStep() {
step_dir.handle();}
void setup() {
encoder.init();
encoder.enableInterrupts(doA, doB);
motor.linkSensor(&encoder);
step_dir.init();
step_dir.enableInterrupt(onStep);
driver.voltage_power_supply = PSU_VOLTAGE;
driver.init();
motor.linkDriver(&driver);
motor.velocity_index_search = 1;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.controller = MotionControlType::angle;
motor.PID_velocity.P = MOTOR_P_VALUE;
motor.PID_velocity.I = MOTOR_I_VALUE;
motor.PID_velocity.D = MOTOR_D_VALUE;
motor.PID_velocity.output_ramp = MOTOR_ACCELERATION;
motor.LPF_velocity.Tf = 0.01;
motor.P_angle.P = 20;
motor.velocity_limit = MOTOR_VELOCITY_LIMIT;
motor.current_limit = MOTOR_CURRENT_LIMIT;
motor.phase_resistance = MOTOR_PHASE_RES;
Serial.begin(BAUD_RATE);
motor.useMonitoring(Serial);
motor.init();
motor.initFOC();
Serial.println("MOTOR READY");
Serial.println("STEP DIR MODE");
_delay(1000);
}
void loop() {
motor.loopFOC();
motor.move(step_dir.getValue());
}
Fabian
November 21, 2021, 9:43pm
8
Wish I had some bldcs to test as well
runger
November 22, 2021, 7:09am
9
I’m guessing that line is the critical difference between the two codes! Should have spotted it before…
Hi, sry for hijacking this thread.
But i think i have the same poblem.
Im also using an optical AB encoder and a step/dir input and a bldc motor.
I wanted use the step dir signals from my 3d printer to drive it but it behaves strange.
At a low step frequency it does nothing, at a step frequency of ca 50khz it works kinda.
Here my code:
#include <SimpleFOC.h>
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(PA7,PB0,PB1,PA8);
Encoder encoder = Encoder(PA2, PA3, 150);
void doA() { encoder.handleA(); }
void doB() { encoder.handleB(); }
StepDirListener step_dir = StepDirListener(PA4, PA5, 2.0f*_PI/200.0);
void onStep() { step_dir.handle(); }
void setup() {
pinMode(PA15, OUTPUT); //Sleep Pin
pinMode(PB3, OUTPUT); //Reset Pin
digitalWrite(PA15, HIGH);
digitalWrite(PB3, HIGH);
encoder.init();
encoder.enableInterrupts(doA, doB);
motor.linkSensor(&encoder);
step_dir.init();
step_dir.enableInterrupt(onStep);
//step_dir.attach(&motor.target);
driver.voltage_power_supply = 12;
driver.init();
motor.linkDriver(&driver);
motor.voltage_sensor_align = 12;
motor.velocity_index_search = 30;
motor.controller = MotionControlType::angle;
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 4;
motor.PID_velocity.D = 0;
motor.voltage_limit = 12;
motor.PID_velocity.output_ramp = 1000;
motor.LPF_velocity.Tf = 0.01;
motor.P_angle.P = 10;
motor.velocity_limit = 100;
motor.init();
motor.initFOC();
_delay(1000);
}
void loop() {
motor.loopFOC();
motor.move(step_dir.getValue());
}
Ok, found a solution, kinda…
The simplefoc code works, but not with the step pulses my printer controller generated.
They where just a few µ-seconds long.
After i changed the marlin firmware to square wave Step pulse, it worked well.
But would it e possible to change the code to adapt to very short implulses on the step input?
Regards
David
runger
April 7, 2022, 10:43pm
12
Hi David,
The shorter impulses should still tigger an interrupt, not sure why they aren’t being detected… these things can be somewhat MCU dependent, which MCU are you using in your project?
On the other hand, if its working with the square wave pulses, why change it, would there be an advantage?
Im using a STM32G031G8, the pulses from the 3D Printer board are just 2-3us.
I thought the problem would lie in the code, so i asked just to mention the code could be improved, so other people who wanna use it dont run into the same problems.
runger
April 8, 2022, 3:46pm
14
Hey,
Are the pulses active-high or active-low?
The StepDirListener is interrupt based, so in principle it should work. I’m wondering if the problem is electrical, or timing related?
The StepDirListener puts the step pin INPUT_PULLUP - if the pulses are from high to low, the drive strength might not be enough, although the internal pull-ups are quite weak… It would be good to know the electrical configuration (circuit) for the step-dir signal in your case.
But looking at the code, there is a time dependency in there, digitalRead() is being called in the interrupt, so thinking about it some more I think the code could actually be improved to handle short pulses…
I don’t really have a test setup for this… would you be able to test it out?