95 lines
1.7 KiB
C++
95 lines
1.7 KiB
C++
/*
|
|
* 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();
|
|
}
|
|
|
|
}
|