#include <stdio.h>

#include "cpu.h"
#include "board.h"
#include "xtimer.h"
#include "periph/pwm.h"
#include "periph/gpio.h"
#include "tb6612fng.h"

#define PWM1        PWM_DEV(0)
#define PWM2	    PWM_DEV(1)

#define MOTOR1_CHANNEL     1
#define MOTOR1_IN1     GPIO_PIN(PORT_E,0)
#define MOTOR1_IN2     GPIO_PIN(PORT_E,2)

#define MOTOR2_CHANNEL     1
#define MOTOR2_IN1     GPIO_PIN(PORT_E,1)
#define MOTOR2_IN2     GPIO_PIN(PORT_E,3)

#define STBY	GPIO_PIN(PORT_D,4)

static motor_t left;
static motor_t right;


int main(void)
{
    int res;
    puts("\nRIOT dc motor test");
    puts("Connect an dc motor or scope to PWM_0/1 channel 1 to see anything");

    res = motor_init(&left,PWM1,MOTOR1_CHANNEL,MOTOR1_IN1,MOTOR1_IN2,STBY,1);
    if (res < 0) {
        puts("Errors while initializing motor");
        return -1;
    }
    puts("motor 1 initialized.");

   res = motor_init(&right,PWM2,MOTOR2_CHANNEL,MOTOR2_IN1,MOTOR2_IN2,STBY,1);
    if (res < 0) {
        puts("Errors while initializing motor");
        return -1;
    }
    puts("motor 2 initialized.");

    while (1) {
        motor_forward(&left,&right,100);
        xtimer_sleep(1);
	motor_back(&left,&right,100);
	xtimer_sleep(1);
    }

    return 0;
}