mirror of
https://gitee.com/peng_zhihui/Dummy-Robot
synced 2025-11-04 20:49:40 +08:00
80 lines
2.0 KiB
C++
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);
|
|
}
|
|
|
|
|