142 lines
4.2 KiB
C++
142 lines
4.2 KiB
C++
#include "Motor.h"
|
||
#include "DevUtil.h"
|
||
#include <vector>
|
||
#include <cmath>
|
||
#include <exception>
|
||
#include "utilsfunc.h"
|
||
#include "MotorConfig.h"
|
||
|
||
#define ZZ_PWM 3
|
||
#define CZ_PWM 2
|
||
|
||
struct MotorMode
|
||
{
|
||
int mod0 : 1;
|
||
int mod1 : 1;
|
||
int mod2 : 1;
|
||
};
|
||
|
||
// const MotorPorts motorPorts_Zouzhi = {.reset = ZouZhiMotor_Reset, .sleep = ZouZhiMotor_Sleep, .enable = ZouZhiMotor_Enable, .dir = ZouZhiMotor_Direction, .decay = ZouZhiMotor_Decay, .home = ZouZhiMotor_Home, .fault = ZouZhiMotor_Fault, .mode0 = ZouZhiMotor_Mode0, .mode1 = ZouZhiMotor_Mode1, .mode2 = ZouZhiMotor_Mode2, .pwm = ZZ_PWM};
|
||
|
||
// const MotorPorts motorPorts_Cuozhi = {.reset = CuoZhiMotor_Reset, .sleep = CuoZhiMotor_Sleep, .enable = CuoZhiMotor_Enable, .dir = CuoZhiMotor_Direction, .decay = CuoZhiMotor_Decay, .home = CuoZhiMotor_Home, .fault = CuoZhiMotor_Fault, .mode0 = CuoZhiMotor_Mode0, .mode1 = CuoZhiMotor_Mode1, .mode2 = CuoZhiMotor_Mode2, .pwm = CZ_PWM};
|
||
const MotorPorts motorPorts_Zouzhi = { .power = PIN_PORT_7010::MOTOR_POWER_1,
|
||
.reset = PIN_PORT_7010::ZOUZHI_PIN_RESET,
|
||
.sleep = PIN_PORT_7010::ZOUZHI_PIN_SLEEP,
|
||
.enable = PIN_PORT_7010::ZOUZHI_PIN_ENABEL,
|
||
.dir = PIN_PORT_7010::ZOUZHI_PIN_DIR,
|
||
.pwm = 1
|
||
};
|
||
|
||
const MotorPorts motorPorts_Cuozhi = { .power =PIN_PORT_7010::MOTOR_POWER_2,
|
||
.reset = PIN_PORT_7010::CUOZHI_PIN_RESET,
|
||
.sleep = PIN_PORT_7010::CUOZHI_PIN_SLEEP,
|
||
.enable = PIN_PORT_7010::CUOZHI_PIN_ENABEL,
|
||
.dir = PIN_PORT_7010::CUOZHI_PIN_DIR,
|
||
.pwm = 0
|
||
};
|
||
|
||
std::vector<int> speedup_cfg(int finalPeriod, int Fmin, int stepnum, int a, int offset, int finalDelay)
|
||
{
|
||
std::vector<int> freqs;
|
||
int period = 0;
|
||
double delay = ((double)finalDelay) / 1000000.0;
|
||
double b = stepnum * delay;
|
||
for (int i = 0; i < stepnum; i++)
|
||
{
|
||
period = (int)(finalPeriod + (Fmin - finalPeriod) / (1 + exp(-a * b + offset)));
|
||
b = b - delay;
|
||
freqs.push_back(period);
|
||
}
|
||
return freqs;
|
||
}
|
||
|
||
//GpioOut Motor::powerPin(MotorPower);
|
||
|
||
Motor::Motor(MotorPorts motorPorts):sleepPin(motorPorts.sleep),
|
||
resetPin(motorPorts.reset),
|
||
enablePin(motorPorts.enable),
|
||
dirPin(motorPorts.dir),
|
||
pwm(motorPorts.pwm),
|
||
motor_power(motorPorts.power)
|
||
{
|
||
sleepPin.setDirection(Gpio::out);
|
||
resetPin.setDirection(Gpio::out);
|
||
enablePin.setDirection(Gpio::out);
|
||
dirPin.setDirection(Gpio::out);
|
||
motor_power.setDirection(Gpio::out);
|
||
|
||
motor_power.setValue(Gpio::High); //0：关闭电机电源 1：开启电机电源 //默认一直开启
|
||
//默认设置 1 1 1
|
||
resetPin.setValue(Gpio::High); //0：重启 1：电机不重启
|
||
sleepPin.setValue(Gpio::High); //0：电机停止转动 1：电机开始转动
|
||
enablePin.setValue(Gpio::High); //0：电机芯片打开电源 1：电机芯片关闭电源
|
||
|
||
}
|
||
|
||
Motor::~Motor()
|
||
{
|
||
stop();
|
||
if (m_mtconfig.get())
|
||
m_mtconfig.reset();
|
||
}
|
||
|
||
void Motor::start(std ::vector<int> &delay_s, const MotorSpeedParam &msp)
|
||
{
|
||
enablePin.setValue(Gpio::Low);
|
||
pwm.enable(true);
|
||
if (!delay_s.empty())
|
||
{
|
||
std::vector<int>::iterator iter = delay_s.begin();
|
||
while (++iter != delay_s.end())
|
||
{
|
||
pwm.setFreq(PWM_PERIOD / (*iter));
|
||
std::this_thread::sleep_for(std::chrono::microseconds((int)msp.finalDelay));
|
||
}
|
||
}
|
||
pwm.setFreq(PWM_PERIOD / msp.finalPeriod);
|
||
//pwm.setFreq((PWM_PERIOD / msp.finalPeriod)/10*10);
|
||
//printf("\n-----pwm Freq = %d------\n",PWM_PERIOD / msp.finalPeriod/10*10);
|
||
}
|
||
|
||
void Motor::start()
|
||
{
|
||
enablePin.setValue(Gpio::Low);
|
||
pwm.enable(true);
|
||
}
|
||
|
||
void Motor::stop()
|
||
{
|
||
enablePin.setValue(Gpio::High);
|
||
pwm.enable(false);
|
||
}
|
||
|
||
void Motor::pause()
|
||
{
|
||
pwm.enable(false);
|
||
enablePin.setValue(Gpio::Low);
|
||
}
|
||
|
||
void Motor::setMode(int mode_input)
|
||
{
|
||
// MotorMode *mode = (MotorMode *)&mode_input;
|
||
// mode0.setValue((Gpio::GpioLevel)mode->mod0);
|
||
// mode1.setValue((Gpio::GpioLevel)mode->mod1);
|
||
// mode2.setValue((Gpio::GpioLevel)mode->mod2);
|
||
}
|
||
|
||
void Motor::setDirection(int dir)
|
||
{
|
||
dirPin.setValue((Gpio::GpioLevel)dir);
|
||
}
|
||
|
||
void Motor::setSpeed(int value)
|
||
{
|
||
pwm.setFreq(value);
|
||
}
|
||
|
||
void Motor::enablePower(bool bEnable)
|
||
{
|
||
LOG("Enable Motor PowerPin %s \n", bEnable ? "true" : "false");
|
||
//powerPin.setValue((Gpio::GpioLevel)bEnable);
|
||
}
|