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

add servo-emulation @ stepper motors

Browse Source
This commit is contained in:
Edward Emelianov 2021-11-17 00:08:02 +03:00
parent 4b50e95838
commit d0ecad3877
8 changed files with 119 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,7 +53,7 @@ static errcodes buzzerparser(uint8_t par, int32_t *val){
static errcodes adcparser(uint8_t par, int32_t *val){
uint8_t n = PARBASE(par);
if(n > NUMBER_OF_ADC_CHANNELS) return ERR_BADPAR;
if(n > NUMBER_OF_ADC_CHANNELS-1) return ERR_BADPAR;
*val = (int32_t) getADCval(n);
return ERR_OK;
}

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

@ -34,7 +34,7 @@ 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, .donthold = 1, .eswinv = 1}
#define DEFMF {.haveencoder = 1, .donthold = 1, .eswinv = 1, .keeppos = 1}
#define USERCONF_INITIALIZER { \
.userconf_sz = sizeof(user_conf) \
@ -42,12 +42,12 @@ static uint32_t maxCnum = 1024 / sizeof(user_conf); // can't use blocksize here
,.CANID = 0xaa \
,.microsteps = {32, 32, 32} \
,.accel = {500, 500, 500} \
,.maxspd = {3000, 3000, 3000} \
,.maxspd = {2000, 2000, 2000} \
,.minspd = {20, 20, 20} \
,.maxsteps = {500000, 500000, 500000} \
,.encrev = {4000,4000,4000} \
,.encperstepmin = {5,5,5} \
,.encperstepmax = {50,50,50} \
,.encperstepmin = {17,17,17} \
,.encperstepmax = {23,23,23} \
,.motflags = {DEFMF,DEFMF,DEFMF} \
,.ESW_reaction = {ESW_ANYSTOP, ESW_ANYSTOP, ESW_ANYSTOP} \
}

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

@ -51,6 +51,7 @@ typedef struct{
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
uint8_t keeppos : 1; // bit5 - keep current position (as servo motor)
} motflags_t;
/*

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

@ -94,13 +94,11 @@ static void setup_mpwm(int i){
#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]);
}
@ -124,7 +122,6 @@ static void setup_enc(int i){
TIM->ARR = the_conf.encrev[i];
// enable timer
TIM->CR1 = TIM_CR1_CKD_1 | TIM_CR1_CEN; /* (4) */
TIM->CNT = 0;
NVIC_EnableIRQ(encirqs[i]);
}
@ -144,9 +141,9 @@ void timers_setup(){
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);
//}
}
}
}

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

Binary file not shown.

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

