/*
 * This program drives a stepper motor using half-step.
 * 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 continous current.
 * 
 * The PWM inputs are connected to PTA 5, 12.
 * The enable pins of the half-bridge are connected to PTC 6-3. 
 * These pins are shared with the LEDs.
 *
 * The four output pins of the motor driver device are connected to
 * the 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.
 */

#include "MKL25Z4.h"

void delayMs(int n);

int main(void)
{
    const char halfsteps[ ] = {0x9, 0x1, 0x5, 0x4, 0x6, 0x2, 0xa, 0x8};
    int i = 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 */
    PORTC->PCR[5] = 0x100;      /* make PTC5 pin as GPIO */
    PORTC->PCR[6] = 0x100;      /* make PTC6 pin as GPIO */
    PTC->PDDR |= 0x78;          /* make PTC6-3 as output pin */
    
    /* enable PTA 5, 12 as output */
    SIM->SCGC5 |= 0x200;        /* enable clock to Port A */
    PORTA->PCR[5] = 0x100;      /* make PTA5 pin as GPIO */
    PORTA->PCR[12] = 0x100;     /* make PTA12 pin as GPIO */
    PTA->PDDR |= 1 << 5 | 1 << 12;  /* make PTA5, 12 as output pin */
    PTA->PDOR |= 1 << 5 | 1 << 12;  /* make PTA5, 12 high to enable motor drive */
    
    while(1)
    {
        PTC->PDOR &= ~(0x0F << 3);       /* clear motor enable pins */
        PTC->PDOR |= halfsteps[i++ & 7] << 3;  /* set the motor enable pins */
        delayMs(100);
    }
}

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