diff --git a/Inc/stepper.h b/Inc/stepper.h index d4abf53..68dab42 100644 --- a/Inc/stepper.h +++ b/Inc/stepper.h @@ -22,7 +22,7 @@ typedef struct { #define STEPPER_MAX_SPEED 400 -#define STEPPER_STEPS_PER_ROTATION 3200 +#define STEPPER_STEPS_PER_ROTATION 3200*4 void Timer1_Init(void); void stepper_gpio_init(); diff --git a/Src/stepper.c b/Src/stepper.c index 41edd7d..f5ed8ee 100644 --- a/Src/stepper.c +++ b/Src/stepper.c @@ -9,8 +9,13 @@ #include "address_selector.h" stepper_T steppers[] = { - {.en_port=GPIOC, .en_pin=11, .dir_port=GPIOB, .dir_pin=4, .step_port=GPIOE, .step_pin=2, .home_port=GPIOF, .home_pin=3, .home_position=1000}, - {.en_port=GPIOB, .en_pin=3, .dir_port=GPIOF, .dir_pin=11, .step_port=GPIOF, .step_pin=12, .home_port=GPIOF, .home_pin=4, .home_position=0} + {.en_port=GPIOC, .en_pin=11, .dir_port=GPIOB, .dir_pin=4, .step_port=GPIOE, .step_pin=2, .home_port=GPIOB, .home_pin=14, .home_position=0, .homed = 0}, + {.en_port=GPIOB, .en_pin=3, .dir_port=GPIOF, .dir_pin=11, .step_port=GPIOF, .step_pin=12, .home_port=GPIOB, .home_pin=13, .home_position=0, .homed = 0}, + {.en_port=GPIOF, .en_pin=10, .dir_port=GPIOD, .dir_pin=6, .step_port=GPIOD, .step_pin=7, .home_port=GPIOF, .home_pin=7, .home_position=0, .homed = 0}, + {.en_port=GPIOD, .en_pin=5, .dir_port=GPIOD, .dir_pin=2, .step_port=GPIOD, .step_pin=3, .home_port=GPIOB, .home_pin=12, .home_position=0, .homed = 0}, + {.en_port=GPIOD, .en_pin=1, .dir_port=GPIOC, .dir_pin=8, .step_port=GPIOC, .step_pin=9, .home_port=GPIOE, .home_pin=7, .home_position=0, .homed = 0}, + {.en_port=GPIOD, .en_pin=15, .dir_port=GPIOD, .dir_pin=9, .step_port=GPIOD, .step_pin=11, .home_port=GPIOB, .home_pin=11, .home_position=0, .homed = 0}, + //{.en_port=GPIOA, .en_pin=15, .dir_port=GPIOA, .dir_pin=14, .step_port=GPIOA, .step_pin=10, .home_port=GPIOB, .home_pin=11, .home_position=0, .homed = 1}, }; void set_pins(stepper_T* stp){ @@ -30,14 +35,18 @@ void set_pins(stepper_T* stp){ // home switch stp->home_port->MODER &= ~(0x3 << (stp->home_pin * 2)); + stp->home_port->PUPDR &= ~(0x3 << (stp->home_pin * 2)); + stp->home_port->PUPDR |= (0x1 << (stp->home_pin * 2)); // make shure stepper is enabled (active low) stp->en_port->BSRR |= (0x1 << (stp->en_pin + 16)); } void stepper_gpio_init(){ + RCC->IOPENR |= RCC_IOPENR_GPIOAEN; RCC->IOPENR |= RCC_IOPENR_GPIOBEN; RCC->IOPENR |= RCC_IOPENR_GPIOCEN; + RCC->IOPENR |= RCC_IOPENR_GPIODEN; RCC->IOPENR |= RCC_IOPENR_GPIOEEN; RCC->IOPENR |= RCC_IOPENR_GPIOFEN; @@ -76,6 +85,7 @@ void TIM1_BRK_UP_TRG_COM_IRQHandler(void) { void do_steps(){ static uint32_t timer = 0; + int16_t rotation_speed = 0; timer++; for(uint8_t i=0; i<(sizeof(steppers)/sizeof(stepper_T)); i++){ @@ -123,31 +133,38 @@ void do_steps(){ steppers[i].next_step = timer + 1; } - printf("motor[%d]: pos %ld\trot: %ld\tspeed %ld\n", i, steppers[i].pos, steppers[i].endless_rot_integrator, steppers[i].speed); + //printf("motor[%d]: pos %ld\trot: %ld\tspeed %ld\n", i, steppers[i].pos, steppers[i].endless_rot_integrator, steppers[i].speed); + //printf("rotation speed: %d\n", rotation_speed); } if(steppers[i].homed){ static uint16_t old_ramp_to_speed; old_ramp_to_speed = steppers[i].ramp_to_speed; - uint8_t raw_endless = rxBuffer[6*i + DMX_ADDRESS + 2]; - int16_t rotation_speed; + uint8_t raw_endless = rxBuffer[3*i + DMX_ADDRESS + 2]; - ///* deadzone on top and bottom */ - //if(raw_endless >= 5 && raw_endless <= 250) { - // rotation_speed = raw_endless - 127; + /* deadzone on top and bottom */ + if(raw_endless >= 5 && raw_endless <= 250) { + rotation_speed = raw_endless - 127; - // /* dead zone in the middle */ - // if(rotation_speed >= 5) - // rotation_speed -= 5; - // else if(rotation_speed <= -5) - // rotation_speed += 5; - // else - // rotation_speed = 0; - //} - //else{ - // rotation_speed = 0; - //} + /* dead zone in the middle */ + if(rotation_speed >= 5) + rotation_speed -= 5; + else if(rotation_speed <= -5) + rotation_speed += 5; + else + rotation_speed = 0; + } + else{ + rotation_speed = 0; + } + + if(steppers[i].pos == STEPPER_STEPS_PER_ROTATION+1){ + steppers[i].pos = 1; + } + if(steppers[i].pos == -1){ + steppers[i].pos = STEPPER_STEPS_PER_ROTATION-1; + } ///* handle endless rotation offset and wraparound */ //if(rotation_speed){ @@ -166,11 +183,20 @@ void do_steps(){ //else // steppers[i].endless_rot_integrator = 0; - - int32_t swapped_val = rxBuffer[6*i + DMX_ADDRESS]<<8 | (rxBuffer[6*i+DMX_ADDRESS+1]&0xFF); - //swapped_val += steppers[i].endless_rot_integrator; - int32_t scaled_steps = swapped_val * STEPPER_STEPS_PER_ROTATION / UINT16_MAX; - steppers[i].ramp_to_speed = scaled_steps - steppers[i].pos; + if(rotation_speed){ + steppers[i].ramp_to_speed = rotation_speed; + } + else{ + int32_t swapped_val = rxBuffer[3*i + DMX_ADDRESS]<<8 | (rxBuffer[3*i+DMX_ADDRESS+1]&0xFF); + //swapped_val += steppers[i].endless_rot_integrator; + int32_t scaled_steps = swapped_val * STEPPER_STEPS_PER_ROTATION / UINT16_MAX; + steppers[i].ramp_to_speed = scaled_steps - steppers[i].pos; + //static int cnt = 0; + //if(i==5 && ++cnt==10000){ + // printf("swapped_val[%d]: %d\n", i, swapped_val); + // cnt=0; + //} + } if(old_ramp_to_speed == 0 && steppers[i].ramp_to_speed != 0) steppers[i].trigger_step = 1; @@ -183,9 +209,16 @@ void do_steps(){ steppers[i].ramp_to_speed = 0; printf("homed %d\n", i); } + else if(steppers[i].pos == 2*STEPPER_STEPS_PER_ROTATION){ + steppers[i].pos = 0; + steppers[i].homed = 1; + steppers[i].speed = 0; + steppers[i].ramp_to_speed = 0; + printf("cant home %d, setting 0 now\n", i); + } else{ if(steppers[i].ramp_to_speed == 0){ - steppers[i].ramp_to_speed=10; + steppers[i].ramp_to_speed=80; steppers[i].trigger_step = 1; // trigger first step } }