95 lines
2.2 KiB
C++
95 lines
2.2 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)
|
|
{
|
|
__HAL_TIM_SET_COMPARE(_left.forward.timer, _left.forward.channel, left);
|
|
__HAL_TIM_SET_COMPARE(_left.backward.timer, _left.backward.channel, 0);
|
|
}
|
|
else
|
|
{
|
|
__HAL_TIM_SET_COMPARE(_left.forward.timer, _left.forward.channel, 0);
|
|
__HAL_TIM_SET_COMPARE(_left.backward.timer, _left.backward.channel, (-1 * left));
|
|
}
|
|
|
|
if (right > 0)
|
|
{
|
|
__HAL_TIM_SET_COMPARE(_right.forward.timer, _right.forward.channel, right);
|
|
__HAL_TIM_SET_COMPARE(_right.backward.timer, _right.backward.channel, 0);
|
|
}
|
|
else
|
|
{
|
|
__HAL_TIM_SET_COMPARE(_right.forward.timer, _right.forward.channel, 0);
|
|
__HAL_TIM_SET_COMPARE(_right.backward.timer, _right.backward.channel, (-1 * right));
|
|
}
|
|
}
|
|
|
|
void DualDrvMotors::Coast()
|
|
{
|
|
__HAL_TIM_SET_COMPARE(_left.forward.timer, _left.forward.channel, 0);
|
|
__HAL_TIM_SET_COMPARE(_left.backward.timer, _left.backward.channel, 0);
|
|
|
|
__HAL_TIM_SET_COMPARE(_right.forward.timer, _right.forward.channel, 0);
|
|
__HAL_TIM_SET_COMPARE(_right.backward.timer, _right.backward.channel, 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();
|
|
}
|
|
|
|
}
|