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

3steppers: fix some bugs, add commands eraseflash and setpos

Browse Source
This commit is contained in:
Edward Emelianov 2022-06-17 12:12:47 +03:00
parent 16f72f6801
commit 6f29257a28
10 changed files with 195 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
F0:F030,F042,F072/3steppersLB/Makefile
View File

@ -5,7 +5,7 @@ BOOTSPEED ?= 57600
FAMILY ?= F0 FAMILY ?= F0
# MCU code # MCU code
MCU ?= F072xB MCU ?= F072xB
DEFS += -DEBUG #DEFS += -DEBUG
# change this linking script depending on particular MCU model, # change this linking script depending on particular MCU model,
# for example, if you have STM32F103VBT6, you should write: # for example, if you have STM32F103VBT6, you should write:
LDSCRIPT ?= stm32f072B.ld LDSCRIPT ?= stm32f072B.ld

103
F0:F030,F042,F072/3steppersLB/Readme.md
View File

@ -142,10 +142,10 @@ Common commands format is cmd[ N[ = val]]
where N is command argument (0..127), val is its value where N is command argument (0..127), val is its value
Different commands: Different commands:
adc - get ADC values adc - get ADC values
button - get buttons state button - get buttons state (return time of last event & ERRCODE == buttonstate)
buzzer - change buzzer state (1/0) buzzer - change buzzer state (1/0)
esw - get end switches state esw - get end switches state (without number - all, by bytes)
ext - external outputs ext - external outputs (without number - all, by bytes; value= 0-off, 1-on, other-toggle)
mcut - get MCU T mcut - get MCU T
mcuvdd - get MCU Vdd mcuvdd - get MCU Vdd
ping - echo given command back ping - echo given command back
@ -175,6 +175,7 @@ Motors' commands:
motreinit - re-init motors after configuration changed motreinit - re-init motors after configuration changed
relpos - set relative steps, get remaining relpos - set relative steps, get remaining
relslow - set relative steps @ lowest speed relslow - set relative steps @ lowest speed
setpos - set/get absolute position (in steps)
state - get motor state state - get motor state
stop - smooth motor stopping stop - smooth motor stopping
USB-only commands: USB-only commands:
@ -185,6 +186,7 @@ USB-only commands:
dumperr - dump error codes dumperr - dump error codes
dumpcmd - dump command codes dumpcmd - dump command codes
dumpconf - dump current configuration dumpconf - dump current configuration
eraseflash - erase flash data storage
filter - add/modify filter, format: bank# FIFO# mode(M/I) num0 [num1 [num2 [num3]]] filter - add/modify filter, format: bank# FIFO# mode(M/I) num0 [num1 [num2 [num3]]]
getctr - get TIM1/2/3 counters getctr - get TIM1/2/3 counters
ignbuf - print ignore buffer ignbuf - print ignore buffer
@ -208,7 +210,6 @@ bytes descr
7 Hdata 7 Hdata
dumperr dumperr
Find known command: dumperr
Error codes: Error codes:
0 - all OK 0 - all OK
1 - wrong parameter's value 1 - wrong parameter's value
@ -219,82 +220,82 @@ Error codes:
dumpcmd dumpcmd
Find known command: dumpcmd
Commands list: Commands list:
0 - Different commands: 1 - echo given command back
1 - change relay state (1/0) 2 - change relay state (1/0)
2 - change buzzer state (1/0) 3 - change buzzer state (1/0)
3 - get ADC values 4 - get ADC values
4 - get buttons state 5 - get buttons state
5 - get end switches state 6 - get end switches state
6 - get MCU T 7 - get MCU T
7 - get MCU Vdd 8 - get MCU Vdd
8 - reset MCU 9 - reset MCU
9 - get time from start 10 - get time from start
10 - pwm value 11 - pwm value
11 - external outputs 12 - external outputs
12 - save current configuration 13 - save current configuration
13 - minimal encoder ticks per step 14 - minimal encoder ticks per step
14 - maximal encoder ticks per step 15 - maximal encoder ticks per step
15 - set/get microsteps settings 16 - set/get microsteps settings
16 - set/get accel/decel (steps/s^2) 17 - set/get accel/decel (steps/s^2)
17 - set/get max speed (steps per sec) 18 - set/get max speed (steps per sec)
18 - set/get min speed (steps per sec) 19 - set/get min speed (steps per sec)
19 - get limiting speed for current microsteps 20 - get limiting speed for current microsteps
20 - set/get max steps (from zero) 21 - set/get max steps (from zero)
21 - set/get max encoder's pulses per revolution 22 - set/get max encoder's pulses per revolution
22 - set/get motorN flags 23 - set/get motorN flags
23 - end-switches reaction 24 - end-switches reaction
24 - re-init motors after configuration changed 25 - re-init motors after configuration changed
25 - set/get position (in steps) 26 - move to/get absolute position (in steps)
26 - set relative steps, get remaining 27 - set relative steps, get remaining
27 - set relative steps @ lowest speed 28 - set relative steps @ lowest speed
28 - emergency stop motor (right now) 29 - emergency stop motor (right now)
29 - smooth motor stopping 30 - smooth motor stopping
30 - emergency stop all motors 31 - emergency stop all motors
31 - find zero position & refresh counters 32 - find zero position & refresh counters
32 - get motor state 33 - get motor state
33 - set/get encoder's position 34 - set/get encoder's position
35 - set/get absolute position (in steps)
dumpconf dumpconf
Find known command: dumpconf userconf_addr=0x08006000// address from which userconf started
flashsize=64*2048=131072 userconf_idx=5 // "index of stored conf"
userconf_addr=0x08006800
userconf_idx=-1 // "index of stored conf"
userconf_sz=68 // "magick number" userconf_sz=68 // "magick number"
canspeed=100 // default CAN speed canspeed=100 // default CAN speed
canid=170 // identifier (0xaa) canid=170 // identifier (0xaa)
microsteps0=32 // microsteps amount per step microsteps0=32 // microsteps amount per step
accel0=500 // acceleration/deceleration (steps/s^2) accel0=1500 // acceleration/deceleration (steps/s^2)
maxspeed0=2000 // max motor speed (steps per second) maxspeed0=1501 // max motor speed (steps per second)
minspeed0=20 // min motor speed (steps per second) minspeed0=20 // min motor speed (steps per second)
maxsteps0=500000 // maximal amount of steps maxsteps0=500000 // maximal amount of steps
encperrev0=4000 // encoders' counts per revolution encperrev0=4000 // encoders' counts per revolution
encperstepmin0=17 // min amount of encoder ticks per one step encperstepmin0=17 // min amount of encoder ticks per one step
encperstepmax0=23 // max amount of encoder ticks per one step encperstepmax0=23 // max amount of encoder ticks per one step
motflags0=0x3c // motor's flags motflags0=0x2f // motor's flags
eswreaction0=0 // end-switches reaction (esw_react) eswreaction0=0 // end-switches reaction (esw_react)
microsteps1=32 microsteps1=32
accel1=500 accel1=1500
maxspeed1=2000 maxspeed1=2000
minspeed1=20 minspeed1=20
maxsteps1=500000 maxsteps1=500000
encperrev1=4000 encperrev1=4000
encperstepmin1=17 encperstepmin1=17
encperstepmax1=23 encperstepmax1=23
motflags1=0x3c motflags1=0x2f
eswreaction1=0 eswreaction1=0
microsteps2=32 microsteps2=32
accel2=500 accel2=1500
maxspeed2=2000 maxspeed2=2500
minspeed2=20 minspeed2=20
maxsteps2=500000 maxsteps2=500000
encperrev2=4000 encperrev2=4000
encperstepmin2=17 encperstepmin2=17
encperstepmax2=23 encperstepmax2=23
motflags2=0x3c motflags2=0x2f
eswreaction2=0 eswreaction2=0
Motor flags: Motor flags:
bit0 - reversing motor rotation bit0 - reversing motor rotation
bit1 - reversing encoder rotation bit1 - reversing encoder rotation

