eddy/stm32samples
1
0
Fork 0
mirror of https://github.com/eddyem/stm32samples.git synced 2025-12-06 10:45:11 +03:00
Code Issues Packages Projects Releases Wiki Activity

First approximation to steppers run

Browse Source
This commit is contained in:
Edward Emelianov 2021-11-16 00:28:20 +03:00
parent bac9912056
commit 4b50e95838
12 changed files with 257 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
F0-nolib/3steppersLB/commonproto.c
View File

@ -171,6 +171,7 @@ static errcodes ustepsparser(uint8_t par, int32_t *val){
if(m < 1 || m > MICROSTEPSMAX) return ERR_BADVAL;
// find most significant bit
if(m != 1<<MSB(m)) return ERR_BADVAL;
if(the_conf.maxspd[n] * m > PCLK/(MOTORTIM_PSC+1)/(MOTORTIM_ARRMIN+1)) return ERR_BADVAL;
the_conf.microsteps[n] = m;
}
*val = the_conf.microsteps[n];
@ -210,21 +211,42 @@ static errcodes accparser(uint8_t par, int32_t *val){
return ERR_OK;
}
// calculate ARR value for given speed, return nearest possible speed
static uint16_t getSPD(uint8_t n, int32_t speed){
uint32_t ARR = PCLK/(MOTORTIM_PSC+1) / the_conf.microsteps[n] / speed - 1;
if(ARR < MOTORTIM_ARRMIN) ARR = MOTORTIM_ARRMIN;
else if(ARR > 0xffff) ARR = 0xffff;
speed = PCLK/(MOTORTIM_PSC+1) / the_conf.microsteps[n] / (ARR + 1);
if(speed > 0xffff) speed = 0xffff;
return (uint16_t)speed;
}
static errcodes maxspdparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){
if(*val/the_conf.microsteps[n] > MAXMAXSPD || *val < 1) return ERR_BADVAL;
the_conf.maxspd[n] = *val;
if(*val <= the_conf.minspd[n]) return ERR_BADVAL;
the_conf.maxspd[n] = getSPD(n, *val);
}
*val = the_conf.maxspd[n];
return ERR_OK;
}
static errcodes minspdparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){
if(*val >= the_conf.maxspd[n]) return ERR_BADVAL;
the_conf.minspd[n] = getSPD(n, *val);
}
*val = the_conf.minspd[n];
return ERR_OK;
}
static errcodes spdlimparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par);
if(n > MOTORSNO-1) return ERR_BADPAR;
*val = MOTORFREQ / the_conf.microsteps[n];
*val = getSPD(n, 0xffff);
return ERR_OK;
}
@ -251,6 +273,16 @@ static errcodes encrevparser(uint8_t par, int32_t *val){
return ERR_OK;
}
static errcodes motflagsparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){
the_conf.motflags[n] = *((motflags_t*)val);
}
*(motflags_t*)val = the_conf.motflags[n];
return ERR_OK;
}
static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){
if(store_userconf()) return ERR_CANTRUN;
return ERR_OK;
@ -284,6 +316,24 @@ static errcodes relstepsparser(uint8_t par, int32_t *val){
if(ISSETTER(par)) return motor_relmove(n, *val);
return getremainsteps(n, val);
}
static errcodes motstateparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par);
if(n > MOTORSNO-1) return ERR_BADPAR;
*val = getmotstate(n);
return ERR_OK;
}
static errcodes encposparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par);
if(n > MOTORSNO-1) return ERR_BADPAR;
errcodes ret = ERR_OK;
if(ISSETTER(par)){
if(!setencpos(n, *val)) ret = ERR_CANTRUN;
}
*val = encoder_position(n);
return ret;
}
/******************* END of motors' parsers *******************/
/*
@ -314,13 +364,17 @@ const commands cmdlist[CMD_AMOUNT] = {
[CMD_MICROSTEPS] = {"microsteps", ustepsparser, "set/get microsteps settings"},
[CMD_ACCEL] = {"accel", accparser, "set/get accel/decel (steps/s^2)"},
[CMD_MAXSPEED] = {"maxspeed", maxspdparser, "set/get max speed (steps per sec)"},
[CMD_MINSPEED] = {"minspeed", minspdparser, "set/get min speed (steps per sec)"},
[CMD_SPEEDLIMIT] = {"speedlimit", spdlimparser, "get limiting speed for current microsteps"},
[CMD_MAXSTEPS] = {"maxsteps", maxstepsparser, "set/get max steps (from zero)"},
[CMD_ENCREV] = {"encrev", encrevparser, "set/get max encoder's pulses per revolution"},
[CMD_MOTFLAGS] = {"motflags", motflagsparser, "set/get motorN flags"},
// motor's commands
[CMD_ABSPOS] = {"abspos", curposparser, "set/get position (in steps)"},
[CMD_RELPOS] = {"relpos", relstepsparser, "set relative steps, get remaining"},
[CMD_STOPMOTOR] = {"stop", mstopparser, "stop motor now"},
[CMD_REINITMOTORS] = {"motreinit", reinitmparser, "re-init motors after configuration changed"},
[CMD_MOTORSTATE] = {"state", motstateparser, "get motor state"},
[CMD_ENCPOS] = {"encpos", encposparser, "set/get encoder's position"},
};

4
F0-nolib/3steppersLB/commonproto.h
View File

@ -76,13 +76,17 @@ enum{
,CMD_MICROSTEPS // get/set microsteps
,CMD_ACCEL // set/get acceleration/deceleration
,CMD_MAXSPEED // set/get maximal speed
,CMD_MINSPEED // set/get minimal speed
,CMD_SPEEDLIMIT // get limit of speed for current microsteps settings
,CMD_MAXSTEPS // max steps (-max..+max)
,CMD_ENCREV // encoder's pulses per revolution
,CMD_MOTFLAGS // motor flags
,CMD_REINITMOTORS // re-init motors after configuration changing
,CMD_ABSPOS // current position (set/get)
,CMD_RELPOS // set relative steps or get steps left
,CMD_STOPMOTOR // stop moving NOW
,CMD_MOTORSTATE // motor state
,CMD_ENCPOS // position of encoder (independing on settings)
//,CMD_STOPDECEL
//,CMD_RELMOVE
//,CMD_ABSMOVE

15
F0-nolib/3steppersLB/flash.c
View File

@ -34,19 +34,20 @@ static const uint32_t blocksize = (uint32_t)&_BLOCKSIZE;
// max amount of Config records stored (will be recalculate in flashstorage_init()
static uint32_t maxCnum = 1024 / sizeof(user_conf); // can't use blocksize here
#define DEFMF {.haveencoder = 1}
#define DEFMF {.haveencoder = 1, .donthold = 1, .eswinv = 1}
#define USERCONF_INITIALIZER { \
.userconf_sz = sizeof(user_conf) \
,.CANspeed = 100 \
,.CANID = 0xaa \
,.microsteps = {32, 32, 32} \
,.accel = {160, 160, 160} \
,.maxspd = {1000, 1000, 1000} \
,.maxsteps = {50000, 50000, 50000} \
,.accel = {500, 500, 500} \
,.maxspd = {3000, 3000, 3000} \
,.minspd = {20, 20, 20} \
,.maxsteps = {500000, 500000, 500000} \
,.encrev = {4000,4000,4000} \
,.encperstepmin = {16,16,16} \
,.encperstepmax = {24,24,24} \
,.encperstepmin = {5,5,5} \
,.encperstepmax = {50,50,50} \
,.motflags = {DEFMF,DEFMF,DEFMF} \
,.ESW_reaction = {ESW_ANYSTOP, ESW_ANYSTOP, ESW_ANYSTOP} \
}
@ -224,6 +225,8 @@ void dump_userconf(_U_ char *txt){
printu(the_conf.accel[i]);
PROPNAME("maxspeed");
printu(the_conf.maxspd[i]);
PROPNAME("minspeed");
printu(the_conf.minspd[i]);
PROPNAME("maxsteps");
printu(the_conf.maxsteps[i]);
PROPNAME("encperrev");

12
F0-nolib/3steppersLB/flash.h
View File

@ -34,8 +34,6 @@
#define MICROSTEPSMAX (512)
// (STEPS per second^2)
#define ACCELMAXSTEPS (1000)
// max speed IN STEPS!
#define MAXMAXSPD (10000)
// max encoder steps per rev
#define MAXENCREV (100000)
@ -48,10 +46,11 @@ blocksizeASH_SIZE_REG ((uint32_t)0x1FFFF7CC)
// motor flags
typedef struct{
uint8_t reverse : 1; // reversing motor rotation
uint8_t encreverse : 1; // reversing encoder rotation TODO: configure encoder's timer to downcounting
uint8_t haveencoder : 1; // have encoder
uint8_t donthold : 1; // clear power @ stop (don't hold motor when stopped)
uint8_t reverse : 1; // bit0 - reversing motor rotation
uint8_t encreverse : 1; // bit1 - reversing encoder rotation TODO: configure encoder's timer to downcounting
uint8_t haveencoder : 1; // bit2 - have encoder
uint8_t donthold : 1; // bit3 - clear power @ stop (don't hold motor when stopped)
uint8_t eswinv : 1; // bit4 - invers end-switches
} motflags_t;
/*
@ -64,6 +63,7 @@ typedef struct __attribute__((packed, aligned(4))){
uint16_t microsteps[MOTORSNO]; // microsteps amount per step
uint16_t accel[MOTORSNO]; // acceleration/deceleration (steps/s^2)
uint16_t maxspd[MOTORSNO]; // max motor speed (steps per second)
uint16_t minspd[MOTORSNO]; // min motor speed (steps per second)
uint32_t maxsteps[MOTORSNO]; // maximal amount of steps
uint16_t encrev[MOTORSNO]; // encoders' counts per revolution
uint16_t encperstepmin[MOTORSNO]; // min amount of encoder ticks per one step

54
F0-nolib/3steppersLB/hardware.c
View File

@ -86,16 +86,21 @@ static IRQn_Type motirqs[MOTORSNO] = {TIM15_IRQn, TIM14_IRQn, TIM16_IRQn};
// motor's PWM
static void setup_mpwm(int i){
volatile TIM_TypeDef *TIM = mottimers[i];
TIM->CR1 = 0;
TIM->PSC = MOTORTIM_PSC - 1; // 64kHz
// PWM mode 1 (OCxM = 110), preload enable
TIM->CCMR1 = TIM_CCMR1_OC1M_2; // Force inactive
TIM->CCER = TIM_CCER_CC1E; // turn it on, active high
TIM->CCR1 = 1; // 20.8us for pulse duration, according to datasheet 1.9us is enough
TIM->BDTR |= TIM_BDTR_MOE; // enable main output
TIM->EGR |= TIM_EGR_UG; // force update generation
TIM->DIER = TIM_DIER_CC1IE; // allow CC interrupt (we should count steps)
TIM->CR1 = TIM_CR1_ARPE; // buffered ARR
TIM->PSC = MOTORTIM_PSC; // 64kHz
// PWM mode 1 (active -> inactive)
TIM->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1;
#if MOTORTIM_ARRMIN < 5
#error "change the code!"
#endif
TIM->CCR1 = MOTORTIM_ARRMIN - 3; // ~10us for pulse duration
TIM->CNT = 0;
TIM->ARR = 0xffff;
// TIM->EGR = TIM_EGR_UG; // generate update to refresh ARR
TIM->BDTR |= TIM_BDTR_MOE; // enable main output
TIM->CCER = TIM_CCER_CC1E; // turn it on, active high
TIM->DIER = TIM_DIER_CC1IE; // allow CC interrupt (we should count steps)
//TIM->CR1 = TIM_CR1_CEN;
NVIC_EnableIRQ(motirqs[i]);
}
@ -128,17 +133,20 @@ void timers_setup(){
RCC->APB1ENR |= RCC_APB1ENR_TIM2EN | RCC_APB1ENR_TIM3EN | RCC_APB1ENR_TIM14EN;
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN | RCC_APB2ENR_TIM15EN | RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN; // enable clocking
// setup PWM @ TIM17 - single PWM channel
// PWM mode 1 (active -> inactive)
TIM17->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1;
TIM17->PSC = 5; // 8MHz for 31kHz PWM
TIM17->ARR = 254; // ARR for 8-bit PWM
TIM17->CCR1 = 0; // level=0
TIM17->BDTR |= TIM_BDTR_MOE; // enable main output
TIM17->CCER = TIM_CCER_CC1E; // enable PWM output
TIM17->CR1 |= TIM_CR1_CEN; // enable timer
TIM17->CR1 = TIM_CR1_CEN; // enable timer
for(int i = 0; i < MOTORSNO; ++i)
setup_mpwm(i);
for(int i = 0; i < MOTORSNO; ++i){
if(the_conf.motflags[i].haveencoder){ // motor have the encoder
//if(the_conf.motflags[i].haveencoder){ // motor have the encoder
setup_enc(i);
}
//}
}
}
@ -189,23 +197,27 @@ uint8_t MSB(uint16_t val){
return ((uint8_t)r + bval[val]);
}
// state 1 - pressed, 0 - released (pin active is zero)
uint8_t ESW_state(uint8_t x){
uint8_t val = ((ESWports[x]->IDR & ESWpins[x]) ? 0 : 1);
if(the_conf.motflags[x].eswinv) val = !val;
return val;
}
void tim14_isr(){
//TIM14->CR1 &= ~TIM_CR1_CEN;
addmicrostep(1);
TIM14->SR = 0;
TIM14->CR1 &= TIM_CR1_CEN;
DBG("UP1");
//addmicrostep(1);
}
void tim15_isr(){
//TIM15->CR1 &= ~TIM_CR1_CEN;
addmicrostep(0);
TIM15->SR = 0;
TIM15->CR1 &= TIM_CR1_CEN;
DBG("UP0");
//addmicrostep(0);
}
void tim16_isr(){
//TIM16->CR1 &= ~TIM_CR1_CEN;
addmicrostep(2);
TIM16->SR = 0;
TIM16->CR1 &= TIM_CR1_CEN;
DBG("UP2");
//addmicrostep(2);
}

17
F0-nolib/3steppersLB/hardware.h
View File

@ -26,12 +26,10 @@
#ifndef PCLK
#define PCLK (48000000)
#endif
// motor frequency in mictosteps - 96kHz
#ifndef MOTORFREQ
#define MOTORFREQ (96000)
#endif
// motors' timer PSC
#define MOTORTIM_PSC (PCLK/MOTORFREQ)
// motors' timer PSC = PCLK/Tfreq - 1, Tfreq=16MHz
#define MOTORTIM_PSC (2)
// minimal ARR value - 99 for 5000 steps per second @ 32 microsteps/step
#define MOTORTIM_ARRMIN (99)
// default CAN bus speed in kbaud
#define DEFAULT_CAN_SPEED (250)
@ -168,8 +166,6 @@ extern const uint32_t BTNpins[BTNSNO];
// ESW ports & pins
extern volatile GPIO_TypeDef *ESWports[ESWNO];
extern const uint32_t ESWpins[ESWNO];
// state 1 - pressed, 0 - released (pin active is zero)
#define ESW_state(x) ((ESWports[x]->IDR & ESWpins[x]) ? 0 : 1)
// motors
#define MOTORSNO (3)
@ -181,12 +177,12 @@ extern volatile GPIO_TypeDef *DIRports[MOTORSNO];
extern const uint32_t DIRpins[MOTORSNO];
#define MOTOR_CW(x) do{ pin_set(DIRports[x], DIRpins[x]); }while(0)
#define MOTOR_CCW(x) do{ pin_clear(DIRports[x], DIRpins[x]); }while(0)
// minimal motor speed - steps per second
#define MOTORMINSPEED (10)
// interval of velocity checking (10ms)
#define MOTCHKINTERVAL (10)
// maximal ticks of encoder per step
#define MAXENCTICKSPERSTEP (100)
// amount of full steps per revolution
#define STEPSPERREV (200)
extern volatile uint32_t Tms;
@ -195,6 +191,7 @@ extern volatile TIM_TypeDef *mottimers[];
// timers for encoders
extern volatile TIM_TypeDef *enctimers[];
uint8_t ESW_state(uint8_t x);
void gpio_setup();
void iwdg_setup();
void timers_setup();

BIN
F0-nolib/3steppersLB/steppers.bin
View File

Binary file not shown.

148
F0-nolib/3steppersLB/steppers.c
View File

@ -54,8 +54,7 @@ static int32_t encperstep[MOTORSNO];
// current speed
static uint16_t curspeed[MOTORSNO];
// delta V according to current acceleration & INTERVAL
static uint16_t dV[MOTORSNO];
static uint16_t decstartspeed[MOTORSNO]; // speed when deceleration starts
// ==1 to stop @ nearest step
static uint8_t stopflag[MOTORSNO];
// motor state
@ -63,16 +62,26 @@ static stp_state state[MOTORSNO];
// move to zero state
static mvto0state mvzerostate[MOTORSNO];
static uint8_t Nstalled = 0; // counter of STALL
// lowest ARR value (highest speed), highest (lowest speed)
//static uint16_t stphighARR[MOTORSNO];
// microsteps=1<<ustepsshift
static uint16_t ustepsshift[MOTORSNO];
// amount of steps for full eceleration/deceleration
// amount of steps for full acceleration/deceleration
static uint32_t accdecsteps[MOTORSNO];
// time when acceleration or deceleration starts
static uint32_t Taccel[MOTORSNO] = {0};
// recalculate ARR according to new speed
TRUE_INLINE void recalcARR(int i){
mottimers[i]->ARR = (MOTORFREQ / curspeed[i]) >> ustepsshift[i];
uint32_t ARR = (((PCLK/(MOTORTIM_PSC+1)) / curspeed[i]) >> ustepsshift[i]) - 1;
if(ARR < MOTORTIM_ARRMIN) ARR = MOTORTIM_ARRMIN;
else if(ARR > 0xffff) ARR = 0xffff;
mottimers[i]->ARR = ARR;
curspeed[i] = (((PCLK/(MOTORTIM_PSC+1)) / (ARR+1)) >> ustepsshift[i]); // recalculate speed due to new val
//SEND("New ARR["); bufputchar('0'+i); SEND("]="); printu(ARR); NL();
}
// run this function after each steppers parameters changing
@ -83,30 +92,40 @@ void init_steppers(){
stopflag[i] = 0;
motdir[i] = 1;
curspeed[i] = 0;
dV[i] = the_conf.accel[i] * MOTCHKINTERVAL;
dV[i] /= 1000; // interval in ms, but accel in steps/s^2
if(dV[i] == 0) dV[i] = 1;
accdecsteps[i] = (the_conf.maxspd[i] * the_conf.maxspd[i]) / the_conf.accel[i] / 2;
state[i] = STP_RELAX;
ustepsshift[i] = MSB(the_conf.microsteps[i]);
encperstep[i] = the_conf.encrev[i] / 200;
encperstep[i] = the_conf.encrev[i] / STEPSPERREV;
if(!the_conf.motflags[i].donthold) MOTOR_EN(i);
else MOTOR_DIS(i);
}
}
// get absolute position by encoder
static int32_t encoder_position(uint8_t i){
int32_t encoder_position(uint8_t i){
int32_t pos = encpos[i];
if(the_conf.motflags[i].encreverse) pos -= enctimers[i]->CNT;
else pos += enctimers[i]->CNT;
return pos;
}
// set encoder's position, return 0 if false (motor's state != relax)
int setencpos(uint8_t i, int32_t position){
if(state[i] != STP_RELAX) return FALSE;
int32_t remain = position % the_conf.encrev[i];
if(remain < 0) remain += the_conf.encrev[i];
enctimers[i]->CNT = remain;
encpos[i] = position - remain;
SEND("position=");printi(position);SEND(", remain=");printi(remain);SEND(", CNT=");printu(enctimers[i]->CNT);SEND(", pos=");printi(encpos[i]);NL();
return TRUE;
}
// get current position
errcodes getpos(uint8_t i, int32_t *position){
if(the_conf.motflags[i].haveencoder){
/*if(the_conf.motflags[i].haveencoder){
*position = encoder_position(i) / encperstep[i];
}else *position = stppos[i];
}else */
*position = stppos[i];
return ERR_OK;
}
@ -121,11 +140,13 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){
if(state[i] != STP_RELAX) return ERR_CANTRUN; // can't move: motor isn't stopping
if(newpos > (int32_t)the_conf.maxsteps[i] || newpos < -(int32_t)the_conf.maxsteps[i] || newpos == stppos[i])
return ERR_BADVAL; // too big position or zero
Nstalled = 0;
targstppos[i] = newpos;
prevencpos[i] = encoder_position(i);
prevstppos[i] = stppos[i];
uint8_t inv = the_conf.motflags[i].reverse;
int32_t delta = newpos - stppos[i];
SEND("delta="); printi(delta);
if(delta > 0){ // positive direction
if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid
decelstartpos[i] = targstppos[i] - accdecsteps[i];
@ -146,11 +167,15 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){
if(inv) MOTOR_CW(i);
else MOTOR_CCW(i);
}
curspeed[i] = MOTORMINSPEED;
SEND("\ntargstppos="); printi(targstppos[i]);
SEND("\ndecelstart="); printi(decelstartpos[i]);
SEND("\naccdecsteps="); printu(accdecsteps[i]); NL();
curspeed[i] = the_conf.minspd[i];
recalcARR(i);
MOTOR_EN(i);
mottimers[i]->CR1 |= TIM_CR1_CEN; // start timer
state[i] = STP_ACCEL;
Taccel[i] = Tms;
return ERR_OK;
}
@ -170,8 +195,6 @@ stp_state getmotstate(uint8_t i){
// count steps @tim 14/15/16
void addmicrostep(uint8_t i){
static volatile uint16_t microsteps[MOTORSNO] = {0}; // current microsteps position
if(mottimers[i]->SR & TIM_SR_UIF){
DBG("MOTUP");
if(ESW_state(i)){ // ESW active
switch(the_conf.ESW_reaction[i]){
case ESW_ANYSTOP: // stop motor in any direction
@ -196,8 +219,6 @@ void addmicrostep(uint8_t i){
}
}
}
mottimers[i]->SR = 0;
}
void encoders_UPD(uint8_t i){
if(enctimers[i]->SR & TIM_SR_UIF){
@ -214,44 +235,55 @@ void encoders_UPD(uint8_t i){
// @return 0 if moving OK,
static t_stalled chkSTALL(uint8_t i){
if(!the_conf.motflags[i].haveencoder) return STALL_NO;
static uint8_t Nstalled = 0; // counter of STALL
int32_t curencpos = encoder_position(i), Denc = curencpos - prevencpos[i];
int32_t curstppos = stppos[i], Dstp = curstppos - prevstppos[i];
if(Denc < 0) Denc = -Denc;
if(Dstp < 0) Dstp = -Dstp;
if(Dstp < 10){ // didn't move even @ 10 steps
return STALL_NO;
}
prevencpos[i] = curencpos;
if(Dstp == 0){ // veird things
stopmotor(i);
return STALL_STOP;
}
if(Denc < the_conf.encperstepmin[i]*Dstp || the_conf.encperstepmax[i]*Dstp < Denc){ // stall?
if(++Nstalled >= NSTALLEDMAX){
stopflag[i] = 1;
Nstalled = 0;
return STALL_STOP;
}else{
uint16_t spd = curspeed[i] >> 1; // speed / 2
curspeed[i] = (spd > MOTORMINSPEED) ? spd : MOTORMINSPEED;
recalcARR(i);
if(state[i] == STP_MOVE)
state[i] = STP_ACCEL;
return STALL_ONCE;
}
}else Nstalled = 0;
curstppos = curencpos / encperstep[i]; // recalculate current position
stppos[i] = curstppos;
prevstppos[i] = curstppos;
if(Denc < the_conf.encperstepmin[i]*Dstp || the_conf.encperstepmax[i]*Dstp < Denc){ // stall?
SEND("Denc="); printu(Denc); SEND(", Dstp="); printu(Dstp); NL();
if(++Nstalled >= NSTALLEDMAX){
stopflag[i] = 1;
Nstalled = 0;
DBG("STALL!");
return STALL_STOP;
}else{
uint16_t spd = curspeed[i] >> 1; // speed / 2
curspeed[i] = (spd > the_conf.minspd[i]) ? spd : the_conf.minspd[i];
recalcARR(i);
if(state[i] == STP_MOVE)
state[i] = STP_ACCEL;
SEND("pre-stall, speed="); printu(curspeed[i]); NL();
return STALL_ONCE;
}
}
Nstalled = 0;
return STALL_NO;
}
#define TODECEL() do{state[i] = STP_DECEL; \
decstartspeed[i] = curspeed[i]; \
Taccel[i] = Tms; \
SEND(" -> DECEL@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL();}while(0)
// check state of i`th stepper
static void chkstepper(int i){
int32_t newspeed;
switch(state[i]){
case STP_ACCEL: // acceleration to max speed
if(STALL_NO == chkSTALL(i)){
newspeed = curspeed[i] + dV[i];
//newspeed = curspeed[i] + dV[i];
newspeed = the_conf.minspd[i] + (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
if(newspeed >= the_conf.maxspd[i]){ // max speed reached -> move with it
curspeed[i] = the_conf.maxspd[i];
state[i] = STP_MOVE;
SEND(" -> MOVE@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL();
}else{ // increase speed
curspeed[i] = newspeed;
}
@ -259,45 +291,57 @@ static void chkstepper(int i){
}
// check position for triangle profile
if(motdir[i] > 0){
if(stppos[i] >= decelstartpos[i]) // reached end of acceleration
state[i] = STP_DECEL;
if(stppos[i] >= decelstartpos[i]){ // reached end of acceleration
TODECEL();
}
}else{
if(stppos[i] <= decelstartpos[i])
state[i] = STP_DECEL;
if(stppos[i] <= decelstartpos[i]){
TODECEL();
}
}
break;
case STP_MOVE: // move @ constant speed until need to decelerate
if(STALL_NO == chkSTALL(i)){
// check position
if(motdir[i] > 0){
if(stppos[i] >= decelstartpos[i]) // reached start of deceleration
state[i] = STP_DECEL;
if(stppos[i] >= decelstartpos[i]){ // reached start of deceleration
TODECEL();
}
}else{
if(stppos[i] <= decelstartpos[i])
state[i] = STP_DECEL;
if(stppos[i] <= decelstartpos[i]){
TODECEL();
}
}
}
break;
case STP_DECEL:
if(STALL_NO == chkSTALL(i)){
newspeed = curspeed[i] - dV[i];
if(newspeed > MOTORMINSPEED){
//newspeed = curspeed[i] - dV[i];
newspeed = decstartspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
if(newspeed > the_conf.minspd[i]){
curspeed[i] = newspeed;
}else{
curspeed[i] = MOTORMINSPEED;
curspeed[i] = the_conf.minspd[i];
state[i] = STP_MVSLOW;
SEND(" -> MVSLOW@"); printi(stppos[i]); NL();
}
recalcARR(i);
//SEND("spd="); printu(curspeed[i]); SEND(", pos="); printi(stppos[i]); newline();
}
// fallthrough
case STP_MVSLOW:
case STP_MVSLOW: // position check is in add_microstep
/*
if(motdir[i] > 0){
if(stppos[i] >= targstppos[i]) // reached start of deceleration
stopflag[i] = 0;
}else{
if(stppos[i] <= targstppos[i])
if(stppos[i] >= targstppos[i]){ // reached start of deceleration
stopflag[i] = 0;
SEND(" -> STOP@"); printi(stppos[i]); NL();
}
}else{
if(stppos[i] <= targstppos[i]){
stopflag[i] = 0;
SEND(" -> STOP@"); printi(stppos[i]); NL();
}
}*/
break;
default: // RELAX, STALL, ERR -> do nothing
return;

5
F0-nolib/3steppersLB/steppers.h
View File

@ -24,7 +24,7 @@
#include "commonproto.h"
// amount of tries to detect motor stall
#define NSTALLEDMAX (5)
#define NSTALLEDMAX (55)
// stepper states
typedef enum{
@ -50,6 +50,9 @@ void addmicrostep(uint8_t i);
void encoders_UPD(uint8_t i);
void init_steppers();
int32_t encoder_position(uint8_t i);
int setencpos(uint8_t i, int32_t position);
errcodes getpos(uint8_t i, int32_t *position);
errcodes getremainsteps(uint8_t i, int32_t *position);
errcodes motor_absmove(uint8_t i, int32_t abssteps);

17
F0-nolib/3steppersLB/strfunct.c
View File

@ -397,6 +397,22 @@ void wdcheck(_U_ char *txt){
while(1){nop();}
}
void stp_check(char *txt){
uint8_t N = *txt - '0';
if(N < 3){
MOTOR_EN(N);
MOTOR_CW(N);
mottimers[N]->ARR = 300;
mottimers[N]->CR1 |= TIM_CR1_CEN;
}else{
for(N = 0; N < 3; ++N){
MOTOR_DIS(N);
MOTOR_CCW(N);
mottimers[N]->CR1 &= ~TIM_CR1_CEN;
}
}
}
typedef void(*specfpointer)(char *arg);
typedef struct{
@ -420,6 +436,7 @@ const speccommands scmdlist[] = {
{"dumpconf", dump_userconf, "dump current configuration"},
{"getctr", getcounter, "get TIM1/2/3 counters"},
{"wd", wdcheck, "check watchdog"},
{"st", stp_check, "check steppers"},
{NULL, NULL, NULL}
};

4
F0-nolib/3steppersLB/version.inc
View File

@ -1,2 +1,2 @@
#define BUILD_NUMBER "43"
#define BUILD_DATE "2021-11-14"
#define BUILD_NUMBER "97"
#define BUILD_DATE "2021-11-16"