#include <cstdio>
#include "motor_controller.h"

const char* echoOff 		= "EO=0;";
const char* echoOn  		= "EO=1;";
const char* initialVelocity	= "JV=100;";
const char* motorOn 		= "MO=1;";
const char* motorOff 		= "MO=0;";
const char* motorStop 		= "ST;";
const char* beginMotion		= "BG;";
const char* baudRate57600	= "PP[4];";

MotorController::MotorController(void) : _aUart(*new USART_6)
{
	_aUart.Open(38400);
	_command(initialVelocity);
}

void MotorController::_command(const char* request)
{
	_aUart.Put(request);
	while (_aUart.Get() != ';');
}

void MotorController::Enable(void)
{
	_command(echoOff);
	_command(motorOn);
}

void MotorController::Disable(void)
{
	_command(motorOff);
	_command(echoOn);
}

void MotorController::Stop(void)
{
	_command(motorStop);
}

bool MotorController::UpdateVelocity(int32_t velocity)
{
	char commandString[32];
	sprintf(commandString,"JV=%d;",static_cast<int>(velocity));
	_command(commandString);
	_command(beginMotion);
	return true;
}