From ac436897911f8d371b0b39e20adeda35236eca8c Mon Sep 17 00:00:00 2001 From: andreika-git Date: Tue, 23 Jan 2018 00:48:12 +0200 Subject: [PATCH] Fix stepper motor (#541) --- firmware/hw_layer/stepper.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/firmware/hw_layer/stepper.cpp b/firmware/hw_layer/stepper.cpp index 27fb318453..be60e21e55 100644 --- a/firmware/hw_layer/stepper.cpp +++ b/firmware/hw_layer/stepper.cpp @@ -30,8 +30,6 @@ static int loadStepperPos() { static msg_t stThread(StepperMotor *motor) { chRegSetThreadName("stepper"); - motor->directionPin.setValue(false); - // try to get saved stepper position (-1 for no data) motor->currentPosition = loadStepperPos(); @@ -60,6 +58,10 @@ static msg_t stThread(StepperMotor *motor) { saveStepperPos(motor->currentPosition); } + // The initial target position should correspond to the saved stepper position. + // Idle thread starts later and sets a new target position. + motor->setTargetPosition(motor->currentPosition); + while (true) { int targetPosition = motor->getTargetPosition(); int currentPosition = motor->currentPosition; @@ -142,6 +144,10 @@ void StepperMotor::initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin efiSetPadMode("stepper enable", enablePin, PAL_MODE_OUTPUT_PUSHPULL); palWritePad(this->enablePort, enablePin, true); // disable stepper + // All pins must be 0 for correct hardware startup (e.g. stepper auto-disabling circuit etc.). + palWritePad(this->stepPort, this->stepPin, false); + this->directionPin.setValue(false); + chThdCreateStatic(stThreadStack, sizeof(stThreadStack), NORMALPRIO, (tfunc_t) stThread, this); }