/*
 * peripherals_motors.cpp
 *
 *  Created on: Feb 24, 2021
 *      Author: erki
 */

#include "peripherals_motors.hpp"

namespace Peripherals
{

DualDrvMotors::DualDrvMotors(const IMotors::TwoChannelMotorData& left,
		const IMotors::TwoChannelMotorData& right, const IO& sleep_pin)
  : _left(left)
  , _right(right)
  , _sleep_pin(sleep_pin)
{
	_left.forward.Enable();
	_left.backward.Enable();
	_right.forward.Enable();
	_right.backward.Enable();

	Set(0, 0);
}

void DualDrvMotors::Set(const std::int16_t left, const std::int16_t right)
{
	if (left > 0)
	{
		_left.forward.SetCompare(left);
		_left.backward.SetCompare(0);
	}
	else
	{
		_left.forward.SetCompare(0);
		_left.backward.SetCompare(-1 * left);
	}

	if (right > 0)
	{
		_right.forward.SetCompare(right);
		_right.backward.SetCompare(0);
	}
	else
	{
		_right.forward.SetCompare(0);
		_right.backward.SetCompare(-1 * right);
	}
}

void DualDrvMotors::Coast()
{
	_left.forward.SetCompare(0);
	_left.backward.SetCompare(0);

	_right.forward.SetCompare(0);
	_right.backward.SetCompare(0);
}

void DualDrvMotors::Break()
{
	Set(0, 0);

	_left.forward.Disable();
	_left.backward.Disable();
	_right.forward.Disable();
	_right.backward.Disable();

	_left.forward.PinToGpio();
	_left.backward.PinToGpio();
	_right.forward.PinToGpio();
	_right.backward.PinToGpio();

	_left.forward.pin.Set(true);
	_left.backward.pin.Set(true);
	_right.forward.pin.Set(true);
	_right.backward.pin.Set(true);
}

void DualDrvMotors::Unbreak()
{
	_left.forward.PinToPwm();
	_left.backward.PinToPwm();
	_right.forward.PinToPwm();
	_right.backward.PinToPwm();

	_left.forward.Enable();
	_left.backward.Enable();
	_right.forward.Enable();
	_right.backward.Enable();
}

}