/*
 * This program drives a standard servo motor.
 *
 * The M1PWM7 is used to generate a 50Hz signal
 * with pulse width between 1ms and 2ms.
 * The quad half-H-bridge motor driver (TB6612FNG) of EduBase
 * board is configured so that the output of the PWM is also
 * available at M4 terminal.
 *
 * PWM1 - PB0 - TIM3_CH3 - AF2
 * PWM2 - PB1 - TIM3_CH4 - AF2
 * PWM3 - PC6 - TIM3_CH1 - AF2
 * PWM4 - PC7 - TIM3_CH2 - AF2

 */

#include "stm32f4xx.h"

#define SERVO 4 /* choose number 1 - 4 to select the servo control output */

void delayMs(int n);

int main(void) {
    int pw = 0;

#if (SERVO == 1)
    RCC->AHB1ENR |=  2;             /* enable GPIOB clock */
    GPIOB->AFR[0] &= ~0x0000000F;
    GPIOB->AFR[0] |=  0x00000002;   /* PB0 pin for TIM3 */
    GPIOB->MODER  &= ~0x00000003;
    GPIOB->MODER  |=  0x00000002;   /* use alternate function */
#elif (SERVO == 2)
    RCC->AHB1ENR |=  2;             /* enable GPIOB clock */
    GPIOB->AFR[0] &= ~0x000000F0;
    GPIOB->AFR[0] |=  0x00000020;   /* PB1 pin for TIM3 */
    GPIOB->MODER  &= ~0x0000000C;
    GPIOB->MODER  |=  0x00000008;   /* use alternate function */
#elif (SERVO == 3)
    RCC->AHB1ENR |=  4;             /* enable GPIOC clock */
    GPIOC->AFR[0] &= ~0x0F000000;
    GPIOC->AFR[0] |=  0x02000000;   /* PC6 pin for TIM3 */
    GPIOC->MODER  &= ~0x00003000;
    GPIOC->MODER  |=  0x00002000;   /* use alternate function */
#elif (SERVO == 4)
    RCC->AHB1ENR |=  4;             /* enable GPIOC clock */
    GPIOC->AFR[0] &= ~0xF0000000;
    GPIOC->AFR[0] |=  0x20000000;   /* PC7 pin for TIM3 */
    GPIOC->MODER  &= ~0x0000C000;
    GPIOC->MODER  |=  0x00008000;   /* use alternate function */
#endif

    /* setup TIM3 */
    RCC->APB1ENR |= 2;              /* enable TIM3 clock */
    TIM3->PSC = 10 - 1;            /* divided by 10 */
    TIM3->ARR = 32000 - 1;           /* divided by 3200 */
    TIM3->CNT = 0;                  /* clear counter */

#if (SERVO == 1)
    TIM3->CCMR2 = 0x0060;           /* PWM mode */
    TIM3->CCER = 0x0500;            /* enable PWM Ch3N and output */
    TIM3->CCR3 = 3200;
#elif (SERVO == 2)
    TIM3->CCMR2 = 0x6000;           /* PWM mode */
    TIM3->CCER = 0x5000;            /* enable PWM Ch4N and output */
    TIM3->CCR4 = 3200;
#elif (SERVO == 3)
    TIM3->CCMR1 = 0x0060;           /* PWM mode */
    TIM3->CCER = 0x0005;            /* enable PWM Ch4N and output */
    TIM3->CCR1 = 3200;
#elif (SERVO == 4)
    TIM3->CCMR1 = 0x6000;           /* PWM mode */
    TIM3->CCER = 0x0050;            /* enable PWM Ch4N and output */
    TIM3->CCR2 = 3200;
#endif

    TIM3->CR1 = 1;                  /* enable timer */

    while(1) {
        for (pw = 3200; pw > 1600; pw -= 20) {
#if (SERVO == 1)
            TIM3->CCR3 = pw;
#elif (SERVO == 2)
            TIM3->CCR4 = pw;
#elif (SERVO == 3)
            TIM3->CCR1 = pw;
#elif (SERVO == 4)
            TIM3->CCR2 = pw;
#endif
            delayMs(50);
        }

        for (pw = 1600; pw < 3200; pw += 20) {
#if (SERVO == 1)
            TIM3->CCR3 = pw;
#elif (SERVO == 2)
            TIM3->CCR4 = pw;
#elif (SERVO == 3)
            TIM3->CCR1 = pw;
#elif (SERVO == 4)
            TIM3->CCR2 = pw;
#endif
            delayMs(50);
        }
    }
}

/* 16 MHz SYSCLK */
void delayMs(int n) {
    int i;
    for (; n > 0; n--)
        for (i = 0; i < 3195; i++) ;
}