Skip to content

Stm32 adjust to slave stepper limits #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: STM32
Choose a base branch
from
Open

Stm32 adjust to slave stepper limits #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
acac5f4
Changed Steppers vMax to always be uint32 instead of int32
ramboerik Jan 28, 2022
d9a8a3e
Select stepper parameters vMax, acceleration, pullin and pullout more…
ramboerik Jan 28, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/RotateControlBase.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,10 +66,10 @@ namespace TeensyStep

this->leadMotor->currentSpeed = 0;

this->leadMotor->A = std::abs(this->leadMotor->vMax);
this->leadMotor->A = this->leadMotor->vMax;
for (int i = 1; i < N; i++)
{
this->motorList[i]->A = std::abs(this->motorList[i]->vMax);
this->motorList[i]->A = this->motorList[i]->vMax;
this->motorList[i]->B = 2 * this->motorList[i]->A - this->leadMotor->A;
}
uint32_t acceleration = (*std::min_element(this->motorList, this->motorList + N, Stepper::cmpAcc))->a; // use the lowest acceleration for the move
Expand Down
40 changes: 24 additions & 16 deletions src/StepControlBase.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ namespace TeensyStep
protected:
void accTimerISR();

uint32_t adjustToSlaveLimit(float reduceFactor, uint32_t stepperMax, uint32_t current);
void doMove(int N, float speedOverride = 1.0f);

Accelerator accelerator;
Expand All @@ -79,6 +80,12 @@ namespace TeensyStep
this->mode = MotorControlBase<t>::Mode::target;
}

template <typename a, typename t>
uint32_t StepControlBase<a, t>::adjustToSlaveLimit(float reduceFactor, uint32_t stepperMax, uint32_t current) {
uint32_t real = reduceFactor * current;
return real > stepperMax ? (stepperMax / reduceFactor) : current;
}

template <typename a, typename t>
void StepControlBase<a, t>::doMove(int N, float speedOverride)
{
Expand All @@ -92,30 +99,31 @@ namespace TeensyStep
}

// Calculate acceleration parameters --------------------------------
uint32_t pullInSpeed = this->leadMotor->vPullIn;
uint32_t pullOutSpeed = this->leadMotor->vPullOut;
uint32_t acceleration = (*std::min_element(this->motorList, this->motorList + N, Stepper::cmpAcc))->a; // use the lowest acceleration for the move

uint32_t targetSpeed = std::abs((*std::min_element(this->motorList, this->motorList + N, Stepper::cmpVmin))->vMax) * speedOverride; // use the lowest max frequency for the move, scale by relSpeed
if (this->leadMotor->A == 0 || targetSpeed == 0) return;
uint32_t targetPullInSpeed = this->leadMotor->vPullIn;
uint32_t targetPullOutSpeed = this->leadMotor->vPullOut;
uint32_t targetAcceleration = this->leadMotor->a;
uint32_t targetSpeed = this->leadMotor->vMax * speedOverride;

// target speed----
if (this->leadMotor->A == 0 || targetSpeed == 0) return;

float x = 0;
float leadSpeed = std::abs(this->leadMotor->vMax);
for (int i = 0; i < N; i++)
//Serial.printf("targetPullInSpeed: %u, targetPullOutSpeed: %u, targetAcceleration: %u, targetSpeed: %u\n", targetPullInSpeed, targetPullOutSpeed, targetAcceleration, targetSpeed);
for (int i = 1; i < N; i++)
{
float relDist = this->motorList[i]->A / (float)this->leadMotor->A * leadSpeed / std::abs(this->motorList[i]->vMax);
if (relDist > x) x = relDist;
// Serial.printf("%d %f\n", i, relDist);
if(this->motorList[i]->A == 0) continue;

float reduceFactor = this->motorList[i]->A / (float)this->leadMotor->A;
targetPullInSpeed = adjustToSlaveLimit(reduceFactor, this->motorList[i]->vPullIn, targetPullInSpeed);
targetPullOutSpeed = adjustToSlaveLimit(reduceFactor, this->motorList[i]->vPullOut, targetPullOutSpeed);
targetAcceleration = adjustToSlaveLimit(reduceFactor, this->motorList[i]->a, targetAcceleration);
targetSpeed = adjustToSlaveLimit(reduceFactor, this->motorList[i]->vMax, targetSpeed);
}
targetSpeed = leadSpeed / x;
//Serial.printf("\n%d\n",targetSpeed);
//Serial.printf("targetPullInSpeed: %u, targetPullOutSpeed: %u, targetAcceleration: %u, targetSpeed: %u\n", targetPullInSpeed, targetPullOutSpeed, targetAcceleration, targetSpeed);

