diff --git a/F0-nolib/3steppersLB/commonproto.c b/F0-nolib/3steppersLB/commonproto.c index 88ada4f..950d87b 100644 --- a/F0-nolib/3steppersLB/commonproto.c +++ b/F0-nolib/3steppersLB/commonproto.c @@ -22,6 +22,7 @@ #include "commonproto.h" #include "flash.h" #include "hardware.h" +#include "steppers.h" #ifdef EBUG #include "strfunct.h" #endif @@ -158,26 +159,103 @@ static errcodes extparser(uint8_t par, int32_t *val){ return ERR_OK; } +/******************* START of config parsers *******************/ +static const uint8_t bval[] = {0,1,2,2,3,3,3,3,4,4,4,4,4,4,4,4}; +static errcodes ustepsparser(uint8_t par, int32_t *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + if(ISSETTER(par)){ +#if MICROSTEPSMAX > 32768 +#error "Change the code here!" +#endif + uint16_t m = (uint16_t)*val; + if(m < 1 || m > MICROSTEPSMAX) return ERR_BADVAL; + // find most significant bit + uint8_t r = 0; + uint16_t x = m; + if(x & 0xff00){r += 8; x >>= 8;} + if(x & 0x00f0){r += 4; x >>= 4;} + uint8_t mostbit = (uint8_t)r + bval[x]; + if(m != 1< MOTORSNO-1) return ERR_BADPAR; + if(ISSETTER(par)){ + if(*val/the_conf.microsteps[n] > ACCELMAXSTEPS || *val < 1) return ERR_BADVAL; + the_conf.accel[n] = *val; + } + *val = the_conf.accel[n]; + return ERR_OK; +} + +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; + } + *val = the_conf.maxspd[n]; + return ERR_OK; +} + +static errcodes maxstepsparser(uint8_t par, int32_t *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + if(ISSETTER(par)){ + if(*val < 1) return ERR_BADVAL; + the_conf.maxsteps[n] = *val; + } + *val = the_conf.maxsteps[n]; + return ERR_OK; +} + +static errcodes encrevparser(uint8_t par, int32_t *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + if(ISSETTER(par)){ + if(*val < 1 || *val > MAXENCREV) return ERR_BADVAL; + the_conf.encrev[n] = *val; + enctimers[n]->ARR = *val; + } + *val = the_conf.encrev[n]; + return ERR_OK; +} + static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){ if(store_userconf()) return ERR_CANTRUN; return ERR_OK; } +/******************* END of config parsers *******************/ -#if 0 -typedef struct __attribute__((packed, aligned(4))){ - uint16_t microsteps; // microsteps amount per step - uint16_t accdecsteps; // amount of steps need for full acceleration/deceleration cycle - uint16_t motspd; // max motor speed (steps per second) - uint32_t maxsteps; // maximal amount of steps from ESW0 to EWS3 - defflags_t defflags; // default flags -} user_conf; -#endif +/******************* START of motors' parsers *******************/ +static errcodes mstopparser(uint8_t par, int32_t _U_ *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + stopmotor(n); + return ERR_OK; +} + +static errcodes curposparser(uint8_t par, int32_t *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + if(ISSETTER(par)) return setpos(n, *val); + return getpos(n, val); +} +/******************* END of motors' parsers *******************/ /* -static CAN_errcodes parser(const uint8_t _U_ par, const int32_t _U_ *val){ - return CANERR_OK; -}*/ +static errcodes parser(uint8_t _U_ par, int32_t _U_ *val){ + return ERR_OK; +} +*/ // the main commands list, index is CAN command code const commands cmdlist[CMD_AMOUNT] = { @@ -194,5 +272,12 @@ const commands cmdlist[CMD_AMOUNT] = { [CMD_PWM] = {"pwm", pwmparser, "pwm value"}, [CMD_EXT] = {"ext", extparser, "external outputs"}, [CMD_SAVECONF] = {"saveconf", saveconfparser, "save current configuration"}, + [CMD_CURPOS] = {"position", curposparser, "set/get position (in steps)"}, + [CMD_MICROSTEPS] = {"microsteps", ustepsparser, "set/get microsteps settings"}, + [CMD_STOPMOTOR] = {"stop", mstopparser, "stop motor now"}, + [CMD_ACCEL] = {"accel", accparser, "set/get accel/decel (usteps/s per 10ms)"}, + [CMD_MAXSPEED] = {"maxspeed", maxspdparser, "set/get max speed (usteps per sec)"}, + [CMD_MAXSTEPS] = {"maxsteps", maxstepsparser, "set/get max steps (from zero)"}, + [CMD_ENCREV] = {"encrev", encrevparser, "set/get max encoder's pulses per revolution"}, }; diff --git a/F0-nolib/3steppersLB/commonproto.h b/F0-nolib/3steppersLB/commonproto.h index 385ae5b..67562b1 100644 --- a/F0-nolib/3steppersLB/commonproto.h +++ b/F0-nolib/3steppersLB/commonproto.h @@ -71,6 +71,13 @@ enum{ ,CMD_PWM // PWM value ,CMD_EXT // value on EXTx outputs ,CMD_SAVECONF // save configuration + ,CMD_MICROSTEPS // get/set microsteps + ,CMD_CURPOS // current position (set/get) + ,CMD_STOPMOTOR // stop moving NOW + ,CMD_ACCEL // set/get acceleration/deceleration + ,CMD_MAXSPEED // set/get maximal speed + ,CMD_MAXSTEPS // max steps (-max..+max) + ,CMD_ENCREV // encoder's pulses per revolution // should be the last: ,CMD_AMOUNT // amount of common commands }; diff --git a/F0-nolib/3steppersLB/flash.c b/F0-nolib/3steppersLB/flash.c index b7a091c..9636339 100644 --- a/F0-nolib/3steppersLB/flash.c +++ b/F0-nolib/3steppersLB/flash.c @@ -33,15 +33,18 @@ 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 USERCONF_INITIALIZER { \ .userconf_sz = sizeof(user_conf) \ ,.CANspeed = 100 \ ,.CANID = 0xaa \ ,.microsteps = {32, 32, 32} \ - ,.accdecsteps = {25, 25, 25} \ + ,.accel = {160, 160, 160} \ ,.maxspd = {1000, 1000, 1000} \ ,.maxsteps = {50000, 50000, 50000} \ - ,.motflags = {{0},{0},{0}} \ + ,.encrev = {800,800,800} \ + ,.motflags = {DEFMF,DEFMF,DEFMF} \ } static int erase_flash(const void*, const void*); static int write2flash(const void*, const void*, uint32_t); @@ -214,7 +217,7 @@ void dump_userconf(_U_ char *txt){ PROPNAME("microsteps"); printu(the_conf.microsteps[i]); PROPNAME("accdecsteps"); - printu(the_conf.accdecsteps[i]); + printu(the_conf.accel[i]); PROPNAME("maxspeed"); printu(the_conf.maxspd[i]); PROPNAME("maxsteps"); diff --git a/F0-nolib/3steppersLB/flash.h b/F0-nolib/3steppersLB/flash.h index bdc613d..b840c06 100644 --- a/F0-nolib/3steppersLB/flash.h +++ b/F0-nolib/3steppersLB/flash.h @@ -30,6 +30,14 @@ #define _U_ __attribute__((unused)) #endif +// limiting values +#define MICROSTEPSMAX (512) +// (STEPS per second per 10ms) +#define ACCELMAXSTEPS (100) +// max speed IN STEPS! +#define MAXMAXSPD (10000) +// max encoder steps per rev +#define MAXENCREV (100000) // register with flash size (in blocks) #ifndef FLASH_SIZE_REG @@ -40,7 +48,9 @@ blocksizeASH_SIZE_REG ((uint32_t)0x1FFFF7CC) // motor flags typedef struct{ - uint8_t reverse : 1; + uint8_t reverse : 1; // reversing motor rotation + uint8_t encreverse : 1; // reversing encoder rotation + uint8_t haveencoder : 1; // have encoder } motflags_t; /* @@ -51,9 +61,10 @@ typedef struct __attribute__((packed, aligned(4))){ uint16_t CANspeed; // default CAN speed uint16_t CANID; // identifier uint16_t microsteps[MOTORSNO]; // microsteps amount per step - uint16_t accdecsteps[MOTORSNO]; // amount of steps need for full acceleration/deceleration cycle - uint16_t maxspd[MOTORSNO]; // max motor speed (steps per second) - uint32_t maxsteps[MOTORSNO]; // maximal amount of steps from ESW0 to EWS3 + uint16_t accel[MOTORSNO]; // acceleration/deceleration (dv microsteps/s per 10ms) + uint16_t maxspd[MOTORSNO]; // max motor speed (microsteps per second) + uint32_t maxsteps[MOTORSNO]; // maximal amount of steps + uint16_t encrev[MOTORSNO]; // encoders' counts per revolution motflags_t motflags[MOTORSNO]; // motor's flags } user_conf; diff --git a/F0-nolib/3steppersLB/hardware.c b/F0-nolib/3steppersLB/hardware.c index 031e51e..e842095 100644 --- a/F0-nolib/3steppersLB/hardware.c +++ b/F0-nolib/3steppersLB/hardware.c @@ -35,6 +35,11 @@ const uint32_t ENpins[MOTORSNO] = {1<<0, 1<<2, 1<<11}; volatile GPIO_TypeDef *DIRports[MOTORSNO] = {GPIOB, GPIOB, GPIOB}; const uint32_t DIRpins[MOTORSNO] = {1<<1, 1<<10, 1<<12}; +// timers for motors +TIM_TypeDef *mottimers[MOTORSNO] = {TIM15, TIM14, TIM16}; +// timers for encoders +TIM_TypeDef *enctimers[MOTORSNO] = {TIM1, TIM2, TIM3}; + void gpio_setup(void){ RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOCEN | RCC_AHBENR_GPIOFEN; GPIOA->MODER = GPIO_MODER_MODER0_AF | GPIO_MODER_MODER1_AF | GPIO_MODER_MODER2_AF | GPIO_MODER_MODER3_AI | @@ -76,8 +81,10 @@ void iwdg_setup(){ IWDG->KR = IWDG_REFRESH; /* (6) */ } +static IRQn_Type motirqs[MOTORSNO] = {TIM15_IRQn, TIM14_IRQn, TIM16_IRQn}; // motor's PWM -static void setup_mpwm(TIM_TypeDef *TIM){ +static void setup_mpwm(int i){ + TIM_TypeDef *TIM = mottimers[i]; TIM->PSC = 999; // 48kHz TIM->ARR = 1000; // starting ARR value // PWM mode 1 (OCxM = 110), preload enable @@ -85,41 +92,53 @@ static void setup_mpwm(TIM_TypeDef *TIM){ 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->CR1 |= TIM_CR1_CEN; // enable timer +// TIM->CR1 |= TIM_CR1_CEN; // enable timer TIM->EGR |= TIM_EGR_UG; // force update generation TIM->DIER = TIM_DIER_CC1IE; // allow CC interrupt (we should count steps) + NVIC_EnableIRQ(motirqs[i]); +} + +static IRQn_Type encirqs[MOTORSNO] = {TIM1_BRK_UP_TRG_COM_IRQn, TIM2_IRQn, TIM3_IRQn}; +static void setup_enc(int i){ + TIM_TypeDef *TIM = enctimers[i]; + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; + /* (1) Configure TI1FP1 on TI1 (CC1S = 01) + configure TI1FP2 on TI2 (CC2S = 01) + filters sampling = fDTS/8, N=6 */ + /* (2) Configure TI1FP1 and TI1FP2 non inverted (CC1P = CC2P = 0, reset value) */ + /* (3) Configure both inputs are active on both rising and falling edges + (SMS = 011), set external trigger filter to f_DTS/8, N=6 (ETF=1000) */ + /* (4) Enable the counter by writing CEN=1 in the TIMx_CR1 register, + set tDTS=4*tCK_INT. */ + TIM->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0 | TIM_CCMR1_IC1F_3 | TIM_CCMR1_IC2F_3; /* (1)*/ + //TIMx->CCER &= (uint16_t)(~(TIM_CCER_CC21 | TIM_CCER_CC2P); /* (2) */ + TIM->SMCR = TIM_SMCR_ETF_3 | TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1; /* (3) */ + // enable update interrupt + TIM->DIER = TIM_DIER_UIE; + // generate interrupt each revolution + TIM->ARR = the_conf.encrev[i]; + // enable timer + TIM->CR1 = TIM_CR1_CKD_1 | TIM_CR1_CEN; /* (4) */ + NVIC_EnableIRQ(encirqs[i]); } -/* -static void setup_enc(TIM_TypeDef *TIM, uint16_t arrval){ - TIM->PSC = 9; // F=48/10 = 4.8MHz - TIM->ARR = arrval; // PWM frequency = 4800/256 = 18.75kHz - // PWM mode 1 (OCxM = 110), preload enable - TIM->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE | - TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE; - TIM->CCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE; - TIM->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E; // active high (CC1P=0), enable outputs - TIM->BDTR |= TIM_BDTR_MOE; // enable main output - TIM->CR1 |= TIM_CR1_CEN; // enable timer - TIM->EGR |= TIM_EGR_UG; // force update generation -}*/ // timers 14,15,16,17 - PWM@ch1, 1,2,3 - encoders @ ch1/2 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_mpwm(TIM14); - setup_mpwm(TIM15); - setup_mpwm(TIM16); - // setup PWM @ TIM17 + // setup PWM @ TIM17 - single PWM channel TIM17->PSC = 5; // 8MHz for 31kHz PWM TIM17->ARR = 254; // ARR for 8-bit PWM TIM17->BDTR |= TIM_BDTR_MOE; // enable main output TIM17->CCER = TIM_CCER_CC1E; // enable PWM output TIM17->CR1 |= TIM_CR1_CEN; // enable timer - ; - NVIC_EnableIRQ(TIM14_IRQn); - NVIC_EnableIRQ(TIM15_IRQn); - NVIC_EnableIRQ(TIM16_IRQn); + 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 + setup_enc(i); + } + } } // pause in milliseconds for some purposes @@ -160,17 +179,23 @@ void Jump2Boot(){ // for STM32F072 } -// count steps @tim 14/15/16 -static void stp_isr(TIM_TypeDef *TIM, int i){ - ++steps[i]; // count steps - TIM->SR = 0; -} void tim14_isr(){ - stp_isr(TIM14, 0); + addmicrostep(0); } void tim15_isr(){ - stp_isr(TIM15, 1); + addmicrostep(1); } void tim16_isr(){ - stp_isr(TIM16, 2); + addmicrostep(2); +} + + +void tim1_isr(){ + encoders_UPD(0); +} +void tim2_isr(){ + encoders_UPD(1); +} +void tim3_isr(){ + encoders_UPD(2); } diff --git a/F0-nolib/3steppersLB/hardware.h b/F0-nolib/3steppersLB/hardware.h index 7898f34..17aa16d 100644 --- a/F0-nolib/3steppersLB/hardware.h +++ b/F0-nolib/3steppersLB/hardware.h @@ -173,6 +173,11 @@ extern const uint32_t DIRpins[MOTORSNO]; extern volatile uint32_t Tms; +// timers of motors +extern TIM_TypeDef *mottimers[]; +// timers for encoders +extern TIM_TypeDef *enctimers[]; + void gpio_setup(); void iwdg_setup(); void timers_setup(); diff --git a/F0-nolib/3steppersLB/main.c b/F0-nolib/3steppersLB/main.c index 194300a..293cfd2 100644 --- a/F0-nolib/3steppersLB/main.c +++ b/F0-nolib/3steppersLB/main.c @@ -22,6 +22,7 @@ #include "custom_buttons.h" #include "flash.h" #include "hardware.h" +#include "steppers.h" #include "strfunct.h" #include "usb.h" #include "usb_lib.h" @@ -72,12 +73,12 @@ int main(void){ char *txt; sysreset(); SysTick_Config(6000, 1); - flashstorage_init(); + flashstorage_init(); // should be called before any other functions gpio_setup(); USB_setup(); CAN_setup(DEFAULT_CAN_SPEED); adc_setup(); - timers_setup(); + init_steppers(); RCC->CSR |= RCC_CSR_RMVF; // remove reset flags iwdg_setup(); diff --git a/F0-nolib/3steppersLB/steppers.bin b/F0-nolib/3steppersLB/steppers.bin index 79b45e5..e1b8217 100755 Binary files a/F0-nolib/3steppersLB/steppers.bin and b/F0-nolib/3steppersLB/steppers.bin differ diff --git a/F0-nolib/3steppersLB/steppers.c b/F0-nolib/3steppersLB/steppers.c index 21fcab2..86a2adb 100644 --- a/F0-nolib/3steppersLB/steppers.c +++ b/F0-nolib/3steppersLB/steppers.c @@ -16,8 +16,83 @@ * along with this program. If not, see . */ +#include "flash.h" #include "hardware.h" #include "steppers.h" -int32_t steps[MOTORSNO] = {0,0,0}; +int8_t motdir[MOTORSNO]; // motors' direction: 1 for positive, -1 for negative (we need it as could be reverse) +static volatile int32_t stppos[MOTORSNO]; // current position (in steps) by STP counter +static int32_t encpos[MOTORSNO]; // current encoder position (4 per ticks) minus TIM->CNT +static int32_t stepsleft[MOTORSNO]; // steps left to stop (full steps or encoder's counters) + +static uint16_t microsteps[MOTORSNO] = {0}; // current microsteps position +static uint8_t stopflag[MOTORSNO]; // ==1 to stop @ nearest step + +void init_steppers(){ + // init variables + for(int i = 0; i < MOTORSNO; ++i){ + mottimers[i]->CR1 &= ~TIM_CR1_CEN; + stepsleft[i] = 0; + microsteps[i] = 0; + stopflag[i] = 0; + motdir[i] = 1; + stppos[i] = 0; + } + timers_setup(); +} + +// get position +errcodes getpos(int i, int32_t *position){ + if(the_conf.motflags[i].haveencoder){ + *position = encpos[i] + enctimers[i]->CNT; + }else *position = stppos[i]; + return ERR_OK; +} +// set position +errcodes setpos(int i, int32_t newpos){ + if(newpos < -(int32_t)the_conf.maxsteps[i] || newpos > (int32_t)the_conf.maxsteps[i]) + return ERR_BADPAR; + int32_t diff = newpos - stppos[i]; + if(diff == 0) return ERR_OK; + if(diff > 0) motdir[i] = 1; + else{ + diff = -diff; + motdir[i] = -1; + } + // TODO: set direction pin, run timer, set target position + // depends on the_conf.motflags[i].reverse + return ERR_OK; +} + +void stopmotor(int i){ + stopflag[i] = 1; + stepsleft[i] = 0; + microsteps[i] = 0; +} + +// count steps @tim 14/15/16 +void addmicrostep(int i){ + if(mottimers[i]->SR & TIM_SR_UIF){ + if(++microsteps[i] == the_conf.microsteps[i]){ + microsteps[i] = 0; + if(stopflag[i]){ // stop NOW + stopflag[i] = 0; + mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer + } + stppos[i] += motdir[i]; + } + } + mottimers[i]->SR = 0; +} + +void encoders_UPD(int i){ + if(enctimers[i]->SR & TIM_SR_UIF){ + int8_t d = 1; // positive (-1 - negative) + if(enctimers[i]->CR1 & TIM_CR1_DIR) d = -d; // negative + if(the_conf.motflags[i].encreverse) d = -d; + if(d == 1) encpos[i] += the_conf.encrev[i]; + else encpos[i] -= the_conf.encrev[i]; + } + enctimers[i]->SR = 0; +} diff --git a/F0-nolib/3steppersLB/steppers.h b/F0-nolib/3steppersLB/steppers.h index 895e84a..3612709 100644 --- a/F0-nolib/3steppersLB/steppers.h +++ b/F0-nolib/3steppersLB/steppers.h @@ -21,7 +21,17 @@ #define STEPPERS_H__ #include +#include "commonproto.h" -extern int32_t steps[]; +// direction +extern int8_t motdir[]; + +void addmicrostep(int i); +void encoders_UPD(int i); + +void init_steppers(); +errcodes getpos(int i, int32_t *position); +errcodes setpos(int i, int32_t newpos); +void stopmotor(int i); #endif // STEPPERS_H__