mirror of
https://github.com/eddyem/stm32samples.git
synced 2025-12-06 10:45:11 +03:00
3steppers: fix some bugs, add commands eraseflash and setpos
This commit is contained in:
parent
16f72f6801
commit
6f29257a28
@ -5,7 +5,7 @@ BOOTSPEED ?= 57600
|
||||
FAMILY ?= F0
|
||||
# MCU code
|
||||
MCU ?= F072xB
|
||||
DEFS += -DEBUG
|
||||
#DEFS += -DEBUG
|
||||
# change this linking script depending on particular MCU model,
|
||||
# for example, if you have STM32F103VBT6, you should write:
|
||||
LDSCRIPT ?= stm32f072B.ld
|
||||
|
||||
@ -142,10 +142,10 @@ Common commands format is cmd[ N[ = val]]
|
||||
where N is command argument (0..127), val is its value
|
||||
Different commands:
|
||||
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)
|
||||
esw - get end switches state
|
||||
ext - external outputs
|
||||
esw - get end switches state (without number - all, by bytes)
|
||||
ext - external outputs (without number - all, by bytes; value= 0-off, 1-on, other-toggle)
|
||||
mcut - get MCU T
|
||||
mcuvdd - get MCU Vdd
|
||||
ping - echo given command back
|
||||
@ -175,6 +175,7 @@ Motors' commands:
|
||||
motreinit - re-init motors after configuration changed
|
||||
relpos - set relative steps, get remaining
|
||||
relslow - set relative steps @ lowest speed
|
||||
setpos - set/get absolute position (in steps)
|
||||
state - get motor state
|
||||
stop - smooth motor stopping
|
||||
USB-only commands:
|
||||
@ -185,6 +186,7 @@ USB-only commands:
|
||||
dumperr - dump error codes
|
||||
dumpcmd - dump command codes
|
||||
dumpconf - dump current configuration
|
||||
eraseflash - erase flash data storage
|
||||
filter - add/modify filter, format: bank# FIFO# mode(M/I) num0 [num1 [num2 [num3]]]
|
||||
getctr - get TIM1/2/3 counters
|
||||
ignbuf - print ignore buffer
|
||||
@ -208,7 +210,6 @@ bytes descr
|
||||
7 Hdata
|
||||
|
||||
dumperr
|
||||
Find known command: dumperr
|
||||
Error codes:
|
||||
0 - all OK
|
||||
1 - wrong parameter's value
|
||||
@ -219,82 +220,82 @@ Error codes:
|
||||
|
||||
|
||||
dumpcmd
|
||||
Find known command: dumpcmd
|
||||
Commands list:
|
||||
0 - Different commands:
|
||||
1 - change relay state (1/0)
|
||||
2 - change buzzer state (1/0)
|
||||
3 - get ADC values
|
||||
4 - get buttons state
|
||||
5 - get end switches state
|
||||
6 - get MCU T
|
||||
7 - get MCU Vdd
|
||||
8 - reset MCU
|
||||
9 - get time from start
|
||||
10 - pwm value
|
||||
11 - external outputs
|
||||
12 - save current configuration
|
||||
13 - minimal encoder ticks per step
|
||||
14 - maximal encoder ticks per step
|
||||
15 - set/get microsteps settings
|
||||
16 - set/get accel/decel (steps/s^2)
|
||||
17 - set/get max speed (steps per sec)
|
||||
18 - set/get min speed (steps per sec)
|
||||
19 - get limiting speed for current microsteps
|
||||
20 - set/get max steps (from zero)
|
||||
21 - set/get max encoder's pulses per revolution
|
||||
22 - set/get motorN flags
|
||||
23 - end-switches reaction
|
||||
24 - re-init motors after configuration changed
|
||||
25 - set/get position (in steps)
|
||||
26 - set relative steps, get remaining
|
||||
27 - set relative steps @ lowest speed
|
||||
28 - emergency stop motor (right now)
|
||||
29 - smooth motor stopping
|
||||
30 - emergency stop all motors
|
||||
31 - find zero position & refresh counters
|
||||
32 - get motor state
|
||||
33 - set/get encoder's position
|
||||
1 - echo given command back
|
||||
2 - change relay state (1/0)
|
||||
3 - change buzzer state (1/0)
|
||||
4 - get ADC values
|
||||
5 - get buttons state
|
||||
6 - get end switches state
|
||||
7 - get MCU T
|
||||
8 - get MCU Vdd
|
||||
9 - reset MCU
|
||||
10 - get time from start
|
||||
11 - pwm value
|
||||
12 - external outputs
|
||||
13 - save current configuration
|
||||
14 - minimal encoder ticks per step
|
||||
15 - maximal encoder ticks per step
|
||||
16 - set/get microsteps settings
|
||||
17 - set/get accel/decel (steps/s^2)
|
||||
18 - set/get max speed (steps per sec)
|
||||
19 - set/get min speed (steps per sec)
|
||||
20 - get limiting speed for current microsteps
|
||||
21 - set/get max steps (from zero)
|
||||
22 - set/get max encoder's pulses per revolution
|
||||
23 - set/get motorN flags
|
||||
24 - end-switches reaction
|
||||
25 - re-init motors after configuration changed
|
||||
26 - move to/get absolute position (in steps)
|
||||
27 - set relative steps, get remaining
|
||||
28 - set relative steps @ lowest speed
|
||||
29 - emergency stop motor (right now)
|
||||
30 - smooth motor stopping
|
||||
31 - emergency stop all motors
|
||||
32 - find zero position & refresh counters
|
||||
33 - get motor state
|
||||
34 - set/get encoder's position
|
||||
35 - set/get absolute position (in steps)
|
||||
|
||||
|
||||
dumpconf
|
||||
Find known command: dumpconf
|
||||
flashsize=64*2048=131072
|
||||
userconf_addr=0x08006800
|
||||
userconf_idx=-1 // "index of stored conf"
|
||||
userconf_addr=0x08006000// address from which userconf started
|
||||
userconf_idx=5 // "index of stored conf"
|
||||
userconf_sz=68 // "magick number"
|
||||
canspeed=100 // default CAN speed
|
||||
canid=170 // identifier (0xaa)
|
||||
microsteps0=32 // microsteps amount per step
|
||||
accel0=500 // acceleration/deceleration (steps/s^2)
|
||||
maxspeed0=2000 // max motor speed (steps per second)
|
||||
accel0=1500 // acceleration/deceleration (steps/s^2)
|
||||
maxspeed0=1501 // max motor speed (steps per second)
|
||||
minspeed0=20 // min motor speed (steps per second)
|
||||
maxsteps0=500000 // maximal amount of steps
|
||||
encperrev0=4000 // encoders' counts per revolution
|
||||
encperstepmin0=17 // min 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)
|
||||
microsteps1=32
|
||||
accel1=500
|
||||
accel1=1500
|
||||
maxspeed1=2000
|
||||
minspeed1=20
|
||||
maxsteps1=500000
|
||||
encperrev1=4000
|
||||
encperstepmin1=17
|
||||
encperstepmax1=23
|
||||
motflags1=0x3c
|
||||
motflags1=0x2f
|
||||
eswreaction1=0
|
||||
microsteps2=32
|
||||
accel2=500
|
||||
maxspeed2=2000
|
||||
accel2=1500
|
||||
maxspeed2=2500
|
||||
minspeed2=20
|
||||
maxsteps2=500000
|
||||
encperrev2=4000
|
||||
encperstepmin2=17
|
||||
encperstepmax2=23
|
||||
motflags2=0x3c
|
||||
motflags2=0x2f
|
||||
eswreaction2=0
|
||||
|
||||
|
||||
Motor flags:
|
||||
bit0 - reversing motor rotation
|
||||
bit1 - reversing encoder rotation
|
||||
|
||||
@ -27,6 +27,11 @@
|
||||
#include "strfunct.h"
|
||||
#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 *******/
|
||||
|
||||
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){
|
||||
NOPARCHK();
|
||||
if(ISSETTER(par)){
|
||||
if(*val) ON(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){
|
||||
NOPARCHK();
|
||||
if(ISSETTER(par)){
|
||||
if(*val) ON(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){
|
||||
uint8_t n = PARBASE(par);
|
||||
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 (uint8_t) keystate(n, (uint32_t*)val);
|
||||
@ -75,39 +82,43 @@ static errcodes eswparser(uint8_t par, int32_t *val){
|
||||
#error "change the code!!!"
|
||||
#endif
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > ESWNO-1){ // all
|
||||
if(n == CANMESG_NOPAR){ // all
|
||||
*val = 0;
|
||||
uint8_t *arr = (uint8_t*)val;
|
||||
for(int i = 0; i < ESWNO; ++i)
|
||||
*arr++ = ESW_state(i);
|
||||
return ERR_OK;
|
||||
}
|
||||
}else if(n > ESWNO - 1) return ERR_BADPAR;
|
||||
*val = (int32_t)ESW_state(n);
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes mcutparser(uint8_t _U_ par, int32_t *val){
|
||||
NOPARCHK();
|
||||
*val = getMCUtemp();
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes mcuvddparser(uint8_t _U_ par, int32_t *val){
|
||||
NOPARCHK();
|
||||
*val = getVdd();
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes resetparser(uint8_t _U_ par, int32_t _U_ *val){
|
||||
NOPARCHK();
|
||||
NVIC_SystemReset();
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes timeparser(uint8_t _U_ par, int32_t *val){
|
||||
NOPARCHK();
|
||||
*val = Tms;
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes pwmparser(uint8_t par, int32_t *val){
|
||||
if(PARBASE(par) > PWMCHMAX && par != CANMESG_NOPAR) return ERR_BADPAR;
|
||||
NOPARCHK();
|
||||
#if PWMCHMAX != 0
|
||||
#error "change the code!!!"
|
||||
#endif
|
||||
@ -144,7 +155,7 @@ static errcodes extparser(uint8_t par, int32_t *val){
|
||||
SEND("par="); printu(par);
|
||||
SEND(", n="); bufputchar('0'+n); newline();
|
||||
#endif
|
||||
if(n > EXTNO-1){ // all
|
||||
if(n == CANMESG_NOPAR){ // all
|
||||
#ifdef EBUG
|
||||
SEND("ALL\n");
|
||||
#endif
|
||||
@ -157,7 +168,7 @@ static errcodes extparser(uint8_t par, int32_t *val){
|
||||
arr[i] = EXT_CHK(i);
|
||||
}
|
||||
return ERR_OK;
|
||||
}
|
||||
}else if(n > EXTNO-1) return ERR_BADPAR;
|
||||
if(ISSETTER(par))
|
||||
setextpar((uint8_t)*val, n);
|
||||
*val = (int32_t) EXT_CHK(n);
|
||||
@ -166,8 +177,7 @@ static errcodes extparser(uint8_t par, int32_t *val){
|
||||
|
||||
/******************* START of config parsers *******************/
|
||||
static errcodes ustepsparser(uint8_t par, int32_t *val){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
#if MICROSTEPSMAX > 512
|
||||
#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){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
if(*val < 1 || *val > MAXENCTICKSPERSTEP - 1) return ERR_BADVAL;
|
||||
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){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
if(*val < 1 || *val > MAXENCTICKSPERSTEP) return ERR_BADVAL;
|
||||
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){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
if(*val/the_conf.microsteps[n] > ACCELMAXSTEPS || *val < 1) return ERR_BADVAL;
|
||||
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){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
if(*val <= the_conf.minspd[n]) return ERR_BADVAL;
|
||||
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){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
if(*val >= the_conf.maxspd[n]) return ERR_BADVAL;
|
||||
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){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
*val = getSPD(n, 0xffff);
|
||||
return ERR_OK;
|
||||
}
|
||||
|
||||
static errcodes maxstepsparser(uint8_t par, int32_t *val){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
if(*val < 1) return ERR_BADVAL;
|
||||
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){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
if(*val < 1 || *val > MAXENCREV) return ERR_BADVAL;
|
||||
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){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
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!
|
||||
static errcodes eswreactparser(uint8_t par, int32_t *val){
|
||||
uint8_t n = PARBASE(par);
|
||||
if(n > MOTORSNO-1) return ERR_BADPAR;
|
||||
uint8_t n; CHECKN(n, par);
|
||||
if(ISSETTER(par)){
|
||||
if(*val < 0 || *val > ESW_AMOUNT-1) return ERR_BADVAL;
|
||||
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){
|
||||
NOPARCHK();
|
||||
if(store_userconf()) return ERR_CANTRUN;
|
||||
return ERR_OK;
|
||||
}
|
||||
@ -310,13 +311,11 @@ static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){
|
||||
|
||||
/******************* START of motors' parsers *******************/
|
||||
static errcodes reinitmparser(uint8_t _U_ par, int32_t _U_ *val){
|
||||
NOPARCHK();
|
||||
init_steppers();
|
||||
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){
|
||||
uint8_t n; CHECKN(n, par);
|
||||
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){
|
||||
NOPARCHK();
|
||||
for(int i = 0; i < MOTORSNO; ++i)
|
||||
emstopmotor(i);
|
||||
return ERR_OK;
|
||||
@ -384,7 +384,6 @@ static errcodes gotozeroparser(uint8_t par, _U_ int32_t *val){
|
||||
return motor_goto0(n);
|
||||
}
|
||||
|
||||
#undef CHECKN
|
||||
/******************* END of motors' parsers *******************/
|
||||
|
||||
/*
|
||||
|
||||
@ -112,7 +112,7 @@ int store_userconf(){
|
||||
// for binarySearch() checking that there's nothing more after it!
|
||||
if(currentconfidx > (int)maxCnum - 3){ // there's no more place
|
||||
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)
|
||||
return write2flash((const void*)&Flash_Data[currentconfidx], &the_conf, sizeof(the_conf));
|
||||
}
|
||||
@ -241,3 +241,7 @@ void dump_userconf(_U_ char *txt){
|
||||
}
|
||||
NL();
|
||||
}
|
||||
|
||||
int erase_storage(){
|
||||
return erase_flash(Flash_Data, NULL);
|
||||
}
|
||||
|
||||
@ -79,5 +79,6 @@ extern user_conf the_conf; // global user config (read from FLASH to RAM)
|
||||
void flashstorage_init();
|
||||
int store_userconf();
|
||||
void dump_userconf(_U_ char *txt);
|
||||
int erase_storage();
|
||||
|
||||
#endif // __FLASH_H__
|
||||
|
||||
Binary file not shown.
@ -34,6 +34,10 @@ typedef enum{
|
||||
STALL_STOP // Nstalled >= limit
|
||||
} t_stalled;
|
||||
|
||||
#ifdef EBUG
|
||||
static uint8_t stp[MOTORSNO] = {0};
|
||||
#endif
|
||||
|
||||
static t_stalled stallflags[MOTORSNO];
|
||||
|
||||
// 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
|
||||
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];
|
||||
if(delta > 0){ // positive direction
|
||||
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);
|
||||
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];
|
||||
Taccel[i] = Tms;
|
||||
recalcARR(i);
|
||||
@ -215,20 +231,21 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){
|
||||
case STP_RELAX:
|
||||
break;
|
||||
case STP_STALL:
|
||||
DBG("Move from STALL");
|
||||
if(dir == stalleddir[i]){
|
||||
DBG("Move to stalled direction!");
|
||||
return ERR_CANTRUN; // can't run into stalled direction
|
||||
}
|
||||
break;
|
||||
default: // moving state
|
||||
DBG("Always moving");
|
||||
DBG("Is moving");
|
||||
return ERR_CANTRUN;
|
||||
}
|
||||
if(newpos > (int32_t)the_conf.maxsteps[i] || newpos < -(int32_t)the_conf.maxsteps[i] || newpos == stppos[i]){
|
||||
DBG("Too much steps");
|
||||
return ERR_BADVAL; // too big position or zero
|
||||
}
|
||||
motdir[i] = dir;
|
||||
motdir[i] = dir; // should be before limit switch check
|
||||
if(esw_block(i)){
|
||||
DBG("Block by ESW");
|
||||
return ERR_CANTRUN; // on end-switch
|
||||
@ -239,6 +256,7 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){
|
||||
prevencpos[i] = encoder_position(i);
|
||||
prevstppos[i] = stppos[i];
|
||||
curspeed[i] = the_conf.minspd[i];
|
||||
state[i] = STP_ACCEL;
|
||||
calcacceleration(i);
|
||||
#ifdef EBUG
|
||||
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 e = motor_absmove(i, stppos[i] + relsteps);
|
||||
if(ERR_OK == e){
|
||||
DBG("-> MVSLOW");
|
||||
state[i] = STP_MVSLOW;
|
||||
}
|
||||
return e;
|
||||
@ -269,6 +288,7 @@ void emstopmotor(uint8_t i){
|
||||
switch(state[i]){
|
||||
case STP_ERR: // clear error state
|
||||
case STP_STALL:
|
||||
DBG("-> RELAX");
|
||||
state[i] = STP_RELAX;
|
||||
// fallthrough
|
||||
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
|
||||
mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer
|
||||
if(stopflag[i]) targstppos[i] = stppos[i]; // keep position (for keep flag)
|
||||
stopflag[i] = 0;
|
||||
mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer
|
||||
if(the_conf.motflags[i].donthold)
|
||||
MOTOR_DIS(i); // turn off power
|
||||
if(stallflags[i] == STALL_STOP){
|
||||
stallflags[i] = STALL_NO;
|
||||
state[i] = STP_STALL;
|
||||
}else
|
||||
}else{
|
||||
state[i] = STP_RELAX;
|
||||
}
|
||||
#ifdef EBUG
|
||||
SEND("MOTOR"); bufputchar('0'+i); SEND(" stop @"); printi(stppos[i]); newline();
|
||||
stp[i] = 1;
|
||||
#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(", speed="); printu(curspeed[i]);
|
||||
#endif
|
||||
if(++Nstalled[i] > NSTALLEDMAX){
|
||||
stopflag[i] = 1;
|
||||
if(++Nstalled[i] >= NSTALLEDMAX){
|
||||
Nstalled[i] = 0;
|
||||
#ifdef EBUG
|
||||
SEND(" --- STALL!");
|
||||
#else
|
||||
SEND("ERRCODE="); bufputchar(ERR_CANTRUN+'0');
|
||||
SEND("\nstate"); bufputchar(i+'0'); bufputchar('=');
|
||||
bufputchar(STP_STALL);
|
||||
bufputchar(STP_STALL + '0');
|
||||
#endif
|
||||
NL();
|
||||
stallflags[i] = STALL_STOP;
|
||||
@ -408,7 +428,19 @@ static t_stalled chkSTALL(uint8_t i){
|
||||
// check state of i`th stepper
|
||||
static void chkstepper(int i){
|
||||
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
|
||||
#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]){
|
||||
case STP_RELAX: // check if need to keep current position
|
||||
if(the_conf.motflags[i].haveencoder){
|
||||
@ -425,18 +457,30 @@ static void chkstepper(int i){
|
||||
if(the_conf.motflags[i].keeppos){ // keep old position
|
||||
diff = targstppos[i] - i32; // check whether we need to change 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
|
||||
SEND("MOTOR"); bufputchar('0'+i);
|
||||
SEND(" curpos="); printi(i32); SEND(", need="); printi(targstppos[i]); NL();
|
||||
#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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STP_ACCEL: // acceleration to max speed
|
||||
if(STALL_NO == chkSTALL(i)){
|
||||
if(s == STALL_NO){
|
||||
//newspeed = curspeed[i] + dV[i];
|
||||
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
|
||||
@ -450,20 +494,22 @@ static void chkstepper(int i){
|
||||
curspeed[i] = i32;
|
||||
}
|
||||
recalcARR(i);
|
||||
}
|
||||
// check position for triangle profile
|
||||
if(motdir[i] > 0){
|
||||
if(stppos[i] >= decelstartpos[i]){ // reached end of acceleration
|
||||
TODECEL();
|
||||
}
|
||||
}else{
|
||||
if(stppos[i] <= decelstartpos[i]){
|
||||
TODECEL();
|
||||
// check position for triangle profile
|
||||
if(motdir[i] > 0){
|
||||
if(stppos[i] >= decelstartpos[i]){ // reached end of acceleration
|
||||
TODECEL();
|
||||
}
|
||||
}else{
|
||||
if(stppos[i] <= decelstartpos[i]){
|
||||
TODECEL();
|
||||
}
|
||||
}
|
||||
}else if(s == STALL_STOP){
|
||||
stopflag[i] = 1;
|
||||
}
|
||||
break;
|
||||
case STP_MOVE: // move @ constant speed until need to decelerate
|
||||
if(STALL_NO == chkSTALL(i)){
|
||||
if(s == STALL_NO){
|
||||
// check position
|
||||
if(motdir[i] > 0){
|
||||
if(stppos[i] >= decelstartpos[i]){ // reached start of deceleration
|
||||
@ -474,10 +520,12 @@ static void chkstepper(int i){
|
||||
TODECEL();
|
||||
}
|
||||
}
|
||||
}else if(s == STALL_STOP){
|
||||
stopflag[i] = 1;
|
||||
}
|
||||
break;
|
||||
case STP_DECEL:
|
||||
if(STALL_NO == chkSTALL(i)){
|
||||
if(s == STALL_NO){
|
||||
//newspeed = curspeed[i] - dV[i];
|
||||
i32 = startspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
|
||||
if(i32 > the_conf.minspd[i]){
|
||||
@ -492,10 +540,12 @@ static void chkstepper(int i){
|
||||
}
|
||||
recalcARR(i);
|
||||
//SEND("spd="); printu(curspeed[i]); SEND(", pos="); printi(stppos[i]); newline();
|
||||
}else if(s == STALL_STOP){
|
||||
stopflag[i] = 1;
|
||||
}
|
||||
break;
|
||||
case STP_MVSLOW:
|
||||
chkSTALL(i);
|
||||
if(s == STALL_STOP) stopflag[i] = 1;
|
||||
break;
|
||||
default: // STALL, ERR -> do nothing, check mvzerostate
|
||||
break;
|
||||
@ -504,12 +554,13 @@ static void chkstepper(int i){
|
||||
case M0FAST:
|
||||
if(state[i] == STP_RELAX || state[i] == STP_STALL){ // stopped -> move to +
|
||||
#ifdef EBUG
|
||||
SEND("M0FAST: motor stopped\n");
|
||||
bufputchar('M'); bufputchar('0'+i); SEND("FAST: motor stopped\n");
|
||||
#endif
|
||||
if(ERR_OK != motor_relslow(i, 1000)){
|
||||
#ifdef EBUG
|
||||
SEND("Can't move\n");
|
||||
#endif
|
||||
DBG("->ERR");
|
||||
state[i] = STP_ERR;
|
||||
mvzerostate[i] = M0RELAX;
|
||||
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
|
||||
#ifdef EBUG
|
||||
SEND("M0SLOW: set position to 0\n"); NL();
|
||||
bufputchar('M'); bufputchar('0'+i); SEND("SLOW: motor stopped\n");
|
||||
#endif
|
||||
ESW_reaction[i] = the_conf.ESW_reaction[i];
|
||||
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
|
||||
mvzerostate[i] = M0RELAX;
|
||||
}
|
||||
@ -541,8 +592,9 @@ static void chkstepper(int i){
|
||||
|
||||
errcodes motor_goto0(uint8_t i){
|
||||
errcodes e = motor_absmove(i, -the_conf.maxsteps[i]);
|
||||
if(ERR_OK != e) return e;
|
||||
ESW_reaction[i] = ESW_STOPMINUS;
|
||||
if(ERR_OK != e){
|
||||
if(!ESW_state(i)) return e; // not @ limit switch -> error
|
||||
}else ESW_reaction[i] = ESW_STOPMINUS;
|
||||
mvzerostate[i] = M0FAST;
|
||||
return e;
|
||||
}
|
||||
|
||||
@ -27,16 +27,18 @@
|
||||
#define NSTALLEDMAX (5)
|
||||
// amount of steps to detect stalled state
|
||||
#define STALLEDSTEPS (15)
|
||||
// amount of tries to keep current position (need for states near problem places)
|
||||
#define KEEPPOSMAX (10)
|
||||
|
||||
// stepper states
|
||||
typedef enum{
|
||||
STP_RELAX, // no moving
|
||||
STP_ACCEL, // start moving with acceleration
|
||||
STP_MOVE, // moving with constant speed
|
||||
STP_MVSLOW, // moving with slowest constant speed (end of moving)
|
||||
STP_DECEL, // moving with deceleration
|
||||
STP_STALL, // stalled
|
||||
STP_ERR // wrong/error state
|
||||
STP_RELAX, // 0 - no moving
|
||||
STP_ACCEL, // 1 - start moving with acceleration
|
||||
STP_MOVE, // 2 - moving with constant speed
|
||||
STP_MVSLOW, // 3 - moving with slowest constant speed (end of moving)
|
||||
STP_DECEL, // 4 - moving with deceleration
|
||||
STP_STALL, // 5 - stalled
|
||||
STP_ERR // 6 - wrong/error state
|
||||
} stp_state;
|
||||
|
||||
// end-switches reaction
|
||||
|
||||
@ -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);
|
||||
|
||||
enum{
|
||||
@ -443,7 +451,7 @@ enum{
|
||||
SCMD_WD,
|
||||
SCMD_DUMPERR,
|
||||
SCMD_DUMPCMD,
|
||||
//SCMD_ST,
|
||||
SCMD_ERASEFLASH,
|
||||
SCMD_AMOUNT
|
||||
};
|
||||
|
||||
@ -466,7 +474,7 @@ static specfpointer speccmdlist[SCMD_AMOUNT] = {
|
||||
[SCMD_WD] = wdcheck,
|
||||
[SCMD_DUMPCMD] = dumpcmdcodes,
|
||||
[SCMD_DUMPERR] = dumperrcodes,
|
||||
//[SCMD_ST] = stp_check,
|
||||
[SCMD_ERASEFLASH] = eraseflash,
|
||||
};
|
||||
|
||||
typedef struct{
|
||||
@ -526,6 +534,7 @@ static const commands textcommands[] = {
|
||||
{-SCMD_DUMPERR, "dumperr", "dump error codes"},
|
||||
{-SCMD_DUMPCMD, "dumpcmd", "dump command codes"},
|
||||
{-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_GETCTR, "getctr", "get TIM1/2/3 counters"},
|
||||
{-SCMD_IGNBUF, "ignbuf", "print ignore buffer"},
|
||||
@ -542,7 +551,7 @@ static const commands textcommands[] = {
|
||||
// dump base commands codes (for CAN protocol)
|
||||
void dumpcmdcodes(_U_ char *txt){
|
||||
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);
|
||||
SEND(" - ");
|
||||
const commands *c = textcommands;
|
||||
@ -656,7 +665,9 @@ void cmd_parser(char *txt){
|
||||
par &= 0x7f;
|
||||
if(par != CANMESG_NOPAR) printu(par);
|
||||
bufputchar('='); printi(val);
|
||||
#ifdef EBUG
|
||||
SEND(" ("); printuhex((uint32_t)val); bufputchar(')');
|
||||
#endif
|
||||
if(ERR_OK != retcode){
|
||||
SEND("\nERRCODE=");
|
||||
printu(retcode);
|
||||
|
||||
@ -1,2 +1,2 @@
|
||||
#define BUILD_NUMBER "156"
|
||||
#define BUILD_DATE "2022-06-10"
|
||||
#define BUILD_NUMBER "168"
|
||||
#define BUILD_DATE "2022-06-17"
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user