// Start move--------------------------
this->timerField.begin();

this->timerField.setStepFrequency(accelerator.prepareMovement(this->leadMotor->current, this->leadMotor->target, targetSpeed, pullInSpeed, pullOutSpeed, acceleration));
this->timerField.setStepFrequency(accelerator.prepareMovement(this->leadMotor->current, this->leadMotor->target, targetSpeed, targetPullInSpeed, targetPullOutSpeed, targetAcceleration));
this->timerField.stepTimerStart();
this->timerField.accTimerStart();
}
Expand Down Expand Up @@ -194,4 +202,4 @@ namespace TeensyStep
delay(1);
}
}
}
}
10 changes: 5 additions & 5 deletions src/Stepper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,19 +75,19 @@ namespace TeensyStep
Stepper& Stepper::setMaxSpeed(int32_t speed)
{
setDir(speed >= 0 ? 1 : -1);
vMax = std::min(vMaxMax, std::max(-vMaxMax, speed));
vMax = std::min(vMaxMax, (uint32_t)std::abs(speed));
return *this;
}

Stepper& Stepper::setPullInSpeed(int32_t speed)
Stepper& Stepper::setPullInSpeed(uint32_t speed)
{
return setPullInOutSpeed(speed, speed);
}

Stepper& Stepper::setPullInOutSpeed(int32_t pullInSpeed, int32_t pullOutSpeed)
Stepper& Stepper::setPullInOutSpeed(uint32_t pullInSpeed, uint32_t pullOutSpeed)
{
vPullIn = std::abs(pullInSpeed);
vPullOut = std::abs(pullOutSpeed);
vPullIn = pullInSpeed;
vPullOut = pullOutSpeed;
return *this;
}

Expand Down
14 changes: 7 additions & 7 deletions src/Stepper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ namespace TeensyStep

class Stepper
{
static constexpr int32_t vMaxMax = 300000; // largest speed possible (steps/s)
static constexpr uint32_t vMaxMax = 300000; // largest speed possible (steps/s)
static constexpr uint32_t aMax = 500000; // speed up to 500kHz within 1 s (steps/s^2)
static constexpr uint32_t vMaxDefault = 800; // should work with every motor (1 rev/sec in 1/4-step mode)
static constexpr uint32_t vPullInOutDefault = 100;
Expand All @@ -18,8 +18,8 @@ namespace TeensyStep
Stepper(const int StepPin, const int DirPin);

Stepper& setMaxSpeed(int32_t speed); // steps/s
Stepper& setPullInSpeed(int32_t speed); // steps/s
Stepper& setPullInOutSpeed(int32_t pullInpeed, int32_t pullOutSpeed); // steps/s
Stepper& setPullInSpeed(uint32_t speed); // steps/s
Stepper& setPullInOutSpeed(uint32_t pullInpeed, uint32_t pullOutSpeed); // steps/s
Stepper& setAcceleration(uint32_t _a); // steps/s^2

Stepper& setStepPinPolarity(int p); // HIGH -> positive pulses, LOW -> negative pulses
Expand All @@ -44,15 +44,15 @@ namespace TeensyStep
volatile int32_t target;

int32_t A, B; // Bresenham paramters
int32_t vMax;
int32_t vPullIn, vPullOut;
uint32_t vMax;
uint32_t vPullIn, vPullOut;
uint32_t a;

// compare functions
static bool cmpDelta(const Stepper* a, const Stepper* b) { return a->A > b->A; }
static bool cmpAcc(const Stepper* a, const Stepper* b) { return a->a < b->a; }
static bool cmpVmin(const Stepper* a, const Stepper* b) { return std::abs(a->vMax) < std::abs(b->vMax); }
static bool cmpVmax(const Stepper* a, const Stepper* b) { return std::abs(a->vMax) > std::abs(b->vMax); }
static bool cmpVmin(const Stepper* a, const Stepper* b) { return a->vMax < b->vMax; }
static bool cmpVmax(const Stepper* a, const Stepper* b) { return a->vMax > b->vMax; }

// Pin & Dir registers
#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
Expand Down