hpgl_xy/stm32f103c8t6/src/pwm_output.c

#include <string.h>
#include "stm32f1xx.h"
#include "pwm_output.h"
#include "config.h"
#include "pinning.h"
#include "ownership.h"

MODULE_OWNS_PIN(GPIOA, PIN_OUTPUT_X);
MODULE_OWNS_PIN(GPIOA, PIN_OUTPUT_Y);
MODULE_OWNS_PIN(GPIOB, PIN_PEN_STATE);
MODULE_OWNS_PERIPHERAL(TIM1);

static HPGL_Movement_t Output_Buffer[CONFIG_BUFFER_MOVEMENTS];
static const HPGL_Movement_t *Output_BufferRead = Output_Buffer;
static HPGL_Movement_t *Output_BufferWrite = Output_Buffer;
static bool Output_PenIsDown;
static unsigned int Output_DelayCounter = 0;

typedef struct
{
    unsigned int x;
    unsigned int y;
} Output_Coordinate_t;

static Output_Coordinate_t Output_CurrentLineStart = {0, 0};
static Output_Coordinate_t Output_CurrentLineEnd = {0, 0};
static unsigned int Output_CurrentLineLength;
static unsigned int Output_CurrentLinePosition;
static unsigned int Output_CurrentVelocity = OUTPUT_LENGTH_SCALE / 2;

bool Output_EnqueueMovement(HPGL_Movement_t movement)
{
    int buffer_space = Output_BufferRead - Output_BufferWrite - 1;
    if(buffer_space < 0)
    {
        buffer_space += CONFIG_BUFFER_MOVEMENTS;
    }
    if(buffer_space == 0)
    {
        return false;
    }

    if(movement.x > OUTPUT_COORDINATE_LIMIT)
    {
        movement.x = OUTPUT_COORDINATE_LIMIT;
    }
    if(movement.y > OUTPUT_COORDINATE_LIMIT)
    {
        movement.y = OUTPUT_COORDINATE_LIMIT;
    }

    memcpy(Output_BufferWrite, &movement, sizeof(HPGL_Movement_t));
    Output_BufferWrite++;
    if(Output_BufferWrite >= Output_Buffer + CONFIG_BUFFER_MOVEMENTS)
    {
        Output_BufferWrite = Output_Buffer;
    }
    return true;
}

static void Output_PenDown(void)
{
    GPIOB->BRR = (1 << PIN_PEN_STATE);
    Output_PenIsDown = true;
}

static void Output_PenUp(void)
{
    GPIOB->BSRR = (1 << PIN_PEN_STATE);
    Output_PenIsDown = false;
}

static unsigned int Output_ApproximateLength(int dx, int dy)
{
    if(dx < 0)
    {
        dx = -dx;
    }
    if(dy < 0)
    {
        dy = -dy;
    }

    // Approximate vector length with alpha max plus beta min algorithm
    if(dx < dy)
    {
        return (dy + dx) * OUTPUT_LENGTH_SCALE / 4;
    }
    else
    {
        return (dx + dy) * OUTPUT_LENGTH_SCALE / 4;
    }
}

static bool Output_FetchNextPoint(void)
{
    if(Output_BufferRead == Output_BufferWrite)
    {
        return false;
    }

    const HPGL_Movement_t *movement = Output_BufferRead;
    if(movement->pen_down != Output_PenIsDown)
    {
        if(movement->pen_down)
        {
            Output_PenDown();
        }
        else
        {
            Output_PenUp();
        }
        Output_DelayCounter = OUTPUT_PEN_DELAY;
        return false;
    }

    Output_CurrentLineStart = Output_CurrentLineEnd;
    Output_CurrentLineEnd.x = movement->x;
    Output_CurrentLineEnd.y = movement->y;
    Output_CurrentLineLength = Output_ApproximateLength(
        Output_CurrentLineEnd.x - Output_CurrentLineStart.x,
        Output_CurrentLineEnd.y - Output_CurrentLineStart.y);
    Output_CurrentLinePosition = 0;

    Output_BufferRead++;
    if(Output_BufferRead >= Output_Buffer + CONFIG_BUFFER_MOVEMENTS)
    {
        Output_BufferRead = Output_Buffer;
    }
    return true;
}

static void Output_Position(Output_Coordinate_t position)
{
    uint16_t compare_x = OUTPUT_PWM_OFFSET + position.x;
    uint16_t compare_y = OUTPUT_PWM_OFFSET + position.y;

    TIM1->CCR1 = compare_x;
    TIM1->CCR2 = compare_y;
}

void Output_Tick(void)
{
    if(Output_DelayCounter > 0)
    {
        Output_DelayCounter--;
        return;
    }

    unsigned int step_length = Output_CurrentVelocity;
    Output_Coordinate_t position;
    while(true)
    {
        // Compute distance from current position to current line’s end
        unsigned int length_ahead = Output_CurrentLineLength
            - Output_CurrentLinePosition;
        if(step_length <= length_ahead)
        {
            // Move closer to current line end point
            Output_CurrentLinePosition += step_length;
            unsigned int remaining = Output_CurrentLineLength
                - Output_CurrentLinePosition;
            position.x = (Output_CurrentLineEnd.x * Output_CurrentLinePosition
                    + Output_CurrentLineStart.x * remaining)
                / Output_CurrentLineLength;
            position.y = (Output_CurrentLineEnd.y * Output_CurrentLinePosition
                    + Output_CurrentLineStart.y * remaining)
                / Output_CurrentLineLength;
            break;
        }

        // Current target point is not far enough away to keep velocity
        step_length -= length_ahead;
        position = Output_CurrentLineEnd;
        Output_CurrentLinePosition = Output_CurrentLineLength;

        // Replace line start with current line end and fetch new line end
        if(!Output_FetchNextPoint())
        {
            // No more points in queue or pen state change
            break;
        }
    }

    Output_Position(position);
}

void Output_Init(void)
{
    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;

    Output_PenUp();

    GPIOA->CRH = (GPIOA->CRH
        & ~(0xf << (4 * PIN_OUTPUT_X - 32))
        & ~(0xf << (4 * PIN_OUTPUT_Y - 32)))
        | (0xa << (4 * PIN_OUTPUT_X - 32))  // AF output, 2 MHz
        | (0xa << (4 * PIN_OUTPUT_Y - 32))  // AF output, 2 MHz
        ;

    GPIOB->CRL = (GPIOB->CRL
        & ~(0xf << (4 * PIN_PEN_STATE)))
        | (0x6 << (4 * PIN_PEN_STATE)) // OD output, 2 MHz
        ;

    TIM1->CCMR1 = TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1
        | TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1;
    TIM1->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E;
    TIM1->BDTR = TIM_BDTR_MOE;
    TIM1->ARR = (1 << OUTPUT_PWM_RESOLUTION) - 1;
    TIM1->DIER = TIM_DIER_UIE;
    TIM1->CR1 = TIM_CR1_CEN;
    NVIC_EnableIRQ(TIM1_UP_IRQn);
}

void TIM1_UP_IRQHandler(void)
{
    Output_Tick();

    TIM1->SR &= ~TIM_SR_UIF;
}