You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
228 lines
8.7 KiB
C
228 lines
8.7 KiB
C
#include "stm32g0b1xx.h"
|
|
#include "stepper.h"
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "SEGGER_SYSVIEW.h"
|
|
#include "uart_dmx.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=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){
|
|
// Set direction (00: Input, 01: Output, 10: Alternate function, 11: Analog)
|
|
|
|
// enable
|
|
stp->en_port->MODER &= ~(0x3 << (stp->en_pin * 2));
|
|
stp->en_port->MODER |= (0x1 << (stp->en_pin * 2));
|
|
|
|
// direction
|
|
stp->dir_port->MODER &= ~(0x3 << (stp->dir_pin * 2));
|
|
stp->dir_port->MODER |= (0x1 << (stp->dir_pin * 2));
|
|
|
|
// step
|
|
stp->step_port->MODER &= ~(0x3 << (stp->step_pin * 2));
|
|
stp->step_port->MODER |= (0x1 << (stp->step_pin * 2));
|
|
|
|
// 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]);
|
|
}
|
|
}
|
|
|
|
void Timer1_Init(void) {
|
|
// Step 1: Enable the clock for Timer 1
|
|
RCC->APBENR2 |= RCC_APBENR2_TIM1EN; // Enable TIM1 clock
|
|
|
|
// Step 2: Configure Timer 1
|
|
TIM1->PSC = 64;
|
|
TIM1->ARR = 100;
|
|
TIM1->DIER |= TIM_DIER_UIE; // Enable update interrupt
|
|
|
|
// Step 3: Enable Timer 1
|
|
TIM1->CR1 |= TIM_CR1_CEN; // Enable the timer
|
|
|
|
// Step 4: Enable the interrupt in NVIC
|
|
NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn); // Enable Timer 1 interrupt in NVIC
|
|
}
|
|
|
|
// Timer 1 Update Interrupt Handler
|
|
void TIM1_BRK_UP_TRG_COM_IRQHandler(void) {
|
|
SEGGER_SYSVIEW_RecordEnterISR();
|
|
if (TIM1->SR & TIM_SR_UIF) { // Check if the update interrupt flag is set
|
|
TIM1->SR &= ~TIM_SR_UIF; // Clear the update interrupt flag
|
|
|
|
//SEGGER_SYSVIEW_PrintfHost("do step");
|
|
do_steps();
|
|
}
|
|
SEGGER_SYSVIEW_RecordExitISR();
|
|
}
|
|
|
|
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++){
|
|
if((steppers[i].ramp_to_speed != 0 && steppers[i].next_step == timer) || steppers[i].trigger_step){
|
|
/* reset trigger */
|
|
steppers[i].trigger_step = 0;
|
|
|
|
/* ramp speed */
|
|
if(steppers[i].speed < steppers[i].ramp_to_speed)
|
|
steppers[i].speed++;
|
|
if(steppers[i].speed > steppers[i].ramp_to_speed)
|
|
steppers[i].speed--;
|
|
|
|
/* limit max speed */
|
|
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)
|
|
steppers[i].dir_port->BSRR |= 1 << steppers[i].dir_pin;
|
|
if(steppers[i].speed<0)
|
|
steppers[i].dir_port->BSRR |= 1 << (steppers[i].dir_pin + 16);
|
|
|
|
// avoid division by zero
|
|
if(steppers[i].speed != 0){
|
|
/* calculate next step time
|
|
*
|
|
* should be safe for 1 overflow; speeds which
|
|
* require longer wait times than uint32_max not supported
|
|
* */
|
|
steppers[i].next_step = timer + (1000/abs(steppers[i].speed));
|
|
|
|
/* step and count position */
|
|
steppers[i].step_port->ODR ^= 1 << steppers[i].step_pin;
|
|
if(steppers[i].speed>0)
|
|
steppers[i].pos++;
|
|
if(steppers[i].speed<0)
|
|
steppers[i].pos--;
|
|
|
|
}
|
|
else{
|
|
// dont send step at speed == 0
|
|
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("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[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 = 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=80;
|
|
steppers[i].trigger_step = 1; // trigger first step
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|