Dummy-Robot/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/Ctrl/Driver/tb67h450_base.cpp

80 lines
2.0 KiB
C++

#include "tb67h450_base.h"
#include "sin_map.h"
void TB67H450Base::Init()
{
InitGpio();
InitPwm();
}
void TB67H450Base::SetFocCurrentVector(uint32_t _directionInCount, int32_t _current_mA)
{
phaseB.sinMapPtr = (_directionInCount) & (0x000003FF);
phaseA.sinMapPtr = (phaseB.sinMapPtr + (256)) & (0x000003FF);
phaseA.sinMapData = sin_pi_m2[phaseA.sinMapPtr];
phaseB.sinMapData = sin_pi_m2[phaseB.sinMapPtr];
uint32_t dac_reg = abs(_current_mA);
dac_reg = (uint32_t) (dac_reg * 5083) >> 12;
dac_reg = dac_reg & (0x00000FFF);
phaseA.dacValue12Bits =
(uint32_t) (dac_reg * abs(phaseA.sinMapData)) >> sin_pi_m2_dpiybit;
phaseB.dacValue12Bits =
(uint32_t) (dac_reg * abs(phaseB.sinMapData)) >> sin_pi_m2_dpiybit;
SetTwoCoilsCurrent(phaseA.dacValue12Bits, phaseB.dacValue12Bits);
if (phaseA.sinMapData > 0)
SetInputA(true, false);
else if (phaseA.sinMapData < 0)
SetInputA(false, true);
else
SetInputA(true, true);
if (phaseB.sinMapData > 0)
SetInputB(true, false);
else if (phaseB.sinMapData < 0)
SetInputB(false, true);
else
SetInputB(true, true);
}
void TB67H450Base::SetTwoCoilsCurrent(uint16_t _currentA_3300mAIn12Bits, uint16_t _currentB_3300mAIn12Bits)
{
/*
* After SetFocCurrentVector calculation a 12bits value was mapped to 0~3300mA.
* And due to used 0.1Ohm shank resistor, 0~3300mV V-ref means 0~3300mA CurrentSetPoint,
* For more details, see TB67H450 Datasheet page.10 .
*/
DacOutputVoltage(_currentA_3300mAIn12Bits, _currentB_3300mAIn12Bits);
}
void TB67H450Base::Sleep()
{
phaseA.dacValue12Bits = 0;
phaseB.dacValue12Bits = 0;
SetTwoCoilsCurrent(phaseA.dacValue12Bits, phaseB.dacValue12Bits);
SetInputA(false, false);
SetInputB(false, false);
}
void TB67H450Base::Brake()
{
phaseA.dacValue12Bits = 0;
phaseB.dacValue12Bits = 0;
SetTwoCoilsCurrent(phaseA.dacValue12Bits, phaseB.dacValue12Bits);
SetInputA(true, true);
SetInputB(true, true);
}