63
F0:F030,F042,F072/3steppersLB/commonproto.c
View File

@ -27,6 +27,11 @@
#include "strfunct.h" #include "strfunct.h"
#endif #endif
#define NOPARCHK() do{uint8_t n = PARBASE(par); if(n != CANMESG_NOPAR) return ERR_BADPAR;}while(0)
#define CHECKN(val, par) do{val = PARBASE(par); \
if(val > MOTORSNO-1) return ERR_BADPAR;}while(0)
/******* All functions from cmdlist[i].function *******/ /******* All functions from cmdlist[i].function *******/
static errcodes pingparser(uint8_t _U_ par, int32_t _U_ *val){ static errcodes pingparser(uint8_t _U_ par, int32_t _U_ *val){
@ -34,6 +39,7 @@ static errcodes pingparser(uint8_t _U_ par, int32_t _U_ *val){
} }
static errcodes relayparser(uint8_t par, int32_t *val){ static errcodes relayparser(uint8_t par, int32_t *val){
NOPARCHK();
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val) ON(RELAY); if(*val) ON(RELAY);
else OFF(RELAY); else OFF(RELAY);
@ -43,6 +49,7 @@ static errcodes relayparser(uint8_t par, int32_t *val){
} }
static errcodes buzzerparser(uint8_t par, int32_t *val){ static errcodes buzzerparser(uint8_t par, int32_t *val){
NOPARCHK();
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val) ON(BUZZER); if(*val) ON(BUZZER);
else OFF(BUZZER); else OFF(BUZZER);
@ -63,7 +70,7 @@ static errcodes adcparser(uint8_t par, int32_t *val){
static errcodes buttonsparser(uint8_t par, int32_t *val){ static errcodes buttonsparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n = PARBASE(par);
if(n > BTNSNO-1){ if(n > BTNSNO-1){
par = CANMESG_NOPAR; // the only chance to understand error *val = CANMESG_NOPAR; // the only chance to understand error
return ERR_BADPAR; return ERR_BADPAR;
} }
return (uint8_t) keystate(n, (uint32_t*)val); return (uint8_t) keystate(n, (uint32_t*)val);
@ -75,39 +82,43 @@ static errcodes eswparser(uint8_t par, int32_t *val){
#error "change the code!!!" #error "change the code!!!"
#endif #endif
uint8_t n = PARBASE(par); uint8_t n = PARBASE(par);
if(n > ESWNO-1){ // all if(n == CANMESG_NOPAR){ // all
*val = 0; *val = 0;
uint8_t *arr = (uint8_t*)val; uint8_t *arr = (uint8_t*)val;
for(int i = 0; i < ESWNO; ++i) for(int i = 0; i < ESWNO; ++i)
*arr++ = ESW_state(i); *arr++ = ESW_state(i);
return ERR_OK; return ERR_OK;
} }else if(n > ESWNO - 1) return ERR_BADPAR;
*val = (int32_t)ESW_state(n); *val = (int32_t)ESW_state(n);
return ERR_OK; return ERR_OK;
} }
static errcodes mcutparser(uint8_t _U_ par, int32_t *val){ static errcodes mcutparser(uint8_t _U_ par, int32_t *val){
NOPARCHK();
*val = getMCUtemp(); *val = getMCUtemp();
return ERR_OK; return ERR_OK;
} }
static errcodes mcuvddparser(uint8_t _U_ par, int32_t *val){ static errcodes mcuvddparser(uint8_t _U_ par, int32_t *val){
NOPARCHK();
*val = getVdd(); *val = getVdd();
return ERR_OK; return ERR_OK;
} }
static errcodes resetparser(uint8_t _U_ par, int32_t _U_ *val){ static errcodes resetparser(uint8_t _U_ par, int32_t _U_ *val){
NOPARCHK();
NVIC_SystemReset(); NVIC_SystemReset();
return ERR_OK; return ERR_OK;
} }
static errcodes timeparser(uint8_t _U_ par, int32_t *val){ static errcodes timeparser(uint8_t _U_ par, int32_t *val){
NOPARCHK();
*val = Tms; *val = Tms;
return ERR_OK; return ERR_OK;
} }
static errcodes pwmparser(uint8_t par, int32_t *val){ static errcodes pwmparser(uint8_t par, int32_t *val){
if(PARBASE(par) > PWMCHMAX && par != CANMESG_NOPAR) return ERR_BADPAR; NOPARCHK();
#if PWMCHMAX != 0 #if PWMCHMAX != 0
#error "change the code!!!" #error "change the code!!!"
#endif #endif
@ -144,7 +155,7 @@ static errcodes extparser(uint8_t par, int32_t *val){
SEND("par="); printu(par); SEND("par="); printu(par);
SEND(", n="); bufputchar('0'+n); newline(); SEND(", n="); bufputchar('0'+n); newline();
#endif #endif
if(n > EXTNO-1){ // all if(n == CANMESG_NOPAR){ // all
#ifdef EBUG #ifdef EBUG
SEND("ALL\n"); SEND("ALL\n");
#endif #endif
@ -157,7 +168,7 @@ static errcodes extparser(uint8_t par, int32_t *val){
arr[i] = EXT_CHK(i); arr[i] = EXT_CHK(i);
} }
return ERR_OK; return ERR_OK;
} }else if(n > EXTNO-1) return ERR_BADPAR;
if(ISSETTER(par)) if(ISSETTER(par))
setextpar((uint8_t)*val, n); setextpar((uint8_t)*val, n);
*val = (int32_t) EXT_CHK(n); *val = (int32_t) EXT_CHK(n);
@ -166,8 +177,7 @@ static errcodes extparser(uint8_t par, int32_t *val){
/******************* START of config parsers *******************/ /******************* START of config parsers *******************/
static errcodes ustepsparser(uint8_t par, int32_t *val){ static errcodes ustepsparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
#if MICROSTEPSMAX > 512 #if MICROSTEPSMAX > 512
#error "Change the code anywhere!" #error "Change the code anywhere!"
@ -184,8 +194,7 @@ static errcodes ustepsparser(uint8_t par, int32_t *val){
} }
static errcodes encstepsminparser(uint8_t par, int32_t *val){ static errcodes encstepsminparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val < 1 || *val > MAXENCTICKSPERSTEP - 1) return ERR_BADVAL; if(*val < 1 || *val > MAXENCTICKSPERSTEP - 1) return ERR_BADVAL;
the_conf.encperstepmin[n] = *val; the_conf.encperstepmin[n] = *val;
@ -195,8 +204,7 @@ static errcodes encstepsminparser(uint8_t par, int32_t *val){
} }
static errcodes encstepsmaxparser(uint8_t par, int32_t *val){ static errcodes encstepsmaxparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val < 1 || *val > MAXENCTICKSPERSTEP) return ERR_BADVAL; if(*val < 1 || *val > MAXENCTICKSPERSTEP) return ERR_BADVAL;
the_conf.encperstepmax[n] = *val; the_conf.encperstepmax[n] = *val;
@ -206,8 +214,7 @@ static errcodes encstepsmaxparser(uint8_t par, int32_t *val){
} }
static errcodes accparser(uint8_t par, int32_t *val){ static errcodes accparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val/the_conf.microsteps[n] > ACCELMAXSTEPS || *val < 1) return ERR_BADVAL; if(*val/the_conf.microsteps[n] > ACCELMAXSTEPS || *val < 1) return ERR_BADVAL;
the_conf.accel[n] = *val; the_conf.accel[n] = *val;
@ -227,8 +234,7 @@ static uint16_t getSPD(uint8_t n, int32_t speed){
} }
static errcodes maxspdparser(uint8_t par, int32_t *val){ static errcodes maxspdparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val <= the_conf.minspd[n]) return ERR_BADVAL; if(*val <= the_conf.minspd[n]) return ERR_BADVAL;
the_conf.maxspd[n] = getSPD(n, *val); the_conf.maxspd[n] = getSPD(n, *val);
@ -238,8 +244,7 @@ static errcodes maxspdparser(uint8_t par, int32_t *val){
} }
static errcodes minspdparser(uint8_t par, int32_t *val){ static errcodes minspdparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val >= the_conf.maxspd[n]) return ERR_BADVAL; if(*val >= the_conf.maxspd[n]) return ERR_BADVAL;
the_conf.minspd[n] = getSPD(n, *val); the_conf.minspd[n] = getSPD(n, *val);
@ -249,15 +254,13 @@ static errcodes minspdparser(uint8_t par, int32_t *val){
} }
static errcodes spdlimparser(uint8_t par, int32_t *val){ static errcodes spdlimparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
*val = getSPD(n, 0xffff); *val = getSPD(n, 0xffff);
return ERR_OK; return ERR_OK;
} }
static errcodes maxstepsparser(uint8_t par, int32_t *val){ static errcodes maxstepsparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val < 1) return ERR_BADVAL; if(*val < 1) return ERR_BADVAL;
the_conf.maxsteps[n] = *val; the_conf.maxsteps[n] = *val;
@ -267,8 +270,7 @@ static errcodes maxstepsparser(uint8_t par, int32_t *val){
} }
static errcodes encrevparser(uint8_t par, int32_t *val){ static errcodes encrevparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val < 1 || *val > MAXENCREV) return ERR_BADVAL; if(*val < 1 || *val > MAXENCREV) return ERR_BADVAL;
the_conf.encrev[n] = *val; the_conf.encrev[n] = *val;
@ -279,8 +281,7 @@ static errcodes encrevparser(uint8_t par, int32_t *val){
} }
static errcodes motflagsparser(uint8_t par, int32_t *val){ static errcodes motflagsparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
the_conf.motflags[n] = *((motflags_t*)val); the_conf.motflags[n] = *((motflags_t*)val);
} }
@ -290,8 +291,7 @@ static errcodes motflagsparser(uint8_t par, int32_t *val){
// setter of GLOBAL reaction, getter of LOCAL! // setter of GLOBAL reaction, getter of LOCAL!
static errcodes eswreactparser(uint8_t par, int32_t *val){ static errcodes eswreactparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par); uint8_t n; CHECKN(n, par);
if(n > MOTORSNO-1) return ERR_BADPAR;
if(ISSETTER(par)){ if(ISSETTER(par)){
if(*val < 0 || *val > ESW_AMOUNT-1) return ERR_BADVAL; if(*val < 0 || *val > ESW_AMOUNT-1) return ERR_BADVAL;
the_conf.ESW_reaction[n] = *val; the_conf.ESW_reaction[n] = *val;
@ -302,6 +302,7 @@ static errcodes eswreactparser(uint8_t par, int32_t *val){
} }
static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){ static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){
NOPARCHK();
if(store_userconf()) return ERR_CANTRUN; if(store_userconf()) return ERR_CANTRUN;
return ERR_OK; return ERR_OK;
} }
@ -310,13 +311,11 @@ static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){
/******************* START of motors' parsers *******************/ /******************* START of motors' parsers *******************/
static errcodes reinitmparser(uint8_t _U_ par, int32_t _U_ *val){ static errcodes reinitmparser(uint8_t _U_ par, int32_t _U_ *val){
NOPARCHK();
init_steppers(); init_steppers();
return ERR_OK; return ERR_OK;
} }
#define CHECKN(val, par) do{val = PARBASE(par); \
if(val > MOTORSNO-1) return ERR_BADPAR;}while(0)
static errcodes emstopparser(uint8_t par, int32_t _U_ *val){ static errcodes emstopparser(uint8_t par, int32_t _U_ *val){
uint8_t n; CHECKN(n, par); uint8_t n; CHECKN(n, par);
emstopmotor(n); emstopmotor(n);
@ -324,6 +323,7 @@ static errcodes emstopparser(uint8_t par, int32_t _U_ *val){
} }
static errcodes emstopallparser(uint8_t _U_ par, int32_t _U_ *val){ static errcodes emstopallparser(uint8_t _U_ par, int32_t _U_ *val){
NOPARCHK();
for(int i = 0; i < MOTORSNO; ++i) for(int i = 0; i < MOTORSNO; ++i)
emstopmotor(i); emstopmotor(i);
return ERR_OK; return ERR_OK;
@ -384,7 +384,6 @@ static errcodes gotozeroparser(uint8_t par, _U_ int32_t *val){
return motor_goto0(n); return motor_goto0(n);
} }
#undef CHECKN
/******************* END of motors' parsers *******************/ /******************* END of motors' parsers *******************/
/* /*

6
F0:F030,F042,F072/3steppersLB/flash.c
View File

@ -112,7 +112,7 @@ int store_userconf(){
// for binarySearch() checking that there's nothing more after it! // for binarySearch() checking that there's nothing more after it!
if(currentconfidx > (int)maxCnum - 3){ // there's no more place if(currentconfidx > (int)maxCnum - 3){ // there's no more place
currentconfidx = 0; currentconfidx = 0;
if(erase_flash(Flash_Data, (&__varsstart))) return 1; if(erase_flash(Flash_Data, NULL)) return 1;
}else ++currentconfidx; // take next data position (0 - within first run after firmware flashing) }else ++currentconfidx; // take next data position (0 - within first run after firmware flashing)
return write2flash((const void*)&Flash_Data[currentconfidx], &the_conf, sizeof(the_conf)); return write2flash((const void*)&Flash_Data[currentconfidx], &the_conf, sizeof(the_conf));
} }
@ -241,3 +241,7 @@ void dump_userconf(_U_ char *txt){
} }
NL(); NL();
} }
int erase_storage(){
return erase_flash(Flash_Data, NULL);
}

1
F0:F030,F042,F072/3steppersLB/flash.h
View File

@ -79,5 +79,6 @@ extern user_conf the_conf; // global user config (read from FLASH to RAM)
void flashstorage_init(); void flashstorage_init();
int store_userconf(); int store_userconf();
void dump_userconf(_U_ char *txt); void dump_userconf(_U_ char *txt);
int erase_storage();
#endif // __FLASH_H__ #endif // __FLASH_H__

BIN
F0:F030,F042,F072/3steppersLB/steppers.bin
View File

Binary file not shown.

108
F0:F030,F042,F072/3steppersLB/steppers.c
View File

@ -34,6 +34,10 @@ typedef enum{
STALL_STOP // Nstalled >= limit STALL_STOP // Nstalled >= limit
} t_stalled; } t_stalled;
#ifdef EBUG
static uint8_t stp[MOTORSNO] = {0};
#endif
static t_stalled stallflags[MOTORSNO]; static t_stalled stallflags[MOTORSNO];
// motors' direction: 1 for positive, -1 for negative (we need it as could be reverse) // motors' direction: 1 for positive, -1 for negative (we need it as could be reverse)
@ -162,6 +166,15 @@ errcodes getremainsteps(uint8_t i, int32_t *position){
// calculate acceleration/deceleration parameters for motor i // calculate acceleration/deceleration parameters for motor i
static void calcacceleration(uint8_t i){ static void calcacceleration(uint8_t i){
switch(state[i]){ // do nothing in case of error/stopping
case STP_ERR:
case STP_RELAX:
case STP_STALL:
return;
break;
default:
break;
}
int32_t delta = targstppos[i] - stppos[i]; int32_t delta = targstppos[i] - stppos[i];
if(delta > 0){ // positive direction if(delta > 0){ // positive direction
if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid
@ -181,7 +194,10 @@ static void calcacceleration(uint8_t i){
if(the_conf.motflags[i].reverse) MOTOR_CW(i); if(the_conf.motflags[i].reverse) MOTOR_CW(i);
else MOTOR_CCW(i); else MOTOR_CCW(i);
} }
if(state[i] != STP_MVSLOW) state[i] = STP_ACCEL; if(state[i] != STP_MVSLOW){
DBG("->accel");
state[i] = STP_ACCEL;
}
startspeed[i] = curspeed[i]; startspeed[i] = curspeed[i];
Taccel[i] = Tms; Taccel[i] = Tms;
recalcARR(i); recalcARR(i);
@ -215,20 +231,21 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){
case STP_RELAX: case STP_RELAX:
break; break;
case STP_STALL: case STP_STALL:
DBG("Move from STALL");
if(dir == stalleddir[i]){ if(dir == stalleddir[i]){
DBG("Move to stalled direction!"); DBG("Move to stalled direction!");
return ERR_CANTRUN; // can't run into stalled direction return ERR_CANTRUN; // can't run into stalled direction
} }
break; break;
default: // moving state default: // moving state
DBG("Always moving"); DBG("Is moving");
return ERR_CANTRUN; return ERR_CANTRUN;
} }
if(newpos > (int32_t)the_conf.maxsteps[i] || newpos < -(int32_t)the_conf.maxsteps[i] || newpos == stppos[i]){ if(newpos > (int32_t)the_conf.maxsteps[i] || newpos < -(int32_t)the_conf.maxsteps[i] || newpos == stppos[i]){
DBG("Too much steps"); DBG("Too much steps");
return ERR_BADVAL; // too big position or zero return ERR_BADVAL; // too big position or zero
} }
motdir[i] = dir; motdir[i] = dir; // should be before limit switch check
if(esw_block(i)){ if(esw_block(i)){
DBG("Block by ESW"); DBG("Block by ESW");
return ERR_CANTRUN; // on end-switch return ERR_CANTRUN; // on end-switch
@ -239,6 +256,7 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){
prevencpos[i] = encoder_position(i); prevencpos[i] = encoder_position(i);
prevstppos[i] = stppos[i]; prevstppos[i] = stppos[i];
curspeed[i] = the_conf.minspd[i]; curspeed[i] = the_conf.minspd[i];
state[i] = STP_ACCEL;
calcacceleration(i); calcacceleration(i);
#ifdef EBUG #ifdef EBUG
SEND("MOTOR"); bufputchar('0'+i); SEND("MOTOR"); bufputchar('0'+i);
@ -259,6 +277,7 @@ errcodes motor_relmove(uint8_t i, int32_t relsteps){
errcodes motor_relslow(uint8_t i, int32_t relsteps){ errcodes motor_relslow(uint8_t i, int32_t relsteps){
errcodes e = motor_absmove(i, stppos[i] + relsteps); errcodes e = motor_absmove(i, stppos[i] + relsteps);
if(ERR_OK == e){ if(ERR_OK == e){
DBG("-> MVSLOW");
state[i] = STP_MVSLOW; state[i] = STP_MVSLOW;
} }
return e; return e;
@ -269,6 +288,7 @@ void emstopmotor(uint8_t i){
switch(state[i]){ switch(state[i]){
case STP_ERR: // clear error state case STP_ERR: // clear error state
case STP_STALL: case STP_STALL:
DBG("-> RELAX");
state[i] = STP_RELAX; state[i] = STP_RELAX;
// fallthrough // fallthrough
case STP_RELAX: // do nothing in stopping state case STP_RELAX: // do nothing in stopping state
@ -301,18 +321,19 @@ void addmicrostep(uint8_t i){
} }
} }
if(stopflag[i] || stop_at_pos){ // stop NOW if(stopflag[i] || stop_at_pos){ // stop NOW
mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer
if(stopflag[i]) targstppos[i] = stppos[i]; // keep position (for keep flag) if(stopflag[i]) targstppos[i] = stppos[i]; // keep position (for keep flag)
stopflag[i] = 0; stopflag[i] = 0;
mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer
if(the_conf.motflags[i].donthold) if(the_conf.motflags[i].donthold)
MOTOR_DIS(i); // turn off power MOTOR_DIS(i); // turn off power
if(stallflags[i] == STALL_STOP){ if(stallflags[i] == STALL_STOP){
stallflags[i] = STALL_NO; stallflags[i] = STALL_NO;
state[i] = STP_STALL; state[i] = STP_STALL;
}else }else{
state[i] = STP_RELAX; state[i] = STP_RELAX;
}
#ifdef EBUG #ifdef EBUG
SEND("MOTOR"); bufputchar('0'+i); SEND(" stop @"); printi(stppos[i]); newline(); stp[i] = 1;
#endif #endif
} }
} }
@ -361,15 +382,14 @@ static t_stalled chkSTALL(uint8_t i){
SEND("MOTOR"); bufputchar('0'+i); SEND(" Denc="); printi(Denc); SEND(", Dstp="); printu(Dstp); SEND("MOTOR"); bufputchar('0'+i); SEND(" Denc="); printi(Denc); SEND(", Dstp="); printu(Dstp);
SEND(", speed="); printu(curspeed[i]); SEND(", speed="); printu(curspeed[i]);
#endif #endif
if(++Nstalled[i] > NSTALLEDMAX){ if(++Nstalled[i] >= NSTALLEDMAX){
stopflag[i] = 1;
Nstalled[i] = 0; Nstalled[i] = 0;
#ifdef EBUG #ifdef EBUG
SEND(" --- STALL!"); SEND(" --- STALL!");
#else #else
SEND("ERRCODE="); bufputchar(ERR_CANTRUN+'0'); SEND("ERRCODE="); bufputchar(ERR_CANTRUN+'0');
SEND("\nstate"); bufputchar(i+'0'); bufputchar('='); SEND("\nstate"); bufputchar(i+'0'); bufputchar('=');
bufputchar(STP_STALL); bufputchar(STP_STALL + '0');
#endif #endif
NL(); NL();
stallflags[i] = STALL_STOP; stallflags[i] = STALL_STOP;
@ -408,7 +428,19 @@ static t_stalled chkSTALL(uint8_t i){
// check state of i`th stepper // check state of i`th stepper
static void chkstepper(int i){ static void chkstepper(int i){
int32_t i32; int32_t i32;
t_stalled s = chkSTALL(i);
static int32_t oldtargpos[3] = {0}; // target position of previous `keep` call
static uint8_t keepposctr[3] = {0}; // counter of tries to keep current position
static uint8_t stopctr[3] = {0}; // counters for encoders/position zeroing after stopping @ esw static uint8_t stopctr[3] = {0}; // counters for encoders/position zeroing after stopping @ esw
#ifdef EBUG
if(stp[i]){
stp[i] = 0;
// motor state could be changed outside of interrupt, so return it to relax or leave in STALL
if(state[i] != STP_STALL) state[i] = STP_RELAX;
SEND("MOTOR"); bufputchar('0'+i); SEND(" stop @"); printi(stppos[i]);
SEND(", curstate="); printu(state[i]); newline();
}
#endif
switch(state[i]){ switch(state[i]){
case STP_RELAX: // check if need to keep current position case STP_RELAX: // check if need to keep current position
if(the_conf.motflags[i].haveencoder){ if(the_conf.motflags[i].haveencoder){
@ -425,18 +457,30 @@ static void chkstepper(int i){
if(the_conf.motflags[i].keeppos){ // keep old position if(the_conf.motflags[i].keeppos){ // keep old position
diff = targstppos[i] - i32; // check whether we need to change position diff = targstppos[i] - i32; // check whether we need to change position
if(diff){ // try to correct position if(diff){ // try to correct position
if(oldtargpos[i] == targstppos[i]){
if(++keepposctr[i] >= KEEPPOSMAX){
DBG("Can't keep position");
targstppos[i] = i32;
return; // can't keep position for
}
}else{
oldtargpos[i] = targstppos[i];
keepposctr[i] = 0;
}
#ifdef EBUG #ifdef EBUG
SEND("MOTOR"); bufputchar('0'+i); SEND("MOTOR"); bufputchar('0'+i);
SEND(" curpos="); printi(i32); SEND(", need="); printi(targstppos[i]); NL(); SEND(" curpos="); printi(i32); SEND(", need="); printi(targstppos[i]); NL();
#endif #endif
if(ERR_OK != motor_absmove(i, targstppos[i])) if(ERR_OK != motor_absmove(i, targstppos[i])){
DBG("Can't move to target position for keeping");
targstppos[i] = i32; // can't move to target position - forget it targstppos[i] = i32; // can't move to target position - forget it
}
} }
} }
} }
break; break;
case STP_ACCEL: // acceleration to max speed case STP_ACCEL: // acceleration to max speed
if(STALL_NO == chkSTALL(i)){ if(s == STALL_NO){
//newspeed = curspeed[i] + dV[i]; //newspeed = curspeed[i] + dV[i];
i32 = the_conf.minspd[i] + (the_conf.accel[i] * (Tms - Taccel[i])) / 1000; i32 = the_conf.minspd[i] + (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
if(i32 >= the_conf.maxspd[i]){ // max speed reached -> move with it if(i32 >= the_conf.maxspd[i]){ // max speed reached -> move with it
@ -450,20 +494,22 @@ static void chkstepper(int i){
curspeed[i] = i32; curspeed[i] = i32;
} }
recalcARR(i); recalcARR(i);
} // check position for triangle profile
// check position for triangle profile if(motdir[i] > 0){
if(motdir[i] > 0){ if(stppos[i] >= decelstartpos[i]){ // reached end of acceleration
if(stppos[i] >= decelstartpos[i]){ // reached end of acceleration TODECEL();
TODECEL(); }
} }else{
}else{ if(stppos[i] <= decelstartpos[i]){
if(stppos[i] <= decelstartpos[i]){ TODECEL();
TODECEL(); }
} }
}else if(s == STALL_STOP){
stopflag[i] = 1;
} }
break; break;
case STP_MOVE: // move @ constant speed until need to decelerate case STP_MOVE: // move @ constant speed until need to decelerate
if(STALL_NO == chkSTALL(i)){ if(s == STALL_NO){
// check position // check position
if(motdir[i] > 0){ if(motdir[i] > 0){
if(stppos[i] >= decelstartpos[i]){ // reached start of deceleration if(stppos[i] >= decelstartpos[i]){ // reached start of deceleration
@ -474,10 +520,12 @@ static void chkstepper(int i){
TODECEL(); TODECEL();
} }
} }
}else if(s == STALL_STOP){
stopflag[i] = 1;
} }
break; break;
case STP_DECEL: case STP_DECEL:
if(STALL_NO == chkSTALL(i)){ if(s == STALL_NO){
//newspeed = curspeed[i] - dV[i]; //newspeed = curspeed[i] - dV[i];
i32 = startspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000; i32 = startspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
if(i32 > the_conf.minspd[i]){ if(i32 > the_conf.minspd[i]){
@ -492,10 +540,12 @@ static void chkstepper(int i){
} }
recalcARR(i); recalcARR(i);
//SEND("spd="); printu(curspeed[i]); SEND(", pos="); printi(stppos[i]); newline(); //SEND("spd="); printu(curspeed[i]); SEND(", pos="); printi(stppos[i]); newline();
}else if(s == STALL_STOP){
stopflag[i] = 1;
} }
break; break;
case STP_MVSLOW: case STP_MVSLOW:
chkSTALL(i); if(s == STALL_STOP) stopflag[i] = 1;
break; break;
default: // STALL, ERR -> do nothing, check mvzerostate default: // STALL, ERR -> do nothing, check mvzerostate
break; break;
@ -504,12 +554,13 @@ static void chkstepper(int i){
case M0FAST: case M0FAST:
if(state[i] == STP_RELAX || state[i] == STP_STALL){ // stopped -> move to + if(state[i] == STP_RELAX || state[i] == STP_STALL){ // stopped -> move to +
#ifdef EBUG #ifdef EBUG
SEND("M0FAST: motor stopped\n"); bufputchar('M'); bufputchar('0'+i); SEND("FAST: motor stopped\n");
#endif #endif
if(ERR_OK != motor_relslow(i, 1000)){ if(ERR_OK != motor_relslow(i, 1000)){
#ifdef EBUG #ifdef EBUG
SEND("Can't move\n"); SEND("Can't move\n");
#endif #endif
DBG("->ERR");
state[i] = STP_ERR; state[i] = STP_ERR;
mvzerostate[i] = M0RELAX; mvzerostate[i] = M0RELAX;
ESW_reaction[i] = the_conf.ESW_reaction[i]; ESW_reaction[i] = the_conf.ESW_reaction[i];
@ -525,11 +576,11 @@ static void chkstepper(int i){
} }
if((state[i] == STP_RELAX || state[i] == STP_STALL) && ++stopctr[i] > 5){ // wait at least 50ms if((state[i] == STP_RELAX || state[i] == STP_STALL) && ++stopctr[i] > 5){ // wait at least 50ms
#ifdef EBUG #ifdef EBUG
SEND("M0SLOW: set position to 0\n"); NL(); bufputchar('M'); bufputchar('0'+i); SEND("SLOW: motor stopped\n");
#endif #endif
ESW_reaction[i] = the_conf.ESW_reaction[i]; ESW_reaction[i] = the_conf.ESW_reaction[i];
prevencpos[i] = encpos[i] = 0; prevencpos[i] = encpos[i] = 0;
targstppos[i] = stppos[i] = 0; prevstppos[i] = targstppos[i] = stppos[i] = 0;
enctimers[i]->CNT = 0; // set encoder counter to zero enctimers[i]->CNT = 0; // set encoder counter to zero
mvzerostate[i] = M0RELAX; mvzerostate[i] = M0RELAX;
} }
@ -541,8 +592,9 @@ static void chkstepper(int i){
errcodes motor_goto0(uint8_t i){ errcodes motor_goto0(uint8_t i){
errcodes e = motor_absmove(i, -the_conf.maxsteps[i]); errcodes e = motor_absmove(i, -the_conf.maxsteps[i]);
if(ERR_OK != e) return e; if(ERR_OK != e){
ESW_reaction[i] = ESW_STOPMINUS; if(!ESW_state(i)) return e; // not @ limit switch -> error
}else ESW_reaction[i] = ESW_STOPMINUS;
mvzerostate[i] = M0FAST; mvzerostate[i] = M0FAST;
return e; return e;
} }

16
F0:F030,F042,F072/3steppersLB/steppers.h
View File

@ -27,16 +27,18 @@
#define NSTALLEDMAX (5) #define NSTALLEDMAX (5)
// amount of steps to detect stalled state // amount of steps to detect stalled state
#define STALLEDSTEPS (15) #define STALLEDSTEPS (15)
// amount of tries to keep current position (need for states near problem places)
#define KEEPPOSMAX (10)
// stepper states // stepper states
typedef enum{ typedef enum{
STP_RELAX, // no moving STP_RELAX, // 0 - no moving
STP_ACCEL, // start moving with acceleration STP_ACCEL, // 1 - start moving with acceleration
STP_MOVE, // moving with constant speed STP_MOVE, // 2 - moving with constant speed
STP_MVSLOW, // moving with slowest constant speed (end of moving) STP_MVSLOW, // 3 - moving with slowest constant speed (end of moving)
STP_DECEL, // moving with deceleration STP_DECEL, // 4 - moving with deceleration
STP_STALL, // stalled STP_STALL, // 5 - stalled
STP_ERR // wrong/error state STP_ERR // 6 - wrong/error state
} stp_state; } stp_state;
// end-switches reaction // end-switches reaction

17
F0:F030,F042,F072/3steppersLB/strfunct.c
View File

@ -423,6 +423,14 @@ void dumperrcodes(_U_ char *txt){
} }
} }
static void eraseflash(_U_ char *txt){
SEND("Start at "); printu(Tms);
int e = erase_storage();
SEND("\nStop at "); printu(Tms); newline();
if(e) SEND("Error\n");
else SEND("OK\n");
}
typedef void(*specfpointer)(char *arg); typedef void(*specfpointer)(char *arg);
enum{ enum{
@ -443,7 +451,7 @@ enum{
SCMD_WD, SCMD_WD,
SCMD_DUMPERR, SCMD_DUMPERR,
SCMD_DUMPCMD, SCMD_DUMPCMD,
//SCMD_ST, SCMD_ERASEFLASH,
SCMD_AMOUNT SCMD_AMOUNT
}; };
@ -466,7 +474,7 @@ static specfpointer speccmdlist[SCMD_AMOUNT] = {
[SCMD_WD] = wdcheck, [SCMD_WD] = wdcheck,
[SCMD_DUMPCMD] = dumpcmdcodes, [SCMD_DUMPCMD] = dumpcmdcodes,
[SCMD_DUMPERR] = dumperrcodes, [SCMD_DUMPERR] = dumperrcodes,
//[SCMD_ST] = stp_check, [SCMD_ERASEFLASH] = eraseflash,
}; };
typedef struct{ typedef struct{
@ -526,6 +534,7 @@ static const commands textcommands[] = {
{-SCMD_DUMPERR, "dumperr", "dump error codes"}, {-SCMD_DUMPERR, "dumperr", "dump error codes"},
{-SCMD_DUMPCMD, "dumpcmd", "dump command codes"}, {-SCMD_DUMPCMD, "dumpcmd", "dump command codes"},
{-SCMD_DUMPCONF, "dumpconf", "dump current configuration"}, {-SCMD_DUMPCONF, "dumpconf", "dump current configuration"},
{-SCMD_ERASEFLASH, "eraseflash", "erase flash data storage"},
{-SCMD_FILTER, "filter", "add/modify filter, format: bank# FIFO# mode(M/I) num0 [num1 [num2 [num3]]]"}, {-SCMD_FILTER, "filter", "add/modify filter, format: bank# FIFO# mode(M/I) num0 [num1 [num2 [num3]]]"},
{-SCMD_GETCTR, "getctr", "get TIM1/2/3 counters"}, {-SCMD_GETCTR, "getctr", "get TIM1/2/3 counters"},
{-SCMD_IGNBUF, "ignbuf", "print ignore buffer"}, {-SCMD_IGNBUF, "ignbuf", "print ignore buffer"},
@ -542,7 +551,7 @@ static const commands textcommands[] = {
// dump base commands codes (for CAN protocol) // dump base commands codes (for CAN protocol)
void dumpcmdcodes(_U_ char *txt){ void dumpcmdcodes(_U_ char *txt){
SEND("CANbus commands list:\n"); SEND("CANbus commands list:\n");
for(uint16_t i = 0; i < CMD_AMOUNT; ++i){ for(uint16_t i = 1; i < CMD_AMOUNT; ++i){
printu(i); printu(i);
SEND(" - "); SEND(" - ");
const commands *c = textcommands; const commands *c = textcommands;
@ -656,7 +665,9 @@ void cmd_parser(char *txt){
par &= 0x7f; par &= 0x7f;
if(par != CANMESG_NOPAR) printu(par); if(par != CANMESG_NOPAR) printu(par);
bufputchar('='); printi(val); bufputchar('='); printi(val);
#ifdef EBUG
SEND(" ("); printuhex((uint32_t)val); bufputchar(')'); SEND(" ("); printuhex((uint32_t)val); bufputchar(')');
#endif
if(ERR_OK != retcode){ if(ERR_OK != retcode){
SEND("\nERRCODE="); SEND("\nERRCODE=");
printu(retcode); printu(retcode);

4
F0:F030,F042,F072/3steppersLB/version.inc
View File

@ -1,2 +1,2 @@
#define BUILD_NUMBER "156" #define BUILD_NUMBER "168"
#define BUILD_DATE "2022-06-10" #define BUILD_DATE "2022-06-17"