@ -42,19 +42,19 @@ static volatile int32_t stppos[MOTORSNO] = {0};
// previous position when check (set to current in start of moving)
static int32_t prevstppos[MOTORSNO];
// target stepper position
static int32_t targstppos[MOTORSNO];
static int32_t targstppos[MOTORSNO] = {0};
// position to start deceleration
static int32_t decelstartpos[MOTORSNO];
// current encoder position (4 per ticks) (without TIM->CNT)
static volatile int32_t encpos[MOTORSNO] = {0};
// previous encoder position
static int32_t prevencpos[MOTORSNO];
static int32_t prevencpos[MOTORSNO] = {0};
// encoders' ticks per step (calculates @ init)
static int32_t encperstep[MOTORSNO];
// current speed
static uint16_t curspeed[MOTORSNO];
static uint16_t decstartspeed[MOTORSNO]; // speed when deceleration starts
static uint16_t startspeed[MOTORSNO]; // speed when deceleration starts
// ==1 to stop @ nearest step
static uint8_t stopflag[MOTORSNO];
// motor state
@ -81,7 +81,6 @@ TRUE_INLINE void recalcARR(int i){
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
@ -115,17 +114,22 @@ int setencpos(uint8_t i, int32_t position){
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();
prevencpos[i] = encpos[i] = position - remain;
SEND("MOTOR"); bufputchar('0'+i); 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){
*position = encoder_position(i) / encperstep[i];
}else */
*position = stppos[i];
if(the_conf.motflags[i].haveencoder){
int32_t p = encoder_position(i);
if(p < 0) p -= encperstep[i]>>1;
else p += encperstep[i]>>1;
p /= encperstep[i];
*position = p;
}else
*position = stppos[i];
return ERR_OK;
}
@ -134,6 +138,35 @@ errcodes getremainsteps(uint8_t i, int32_t *position){
return ERR_OK;
}
// calculate acceleration/deceleration parameters for motor i
static void calcacceleration(uint8_t i){
int32_t delta = targstppos[i] - stppos[i];
if(delta > 0){ // positive direction
if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid
decelstartpos[i] = targstppos[i] - accdecsteps[i];
}else{ // triangle speed profile
decelstartpos[i] = stppos[i] + delta/2;
}
motdir[i] = 1;
if(the_conf.motflags[i].reverse) MOTOR_CCW(i);
else MOTOR_CW(i);
}else{ // negative direction
delta = -delta;
if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid
decelstartpos[i] = targstppos[i] + accdecsteps[i];
}else{ // triangle speed profile
decelstartpos[i] = stppos[i] - delta/2;
}
motdir[i] = -1;
if(the_conf.motflags[i].reverse) MOTOR_CW(i);
else MOTOR_CCW(i);
}
state[i] = STP_ACCEL;
startspeed[i] = curspeed[i];
Taccel[i] = Tms;
recalcARR(i);
}
// move to absolute position
errcodes motor_absmove(uint8_t i, int32_t newpos){
//if(i >= MOTORSNO) return ERR_BADPAR; // bad motor number
@ -144,38 +177,14 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){
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];
}else{ // triangle speed profile
decelstartpos[i] = stppos[i] + delta/2;
}
motdir[i] = 1;
if(inv) MOTOR_CCW(i);
else MOTOR_CW(i);
}else{ // negative direction
delta = -delta;
if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid
decelstartpos[i] = targstppos[i] + accdecsteps[i];
}else{ // triangle speed profile
decelstartpos[i] = stppos[i] - delta/2;
}
motdir[i] = -1;
if(inv) MOTOR_CW(i);
else MOTOR_CCW(i);
}
SEND("\ntargstppos="); printi(targstppos[i]);
SEND("\ndecelstart="); printi(decelstartpos[i]);
SEND("\naccdecsteps="); printu(accdecsteps[i]); NL();
curspeed[i] = the_conf.minspd[i];
recalcARR(i);
calcacceleration(i);
SEND("MOTOR"); bufputchar('0'+i);
SEND(" targstppos="); printi(targstppos[i]);
SEND(", decelstart="); printi(decelstartpos[i]);
SEND(", accdecsteps="); printu(accdecsteps[i]); NL();
MOTOR_EN(i);
mottimers[i]->CR1 |= TIM_CR1_CEN; // start timer
state[i] = STP_ACCEL;
Taccel[i] = Tms;
return ERR_OK;
}
@ -210,12 +219,24 @@ void addmicrostep(uint8_t i){
if(++microsteps[i] == the_conf.microsteps[i]){
microsteps[i] = 0;
stppos[i] += motdir[i];
if(stopflag[i] || stppos[i] == targstppos[i]){ // stop NOW
uint8_t stop_at_pos = 0;
if(motdir[i] > 0){
if(stppos[i] >= targstppos[i]){ // reached start of deceleration
stop_at_pos = 1;
}
}else{
if(stppos[i] <= targstppos[i]){
stop_at_pos = 1;
}
}
if(stopflag[i] || stop_at_pos){ // stop NOW
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
state[i] = STP_RELAX;
SEND("MOTOR"); bufputchar('0'+i); SEND(" stop @"); printi(stppos[i]); newline();
}
}
}
@ -237,45 +258,76 @@ static t_stalled chkSTALL(uint8_t i){
if(!the_conf.motflags[i].haveencoder) return STALL_NO;
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;
int difsign = 1;
if(Dstp < 0){
Dstp = -Dstp;
difsign = -difsign;
}
if(Dstp < 10){ // didn't move even @ 10 steps
return STALL_NO;
}
if(Denc < 0){
Denc = -Denc;
difsign = -difsign;
}
if(difsign == -1){ // motor and encoder moves to different sides!!!
Denc = -Denc; // init STALL state
}
prevencpos[i] = curencpos;
curstppos = curencpos / encperstep[i]; // recalculate current position
getpos(i, &curstppos); // 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){
SEND("MOTOR"); bufputchar('0'+i); SEND(" Denc="); printi(Denc); SEND(", Dstp="); printu(Dstp);
SEND(", speed="); printu(curspeed[i]);
if(++Nstalled > NSTALLEDMAX){
stopflag[i] = 1;
Nstalled = 0;
DBG("STALL!");
SEND(" --- STALL!"); NL();
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();
// now recalculate acc/dec parameters
calcacceleration(i);
SEND(" --- pre-stall, newspeed="); printu(curspeed[i]); NL();
return STALL_ONCE;
}
}
prevstppos[i] = curstppos;
Nstalled = 0;
return STALL_NO;
}
#define TODECEL() do{state[i] = STP_DECEL; \
decstartspeed[i] = curspeed[i]; \
startspeed[i] = curspeed[i]; \
Taccel[i] = Tms; \
SEND(" -> DECEL@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL();}while(0)
SEND("MOTOR"); bufputchar('0'+i); \
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_RELAX: // check if need to keep current position
if(the_conf.motflags[i].haveencoder && the_conf.motflags[i].keeppos){
getpos(i, &newspeed);
int32_t diff = stppos[i] - newspeed; // correct `curpos` counter by encoder
if(diff){
SEND("MOTOR"); bufputchar('0'+i);
SEND(" diff="); printi(diff);
SEND(", change stppos from "); printi(stppos[i]); SEND(" to "); printi(newspeed); NL();
stppos[i] = newspeed;
}
diff = targstppos[i] - newspeed; // check whether we need to change position
if(diff){ // try to correct position
SEND("MOTOR"); bufputchar('0'+i);
SEND(" curpos="); printi(newspeed); SEND(", need="); printi(targstppos[i]); NL();
motor_absmove(i, targstppos[i]);
}
}
break;
case STP_ACCEL: // acceleration to max speed
if(STALL_NO == chkSTALL(i)){
//newspeed = curspeed[i] + dV[i];
@ -283,6 +335,7 @@ static void chkstepper(int i){
if(newspeed >= the_conf.maxspd[i]){ // max speed reached -> move with it
curspeed[i] = the_conf.maxspd[i];
state[i] = STP_MOVE;
SEND("MOTOR"); bufputchar('0'+i);
SEND(" -> MOVE@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL();
}else{ // increase speed
curspeed[i] = newspeed;
@ -317,33 +370,20 @@ static void chkstepper(int i){
case STP_DECEL:
if(STALL_NO == chkSTALL(i)){
//newspeed = curspeed[i] - dV[i];
newspeed = decstartspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
newspeed = startspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
if(newspeed > the_conf.minspd[i]){
curspeed[i] = newspeed;
}else{
curspeed[i] = the_conf.minspd[i];
state[i] = STP_MVSLOW;
SEND("MOTOR"); bufputchar('0'+i);
SEND(" -> MVSLOW@"); printi(stppos[i]); NL();
}
recalcARR(i);
//SEND("spd="); printu(curspeed[i]); SEND(", pos="); printi(stppos[i]); newline();
}
// fallthrough
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;
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
default: // STP_MVSLOW, STALL, ERR -> do nothing
return;
}
switch(mvzerostate[i]){

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

@ -24,7 +24,7 @@
#include "commonproto.h"
// amount of tries to detect motor stall
#define NSTALLEDMAX (55)
#define NSTALLEDMAX (5)
// stepper states
typedef enum{

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

@ -1,2 +1,2 @@
#define BUILD_NUMBER "97"
#define BUILD_DATE "2021-11-16"
#define BUILD_NUMBER "114"
#define BUILD_DATE "2021-11-17"