126 lines
3.5 KiB
C++
126 lines
3.5 KiB
C++
#include "CuoZhiMotor.h"
|
|
#include <random>
|
|
|
|
CuoZhiMotor::CuoZhiMotor()
|
|
: Motor(motorPorts_Cuozhi)
|
|
{
|
|
auto t_smbtype = MotorConfig::MTBDType::MT_TMC;//SMBType::MB_DRV_TMC216;//smbtype == SMBType::MB_DRV_TMC216 ? MotorConfig::MTBDType::MT_TMC : MotorConfig::MTBDType::MT_DRV;
|
|
m_cuoparamex = m_mtconfig->GetMotorSpeedParam(false, t_smbtype, 4, 1, 1);
|
|
speedConfig();
|
|
mspCuozhiFeeding = {.finalPeriod = 1427500, .Fmin = 2027500, .stepnum = 30, .a = 100, .offset = 4, .finalDelay = 3000};
|
|
//if (smbtype == SMBType::MB_DRV_TMC216)
|
|
{
|
|
mspCuozhiFeeding.finalPeriod /= 4;
|
|
mspCuozhiFeeding.Fmin /= 4;
|
|
}
|
|
delays_feeding = speedup_cfg(mspCuozhiFeeding.finalPeriod, mspCuozhiFeeding.Fmin, mspCuozhiFeeding.stepnum, mspCuozhiFeeding.a,
|
|
mspCuozhiFeeding.offset, mspCuozhiFeeding.finalDelay);
|
|
}
|
|
|
|
CuoZhiMotor::~CuoZhiMotor()
|
|
{
|
|
}
|
|
|
|
void CuoZhiMotor::pauseWaitForThread()
|
|
{
|
|
if (thStart.joinable())
|
|
{
|
|
thStart.join();
|
|
}
|
|
pause();
|
|
}
|
|
|
|
void CuoZhiMotor::startAsyn()
|
|
{
|
|
if (thStart.joinable())
|
|
{
|
|
thStart.join();
|
|
}
|
|
thStart = std::thread(&CuoZhiMotor::forward, this);
|
|
}
|
|
|
|
void CuoZhiMotor::start()
|
|
{
|
|
forward();
|
|
}
|
|
|
|
void CuoZhiMotor::reset()
|
|
{
|
|
if (thStart.joinable())
|
|
{
|
|
thStart.join();
|
|
}
|
|
|
|
setDirection(1);
|
|
delays_forward.clear();
|
|
delays_forward = speedup_cfg(mspCuozhiBackward.finalPeriod,
|
|
mspCuozhiBackward.Fmin,
|
|
mspCuozhiBackward.stepnum,
|
|
mspCuozhiBackward.a,
|
|
mspCuozhiBackward.offset,
|
|
mspCuozhiBackward.finalDelay);
|
|
printf("finalPeriod=%d Fmin=%d \n", mspCuozhiBackward.finalPeriod, mspCuozhiBackward.Fmin);
|
|
Motor::start(delays_forward, mspCuozhiBackward);
|
|
std::random_device rd; // obtain a random number from hardware
|
|
std::mt19937 gen(rd()); // seed the generator
|
|
std::uniform_int_distribution<> distr(100, 400); // define the range
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(distr(gen)));
|
|
stop();
|
|
delays_backward.clear();
|
|
delays_backward = speedup_cfg(mspCuozhiBackward.finalPeriod, mspCuozhiBackward.Fmin, mspCuozhiBackward.stepnum, mspCuozhiBackward.a,
|
|
mspCuozhiBackward.offset, mspCuozhiBackward.finalDelay);
|
|
setDirection(0);
|
|
Motor::start(delays_backward, mspCuozhiBackward);
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(150));
|
|
|
|
stop();
|
|
speedRecover();
|
|
}
|
|
|
|
void CuoZhiMotor::forward()
|
|
{
|
|
setDirection(1);
|
|
Motor::start(delays_forward, m_cuoparamex.mt_param);
|
|
}
|
|
|
|
void CuoZhiMotor::backward()
|
|
{
|
|
setDirection(0);
|
|
Motor::start(delays_backward, mspCuozhiBackward);
|
|
}
|
|
|
|
void CuoZhiMotor::feeding()
|
|
{
|
|
setDirection(1);
|
|
Motor::start(delays_feeding, mspCuozhiFeeding);
|
|
}
|
|
|
|
void CuoZhiMotor::speedChange(MotorSpeedParamEx paramex)
|
|
{
|
|
m_cuoparamex = paramex;
|
|
speedConfig();
|
|
}
|
|
|
|
void CuoZhiMotor::speedRecover()
|
|
{
|
|
speedConfig();
|
|
}
|
|
|
|
void CuoZhiMotor::speedConfig()
|
|
{
|
|
delays_forward.clear();
|
|
delays_forward = speedup_cfg(m_cuoparamex.mt_param.finalPeriod, m_cuoparamex.mt_param.Fmin, m_cuoparamex.mt_param.stepnum, m_cuoparamex.mt_param.a,
|
|
m_cuoparamex.mt_param.offset, m_cuoparamex.mt_param.finalDelay);
|
|
std::vector<int> vct_speedupdon(delays_forward);
|
|
|
|
int size= vct_speedupdon.size();
|
|
int half_len = vct_speedupdon.size()/3;
|
|
for (int i = size-1; i > 0; i--)
|
|
{
|
|
delays_forward.push_back(vct_speedupdon[i]);
|
|
}
|
|
delays_backward.clear();
|
|
delays_backward = speedup_cfg(mspCuozhiBackward.finalPeriod, mspCuozhiBackward.Fmin, mspCuozhiBackward.stepnum, mspCuozhiBackward.a,
|
|
mspCuozhiBackward.offset, mspCuozhiBackward.finalDelay);
|
|
} |