diff --git a/F3:F303/Multistepper/commonproto.c b/F3:F303/Multistepper/commonproto.c index 7674ab8..5e1e756 100644 --- a/F3:F303/Multistepper/commonproto.c +++ b/F3:F303/Multistepper/commonproto.c @@ -359,9 +359,14 @@ errcodes cu_motmul(uint8_t _U_ par, int32_t _U_ *val){ return ERR_BADCMD; } +// witout parameter - reinit all steppers; with parameter - just update current errcodes cu_motreinit(uint8_t _U_ par, int32_t _U_ *val){ - NOPARCHK(par); - init_steppers(); + uint8_t n = PARBASE(par); + if(n == CANMESG_NOPAR) init_steppers(); + else{ + if(n > MOTORSNO - 1) return ERR_BADPAR; + if(!update_stepper(n)) return ERR_CANTRUN; + } return ERR_OK; } diff --git a/F3:F303/Multistepper/esprif.conf b/F3:F303/Multistepper/esprif.conf new file mode 100644 index 0000000..661e342 --- /dev/null +++ b/F3:F303/Multistepper/esprif.conf @@ -0,0 +1,66 @@ +canspeed=100 +canid=1 +microsteps0=32 +accel0=3000 +maxspeed0=2000 +minspeed0=20 +maxsteps0=500000 +motcurrent0=31 +motflags0=0x69 +eswreact0=1 +microsteps1=32 +accel1=3000 +maxspeed1=2000 +minspeed1=20 +maxsteps1=500000 +motcurrent1=31 +motflags1=0x69 +eswreact1=1 +microsteps2=32 +accel2=2000 +maxspeed2=1000 +minspeed2=20 +maxsteps2=500000 +motcurrent2=31 +motflags2=0x69 +eswreact2=1 +microsteps3=32 +accel3=2000 +maxspeed3=1000 +minspeed3=20 +maxsteps3=500000 +motcurrent3=31 +motflags3=0x78 +eswreact3=0 +microsteps4=32 +accel4=2000 +maxspeed4=2000 +minspeed4=20 +maxsteps4=500000 +motcurrent4=31 +motflags4=0x68 +eswreact4=0 +microsteps5=32 +accel5=500 +maxspeed5=2000 +minspeed5=20 +maxsteps5=500000 +motcurrent5=31 +motflags5=0x68 +eswreact5=0 +microsteps6=32 +accel6=500 +maxspeed6=2000 +minspeed6=20 +maxsteps6=500000 +motcurrent6=31 +motflags6=0x68 +eswreact6=0 +microsteps7=32 +accel7=500 +maxspeed7=2000 +minspeed7=20 +maxsteps7=500000 +motcurrent7=31 +motflags7=0x68 +eswreact7=0 diff --git a/F3:F303/Multistepper/hardware.c b/F3:F303/Multistepper/hardware.c index 7244b1c..3ea5645 100644 --- a/F3:F303/Multistepper/hardware.c +++ b/F3:F303/Multistepper/hardware.c @@ -103,8 +103,8 @@ TRUE_INLINE void gpio_setup(){ RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOCEN | RCC_AHBENR_GPIODEN | RCC_AHBENR_GPIOEEN | RCC_AHBENR_GPIOFEN; // enable timers: 1,2,3,4,8,15,16,17 - RCC->APB1ENR |= RCC_APB1ENR_TIM2EN | RCC_APB1ENR_TIM3EN | RCC_APB1ENR_TIM4EN; - RCC->APB2ENR |= RCC_APB2ENR_TIM1EN | RCC_APB2ENR_TIM8EN | RCC_APB2ENR_TIM15EN | RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN; + RCC->APB1ENR |= RCC_APB1ENR_TIM2EN | RCC_APB1ENR_TIM3EN | RCC_APB1ENR_TIM4EN; // 36MHz + RCC->APB2ENR |= RCC_APB2ENR_TIM1EN | RCC_APB2ENR_TIM8EN | RCC_APB2ENR_TIM15EN | RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN; // 72MHz for(int i = 0; i < 10000; ++i) nop(); GPIOA->ODR = 0; GPIOA->AFR[0] = AFRf(5, 5) | AFRf(5, 6) | AFRf(5, 7); @@ -194,28 +194,33 @@ TRUE_INLINE void iwdg_setup(){ static void setup_mpwm(int i){ volatile TIM_TypeDef *TIM = mottimers[i]; TIM->CR1 = TIM_CR1_ARPE; // buffered ARR - TIM->PSC = MOTORTIM_PSC; // 26MHz + TIM->PSC = MOTORTIM_PSC; // 24MHz // PWM mode 1 (active -> inactive) uint8_t n = mottchannels[i]; + volatile uint32_t *CCRx = &TIM->CCR1; switch(n){ case 1: TIM->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1; + //CCRx = &TIM->CCR1; break; case 2: TIM->CCMR1 = TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1; + //CCRx = &TIM->CCR2; break; case 3: TIM->CCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1; + //CCRx = &TIM->CCR3; break; default: TIM->CCMR2 = TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1; + //CCRx = &TIM->CCR4; } #if MOTORTIM_ARRMIN < 5 #error "change the code!" #endif - TIM->CCR1 = MOTORTIM_ARRMIN - 3; // ~10us for pulse duration - TIM->ARR = 0xffff; -// TIM->EGR = TIM_EGR_UG; // generate update to refresh ARR + CCRx[n-1] = MOTORTIM_ARRMIN - 3; // ~10us for pulse duration + TIM->ARR = MOTORTIM_ARRMIN * 2; + TIM->EGR = TIM_EGR_UG; // generate update to refresh ARR TIM->BDTR |= TIM_BDTR_MOE; // enable main output TIM->CCER = 1<<((n-1)*4); // turn it on, active high TIM->DIER = 1< - + EnvironmentId diff --git a/F3:F303/Multistepper/pdnuart.c b/F3:F303/Multistepper/pdnuart.c index 105a7fb..fbb50fe 100644 --- a/F3:F303/Multistepper/pdnuart.c +++ b/F3:F303/Multistepper/pdnuart.c @@ -184,25 +184,47 @@ int pdnuart_setcurrent(uint8_t no, uint8_t val){ return writeregister(no, TMC2209Reg_IHOLD_IRUN, regval.value); } -// set microsteps over UART +// set microsteps over UART && clear error flags int pdnuart_microsteps(uint8_t no, uint32_t val){ if(val > 256) return FALSE; TMC2209_chopconf_reg_t regval; if(!readregister(no, TMC2209Reg_CHOPCONF, ®val.value)) return FALSE; if(val == 256) regval.mres = 0; else regval.mres = 8 - MSB(val); - return writeregister(no, TMC2209Reg_CHOPCONF, regval.value); + uint32_t toff = regval.toff; + if(toff == 0) toff = 1; + regval.toff = 0; // disable drive to clear errors + int e = writeregister(no, TMC2209Reg_CHOPCONF, regval.value); + TMC2209_gstat_reg_t stat = {.value = 0}; + stat.drv_err = 1; + // clear error flags + if(!writeregister(no, TMC2209Reg_GSTAT, stat.value)) e = FALSE; + regval.toff = toff; // now enable drive + if(!writeregister(no, TMC2209Reg_CHOPCONF, regval.value)) e = FALSE; + return e; } // init driver number `no`, return FALSE if failed int pdnuart_init(uint8_t no){ TMC2209_gconf_reg_t gconf; - if(!pdnuart_microsteps(no, the_conf.microsteps[no])) return FALSE; - if(!pdnuart_setcurrent(no, the_conf.motcurrent[no])) return FALSE; - if(!readregister(no, TMC2209Reg_GCONF, &gconf.value)) return FALSE; + if(!pdnuart_microsteps(no, the_conf.microsteps[no])){ + USB_sendstr("No usteps\n"); + return FALSE; + } + if(!pdnuart_setcurrent(no, the_conf.motcurrent[no])){ + USB_sendstr("No current\n"); + return FALSE; + } + if(!readregister(no, TMC2209Reg_GCONF, &gconf.value)){ + USB_sendstr("Can't read GCONF\n"); + return FALSE; + } gconf.pdn_disable = 1; // PDN now is UART gconf.mstep_reg_select = 1; // microsteps are by MSTEP - if(!writeregister(no, TMC2209Reg_GCONF, gconf.value)) return FALSE; + if(!writeregister(no, TMC2209Reg_GCONF, gconf.value)){ + USB_sendstr("Can't write GCONF\n"); + return FALSE; + } return TRUE; } diff --git a/F3:F303/Multistepper/steppers.c b/F3:F303/Multistepper/steppers.c index bbd530f..13df5ec 100644 --- a/F3:F303/Multistepper/steppers.c +++ b/F3:F303/Multistepper/steppers.c @@ -75,8 +75,21 @@ TRUE_INLINE void recalcARR(int 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; +/* +#ifdef EBUG + USB_sendstr("motor"); USB_putbyte('0'+i); USB_sendstr(", ARR="); + USB_sendstr(u2str(ARR)); USB_sendstr(", curspeed="); + USB_sendstr(u2str(curspeed[i])); +#endif +*/ mottimers[i]->ARR = ARR; curspeed[i] = (((PCLK/(MOTORTIM_PSC+1)) / (ARR+1)) >> ustepsshift[i]); // recalculate speed due to new val +/* +#ifdef EBUG + USB_sendstr(", curspeedNEW="); + USB_sendstr(u2str(curspeed[i])); newline(); +#endif +*/ } // update stepper's settings @@ -103,10 +116,9 @@ void init_steppers(){ stopflag[i] = 0; motdir[i] = 0; curspeed[i] = 0; - state[i] = STP_RELAX; if(!the_conf.motflags[i].donthold) MOTOR_EN(i); else MOTOR_DIS(i); - update_stepper(i); + state[i] = update_stepper(i) ? STP_RELAX : STP_ERR; } } @@ -148,8 +160,13 @@ static void calcacceleration(uint8_t i){ }else{ // triangle speed profile decelstartpos[i] = stppos[i] + delta/2; } - if(the_conf.motflags[i].reverse) MOTOR_CCW(i); - else MOTOR_CW(i); + if(the_conf.motflags[i].reverse){ + DBG("positive - CCW (r)"); + MOTOR_CCW(i); + }else{ + DBG("positive - CW"); + MOTOR_CW(i); + } }else{ // negative direction delta = -delta; if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid @@ -157,8 +174,13 @@ static void calcacceleration(uint8_t i){ }else{ // triangle speed profile decelstartpos[i] = stppos[i] - delta/2; } - if(the_conf.motflags[i].reverse) MOTOR_CW(i); - else MOTOR_CCW(i); + if(the_conf.motflags[i].reverse){ + DBG("negative - CW (r)"); + MOTOR_CW(i); + }else{ + DBG("negative - CCW"); + MOTOR_CCW(i); + } } if(state[i] != STP_MVSLOW){ DBG("->accel"); @@ -365,7 +387,7 @@ static void chkstepper(int i){ if(stp[i]){ stp[i] = 0; // motor state could be changed outside of interrupt, so return it to relax - state[i] = STP_RELAX; + //state[i] = STP_RELAX; USB_sendstr("MOTOR"); USB_putbyte(Nch); USB_sendstr(" stop @"); printi(stppos[i]); USB_sendstr(", V="); printu(curspeed[i]); USB_sendstr(", curstate="); printu(state[i]); newline(); diff --git a/F3:F303/Multistepper/stty b/F3:F303/Multistepper/stty new file mode 100755 index 0000000..298cfa1 --- /dev/null +++ b/F3:F303/Multistepper/stty @@ -0,0 +1,4 @@ +#!/bin/bash + +stty -F /dev/multistepper0 -brkint -icrnl -imaxbel -opost -onlcr -isig -icanon -iexten -echo -echoe -echok -echoctl -echoke min 0 time 5 +while read x; do echo $x > /dev/multistepper0; cat /dev/multistepper0; done < esprif.conf diff --git a/F3:F303/Multistepper/version.inc b/F3:F303/Multistepper/version.inc index 592d71c..c5f29b2 100644 --- a/F3:F303/Multistepper/version.inc +++ b/F3:F303/Multistepper/version.inc @@ -1,2 +1,2 @@ -#define BUILD_NUMBER "205" -#define BUILD_DATE "2026-01-16" +#define BUILD_NUMBER "212" +#define BUILD_DATE "2026-01-20" diff --git a/F3:F303/inc/Fx/common_macros.h b/F3:F303/inc/Fx/common_macros.h index 670e534..f36145b 100644 --- a/F3:F303/inc/Fx/common_macros.h +++ b/F3:F303/inc/Fx/common_macros.h @@ -44,10 +44,6 @@ #define NULL (0) #endif -#ifndef _U_ -#define _U_ __attribute__((__unused__)) -#endif - // some good things from CMSIS #define nop() __NOP()