/*
 * This program controls the rotational speed and direction of 
 * a DC motor.
 *
 * The Wytec EduBase board has a quad half-H-bridge motor driver
 * (TB6612FNG), which is capable of driving two DC motors
 * or a stepper motor with up to 15V and 1.2A continuous current.
 *
 * The four output pins of the motor driver device are connected to
 * the blue terminals marked M1, M2, M3, and M4.  Use of external
 * power supply for motor is recommended.  External power should be
 * connected to the terminal T3 and the jumper J4 should be at EXT.
 *
 * This program uses PORTC 3, 4 for direction control and PTA 12 for
 * PWM control.  The outputs are terminals M1 and M2.
 * The PWM is generated by TPM1_CH0.
 */

#include "MKL25Z4.h"

void delayMs(int n);

int main(void)
{
    int pw = 0;

    /* enable PTC 6-3 as output */
    SIM->SCGC5 |= 0x800;        /* enable clock to Port C */
    PORTC->PCR[3] = 0x100;      /* make PTC3 pin as GPIO */
    PORTC->PCR[4] = 0x100;      /* make PTC4 pin as GPIO */
    PTC->PDDR |= 0x18;          /* make PTC4-3 as output pin */
    PTC->PSOR = 1 << 3;
    PTC->PCOR = 1 << 4;
    
    /* enable PTA 12 as output of TPM1_CH0 */
    SIM->SCGC5 |= 0x200;        /* enable clock to Port A */
    PORTA->PCR[12] = 0x300;     /* make PTA12 pin as TPM1 output */

    /* enable TPM1_CH0 */
    SIM->SCGC6 |= 0x2000000u;   /* enable clock to TPM1 */
    SIM->SOPT2 |= 0x01000000;   /* use MCGFLLCLK as timer counter clock */
    TPM1->SC = 0;               /* disable timer */
    TPM1->CONTROLS[0].CnSC = 0x20 | 0x08;   /* edge-aligned, pulse high */
    TPM1->MOD = 4193;           /* Set up modulo register for 10kHz */
    TPM1->CONTROLS[0].CnV = 100;  /* Startup dutycycle */
    TPM1->SC = 0x08;            /* enable TPM1 */
    
    while(1)
    {
        // speed up
        for (pw = 100; pw < 4000; pw += 20)
        {
            TPM1->CONTROLS[0].CnV = pw;
            delayMs(50);
        }
        // slow down
        for (pw = 4000; pw >100; pw -= 20)
        {
            TPM1->CONTROLS[0].CnV = pw;
            delayMs(50);
        }
        // change dirction
        PTC->PCOR = 1 << 3;
        PTC->PSOR = 1 << 4;
        
        // speed up
        for (pw = 100; pw < 4000; pw += 20)
        {
            TPM1->CONTROLS[0].CnV = pw;
            delayMs(50);
        }
        // slow down
        for (pw = 4000; pw >100; pw -= 20)
        {
            TPM1->CONTROLS[0].CnV = pw;
            delayMs(50);
        }
        // change dirction
        PTC->PSOR = 1 << 3;
        PTC->PCOR = 1 << 4;
    }
}

// delay n milliseconds (41.94MHz CPU clock)
void delayMs(int n)
{
    int i, j;
    for(i = 0 ; i < n; i++)
        for(j = 0; j < 3180; j++)
            {}  // do nothing
}