|
|
|
|
@ -6,11 +6,16 @@
|
|
|
|
|
|
|
|
|
|
#include "SEGGER_SYSVIEW.h"
|
|
|
|
|
#include "uart_dmx.h"
|
|
|
|
|
#include "mapping_table.h"
|
|
|
|
|
#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},
|
|
|
|
|
{.en_port=GPIOB, .en_pin=3, .dir_port=GPIOF, .dir_pin=11, .step_port=GPIOF, .step_pin=12, .home_port=GPIOF, .home_pin=4}
|
|
|
|
|
{.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,23 +35,24 @@ 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;
|
|
|
|
|
|
|
|
|
|
for(uint8_t i=0; i<(sizeof(steppers)/sizeof(stepper_T)); i++){
|
|
|
|
|
set_pins(&steppers[i]);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
steppers[0].homed = 1;
|
|
|
|
|
steppers[1].homed = 1;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Timer1_Init(void) {
|
|
|
|
|
@ -79,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++){
|
|
|
|
|
@ -93,10 +100,10 @@ void do_steps(){
|
|
|
|
|
steppers[i].speed--;
|
|
|
|
|
|
|
|
|
|
/* limit max speed */
|
|
|
|
|
if(steppers[i].speed >= STEPPER_SPEED_LIMIT)
|
|
|
|
|
steppers[i].speed=STEPPER_SPEED_LIMIT;
|
|
|
|
|
if(steppers[i].speed <= -STEPPER_SPEED_LIMIT)
|
|
|
|
|
steppers[i].speed=-STEPPER_SPEED_LIMIT;
|
|
|
|
|
if(steppers[i].speed >= STEPPER_MAX_SPEED)
|
|
|
|
|
steppers[i].speed=STEPPER_MAX_SPEED;
|
|
|
|
|
if(steppers[i].speed <= -STEPPER_MAX_SPEED)
|
|
|
|
|
steppers[i].speed=-STEPPER_MAX_SPEED;
|
|
|
|
|
|
|
|
|
|
/* get direction from sign */
|
|
|
|
|
if(steppers[i].speed>0)
|
|
|
|
|
@ -126,29 +133,92 @@ void do_steps(){
|
|
|
|
|
steppers[i].next_step = timer + 1;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
printf("pos %d\tspeed %d\n", steppers[i].pos, 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;
|
|
|
|
|
motor_mapping_T* tmp = &(((fixture_mapping_T*)&rxBuffer[1])->channels[i]);
|
|
|
|
|
steppers[i].ramp_to_speed = tmp->tilt - steppers[i].pos;
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
|
|
|
|
|
/* 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){
|
|
|
|
|
// steppers[i].endless_rot_integrator += rotation_speed;
|
|
|
|
|
// if(steppers[i].endless_rot_integrator >= STEPPER_STEPS_PER_ROTATION)
|
|
|
|
|
// {
|
|
|
|
|
// steppers[i].endless_rot_integrator -= STEPPER_STEPS_PER_ROTATION;
|
|
|
|
|
// steppers[i].pos -= STEPPER_STEPS_PER_ROTATION;
|
|
|
|
|
// }
|
|
|
|
|
// else if(steppers[i].endless_rot_integrator <= 0)
|
|
|
|
|
// {
|
|
|
|
|
// steppers[i].endless_rot_integrator += STEPPER_STEPS_PER_ROTATION;
|
|
|
|
|
// steppers[i].pos += STEPPER_STEPS_PER_ROTATION;
|
|
|
|
|
// }
|
|
|
|
|
//}
|
|
|
|
|
//else
|
|
|
|
|
// steppers[i].endless_rot_integrator = 0;
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
else{
|
|
|
|
|
if(~(steppers[i].home_port->IDR) & (1 << steppers[i].home_pin)){
|
|
|
|
|
steppers[i].pos = 0;
|
|
|
|
|
steppers[i].pos = steppers[i].home_position;
|
|
|
|
|
steppers[i].homed = 1;
|
|
|
|
|
steppers[i].speed = 0;
|
|
|
|
|
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
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
Eggert Jung