Skip to content
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.

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

fix: timers on STM32F411xE boards #138

Open
wants to merge 7 commits into
base: STM32
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion library.json
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
"maintainer": true
},
"homepage": "https://luni64.github.io/TeensyStep",
"version": "2.3.1",
"version": "2.3.2",
"frameworks": "arduino",
"platforms": ["Teensy", "stm32"]
}
2 changes: 1 addition & 1 deletion library.properties
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
name=TeensyStep
version=2.3.1
version=2.3.2
author=luni64
maintainer=luni64
sentence=High speed stepper driver for PJRC Teensy boards (T3.0 - T3.6) and STM32F4
Expand Down
5 changes: 4 additions & 1 deletion src/MotorControlBase.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,9 @@ namespace TeensyStep{

void emergencyStop() { timerField.end(); }

// set callback function to be called when target is reached
void setCallback(void (*_callback)()) { this->callback = _callback; }

virtual ~MotorControlBase();

void attachErrorFunction(ErrFunc ef) { errFunc = ef; }
Expand Down Expand Up @@ -95,7 +98,7 @@ namespace TeensyStep{
template <typename t>
MotorControlBase<t>::MotorControlBase(unsigned pulseWidth, unsigned accUpdatePeriod)
: timerField(this), mCnt(0)
{
{
timerField.setPulseWidth(pulseWidth);
timerField.setAccUpdatePeriod(accUpdatePeriod);
this->accUpdatePeriod = accUpdatePeriod;
Expand Down
7 changes: 4 additions & 3 deletions src/RotateControlBase.h
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 = abs(this->leadMotor->vMax);
for (int i = 1; i < N; i++)
{
this->motorList[i]->A = std::abs(this->motorList[i]->vMax);
this->motorList[i]->A = abs(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 All @@ -93,6 +93,7 @@ namespace TeensyStep
this->timerField.end();
this->leadMotor->currentSpeed = 0;
isStopping = false;
if (this->callback) this->callback();
return;
}

Expand All @@ -112,7 +113,7 @@ namespace TeensyStep
delayMicroseconds(this->pulseWidth); // dir signal need some lead time
}

this->timerField.setStepFrequency(std::abs(newSpeed)); // speed changed, update timer
this->timerField.setStepFrequency(abs(newSpeed)); // speed changed, update timer
this->leadMotor->currentSpeed = newSpeed;
}

Expand Down
10 changes: 5 additions & 5 deletions src/StepControlBase.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,8 @@ namespace TeensyStep

// Misc ---------------------------------------------------------

// set callback function to be called when target is reached
void setCallback(void (*_callback)()) { this->callback = _callback; }
// // set callback function to be called when target is reached
//void setCallback(void (*_callback)()) { this->callback = _callback; }

protected:
void accTimerISR();
Expand Down Expand Up @@ -96,16 +96,16 @@ namespace TeensyStep
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
uint32_t targetSpeed = 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;

// target speed----

float x = 0;
float leadSpeed = std::abs(this->leadMotor->vMax);
float leadSpeed = abs(this->leadMotor->vMax);
for (int i = 0; i < N; i++)
{
float relDist = this->motorList[i]->A / (float)this->leadMotor->A * leadSpeed / std::abs(this->motorList[i]->vMax);
float relDist = this->motorList[i]->A / (float)this->leadMotor->A * leadSpeed / abs(this->motorList[i]->vMax);
if (relDist > x) x = relDist;
// Serial.printf("%d %f\n", i, relDist);
}
Expand Down
6 changes: 6 additions & 0 deletions src/Stepper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,12 @@

namespace TeensyStep
{
constexpr int32_t Stepper::vMaxMax;
constexpr uint32_t Stepper::aMax;
constexpr uint32_t Stepper::vMaxDefault;
constexpr uint32_t Stepper::vPullInOutDefault;
constexpr uint32_t Stepper::aDefault;

Stepper::Stepper(const int _stepPin, const int _dirPin)
: current(0), stepPin(_stepPin), dirPin(_dirPin)
{
Expand Down
4 changes: 2 additions & 2 deletions src/Stepper.h
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,8 @@ namespace TeensyStep
// 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 abs(a->vMax) < abs(b->vMax); }
static bool cmpVmax(const Stepper* a, const Stepper* b) { return abs(a->vMax) > abs(b->vMax); }

// Pin & Dir registers
#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
Expand Down
11 changes: 10 additions & 1 deletion src/timer/stm32/TimerField.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,15 @@
#if defined(STM32F4xx)
#include "TimerField.h"
int TimerField::instances = 0;
TIM_TypeDef* TimerField::timer_mapping[MAX_TIMERS] = { TIM1, TIM2, TIM4, TIM5, TIM6, TIM7, TIM8, TIM9, TIM10, TIM11, TIM12, TIM14 };

// TIM3 and TIM13 used by HAL/FreeRTOS? doesn't work well to use.

// --- Different boards. See TimerField.h for more
#if defined(STM32F429xx)
TIM_TypeDef* TimerField::timer_mapping[MAX_TIMERS] = { TIM1, TIM2, TIM4, TIM5, TIM6, TIM7, TIM8, TIM9, TIM10, TIM11, TIM12, TIM14 };

#elif defined(STM32F411xE)
TIM_TypeDef* TimerField::timer_mapping[MAX_TIMERS] = { TIM1, TIM2, TIM4, TIM5, TIM9, TIM10, TIM11 };
#endif

#endif
9 changes: 8 additions & 1 deletion src/timer/stm32/TimerField.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,15 @@

#include "../TF_Handler.h"


// --- Different boards:
#if defined(STM32F429xx)
#define MAX_TIMERS 12
#elif defined(STM32F411xE)
#define MAX_TIMERS 7
#else
#error Board not currently supported. See: https://github.com/luni64/TeensyStep/issues/137
#endif

class TimerField
{
public:
Expand Down