#include "HardwareInfo.c"
#include "JMLib.c"
#include <SetDisplayString.h>
#include <SetPIDMotorRef.h>
#include <SetDisplayVar.h>
#include <SetPIDMotor.h>
#include <SetWaitForTime.h>

int main(void)
{
    E2RCU_Init();
    int speed = 100;
    int cnt = 1;
    SetDisplayString(1, "PID Motor Test", YELLOW, BLACK);
    SetPIDMotorRef(180);
    while (1)
    {
        SetDisplayVar(1, cnt, YELLOW, BLACK);
        SetPIDMotor(_M1_, speed);
        SetPIDMotor(_M2_, speed);
        SetPIDMotor(_M3_, speed);
        SetPIDMotor(_M4_, speed);
        SetWaitForTime(1);
        SetPIDMotor(_M1_, 0);
        SetPIDMotor(_M2_, 0);
        SetPIDMotor(_M3_, 0);
        SetPIDMotor(_M4_, 0);
        SetWaitForTime(1);
        cnt=2;speed=-speed;
        SetDisplayVar(1, cnt, YELLOW, BLACK);
        SetPIDMotor(_M1_, speed);
        SetPIDMotor(_M2_, speed);
        SetPIDMotor(_M3_, speed);
        SetPIDMotor(_M4_, speed);
        SetWaitForTime(1);
        SetPIDMotor(_M1_, 0);
        SetPIDMotor(_M2_, 0);
        SetPIDMotor(_M3_, 0);
        SetPIDMotor(_M4_, 0);
        cnt=1;speed=-speed;
        SetWaitForTime(1);
    }
    while(1);
}

