project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
c13f0f7866
commit
46a2a8c103
5 changed files with 106 additions and 40 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 56
      components/tmc/ic/ztmc4361A.cpp
  3. 12
      components/tmc/ic/ztmc4361A.hpp
  4. 47
      components/tmc/ic/ztmc5130.cpp
  5. 13
      components/tmc/ic/ztmc5130.hpp

18
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -578,18 +578,18 @@ void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t
}
void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) {
memset(&cfg, 0, sizeof(cfg));
cfg.base_param.distance_scale = 1000;
cfg.base_param.distance_scale = 10000;
cfg.base_param.ihold = 0;
cfg.base_param.irun = 8;
cfg.base_param.iholddelay = 100;
cfg.base_param.acc = 3000000;
cfg.base_param.dec = 3000000;
cfg.base_param.maxspeed = 500000;
cfg.base_param.run_to_zero_move_to_zero_max_d = 51200 * 1000;
cfg.base_param.run_to_zero_leave_from_zero_max_d = 51200 * 10;
cfg.base_param.run_to_zero_speed = 100000;
cfg.base_param.run_to_zero_dec = 3000000;
cfg.base_param.acc = 300;
cfg.base_param.dec = 300;
cfg.base_param.maxspeed = 800;
cfg.base_param.run_to_zero_move_to_zero_max_d = 10000 * 100;
cfg.base_param.run_to_zero_leave_from_zero_max_d = 10000 * 2;
cfg.base_param.run_to_zero_speed = 100;
cfg.base_param.run_to_zero_dec = 600;
}
int32_t StepMotorCtrlModule::flush() {

56
components/tmc/ic/ztmc4361A.cpp
View File

@ -141,8 +141,8 @@ void TMC4361A::setAcceleration(float accelerationpps2) {
* AMAX [pps2] = AMAX / 237 ?? fCLK^2
*/
accelerationpps2 *= m_scale;
int32_t acc = (int32_t)accelerationpps2;
accelerationpps2 = to_motor_acc(accelerationpps2);
int32_t acc = (int32_t)accelerationpps2;
writeInt(TMC4361A_AMAX, acc << 2);
}
void TMC4361A::setDeceleration(float accelerationpps2) {
@ -156,8 +156,8 @@ void TMC4361A::setDeceleration(float accelerationpps2) {
* a[?v per clk_cycle]= AMAX / 2^37
* AMAX [pps2] = AMAX / 237 ?? fCLK^2
*/
accelerationpps2 *= m_scale;
int32_t acc = (int32_t)accelerationpps2;
accelerationpps2 = to_motor_acc(accelerationpps2);
int32_t acc = (int32_t)accelerationpps2;
writeInt(TMC4361A_DMAX, acc << 2);
}
#define INIT_GPIO(m_pin, pin, ...) \
@ -201,7 +201,7 @@ void TMC4361A::initialize(cfg_t *cfg) {
driverIC_setIHOLD_IRUN(1, 3, 0);
}
void TMC4361A::setScale(float scale) { m_scale = scale; }
void TMC4361A::setScale(int32_t scale) { m_scale = scale; }
uint8_t TMC4361A::reset() {
// Pulse the low-active hardware reset pin
@ -247,13 +247,13 @@ uint8_t TMC4361A::reset() {
}
uint8_t TMC4361A::restore() { return 1; }
int32_t TMC4361A::getXACTUAL() { return readInt(TMC4361A_XACTUAL) / m_scale; }
void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, value * m_scale); }
int32_t TMC4361A::getVACTUAL() { return readInt(TMC4361A_VACTUAL) / m_scale; }
int32_t TMC4361A::getXTARGET() { return readInt(TMC4361A_X_TARGET) / m_scale; }
int32_t TMC4361A::getENC_POS() { return readInt(TMC4361A_ENC_POS) / m_scale; }
void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, value * m_scale); }
int32_t TMC4361A::getENC_POS_DEV() { return readInt(TMC4361A_ENC_POS_DEV_RD) / m_scale; }
int32_t TMC4361A::getXACTUAL() { return to_user_pos(readInt(TMC4361A_XACTUAL)); }
void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, to_motor_pos(value)); }
int32_t TMC4361A::getVACTUAL() { return to_user_pos(readInt(TMC4361A_VACTUAL)); }
int32_t TMC4361A::getXTARGET() { return to_user_pos(readInt(TMC4361A_X_TARGET)); }
int32_t TMC4361A::getENC_POS() { return to_user_pos(readInt(TMC4361A_ENC_POS)); }
void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, to_motor_pos(value)); }
int32_t TMC4361A::getENC_POS_DEV() { return to_user_pos(readInt(TMC4361A_ENC_POS_DEV_RD)); }
void TMC4361A::enableIC(bool enable) {
SET_PIN(m_ennPin, !enable);
SET_PIN(m_driverIC_ennPin, !enable);
@ -266,15 +266,19 @@ int32_t tmc4361A_discardVelocityDecimals(int32_t value) {
return value << 8;
}
void TMC4361A::rotate(int32_t velocity) {
velocity *= m_scale;
// velocity *= m_scale;
velocity = to_motor_vel(velocity);
m_motor_mode = kvelmode;
PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0);
writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity));
}
void TMC4361A::stop() { rotate(0); }
void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) {
position *= m_scale;
velocityMax *= m_scale;
// position *= m_scale;
// velocityMax *= m_scale;
position = to_motor_pos(position);
velocityMax = to_motor_vel(velocityMax);
m_motor_mode = kposmode;
PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1);
@ -387,6 +391,28 @@ void TMC4361A::setRightVirtualLimitSwitch(bool enable, int32_t position) {
writeInt(TMC4361A_VIRT_STOP_RIGHT, position);
}
int32_t TMC4361A::to_motor_acc(int32_t acc) { //
int32_t val = acc / 60.0 * 51200;
return val;
}
int32_t TMC4361A::to_motor_vel(int32_t vel) { //
int32_t val = vel / 60.0 * 51200;
return val;
} // rpm
int32_t TMC4361A::to_motor_pos(int32_t pos) { //
int32_t val = pos * 1.0 / m_scale * 51200.0;
return val;
} //
int32_t TMC4361A::to_user_pos(int32_t pos) { //
int32_t val = pos / 51200.0 * m_scale;
return 0;
} //
int32_t TMC4361A::to_user_vel(int32_t vel) { //
int32_t val = vel * 60.0 / 51200.0;
return 0;
}
#endif
#endif

12
components/tmc/ic/ztmc4361A.hpp
View File

@ -74,7 +74,7 @@ class TMC4361A : public IStepperMotor {
motor_mode_t m_motor_mode = kvelmode;
float m_scale = 1.0;
int32_t m_scale = 10000; // 10000һȦ
public:
TMC4361A(/* args */);
@ -116,7 +116,7 @@ class TMC4361A : public IStepperMotor {
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); }
virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); }
virtual void setScale(float scale);
virtual void setScale(int32_t scale);
// Left Virtual Limit Switch XACTUAL ¡Ü VIRT_STOP_LEFT ʱ´¥·¢
void setLeftVirtualLimitSwitch(bool enable, int32_t position);
@ -172,6 +172,14 @@ class TMC4361A : public IStepperMotor {
int32_t driverIC_readInt(uint8_t address);
void driverIC_setMotorShaft(bool reverse);
uint32_t driverIC_readICVersion();
private:
int32_t to_motor_acc(int32_t acc); // rpm/s^2
int32_t to_motor_vel(int32_t vel); // rpm
int32_t to_motor_pos(int32_t pos); //
int32_t to_user_pos(int32_t pos); //
int32_t to_user_vel(int32_t vel);
};
} // namespace iflytop

47
components/tmc/ic/ztmc5130.cpp
View File

@ -70,8 +70,8 @@ void TMC5130::initialize(cfg_t *cfg) {
writeInt(TMC5130_VSTOP, 10);
writeInt(TMC5130_TZEROWAIT, 1000);
setAcceleration(100000);
setDeceleration(100000);
setAcceleration(100);
setDeceleration(100);
setIHOLD_IRUN(2, 10, 1);
enableIC(true);
}
@ -97,11 +97,11 @@ uint8_t TMC5130::reset() {
}
return 0;
}
int32_t TMC5130::getXACTUAL() { return readInt(TMC5130_XACTUAL) / m_scale; }
void TMC5130::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, value * m_scale); }
int32_t TMC5130::getVACTUAL() { return readInt(TMC5130_VACTUAL) / m_scale; }
void TMC5130::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(accelerationpps2 * m_scale)); } // 设置最大加速度
void TMC5130::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(accelerationpps2 * m_scale)); } // 设置最大减速度
int32_t TMC5130::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); }
void TMC5130::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, to_motor_pos(value)); }
int32_t TMC5130::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); }
void TMC5130::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大加速度
void TMC5130::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大减速度
void TMC5130::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); }
void TMC5130::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT)); }
uint32_t TMC5130::readICVersion() {
@ -115,15 +115,18 @@ Tmc5130RampStat TMC5130::getTMC5130_RAMPSTAT2() {
}
void TMC5130::stop() { rotate(0); }
void TMC5130::rotate(int32_t velocity) {
velocity *= m_scale;
// velocity *= m_scale;
to_motor_vel(velocity);
writeInt(TMC5130_VMAX, abs(velocity));
writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG);
}
void TMC5130::right(int32_t velocity) { rotate(velocity); }
void TMC5130::left(int32_t velocity) { rotate(-velocity); }
void TMC5130::moveTo(int32_t position, uint32_t velocityMax) {
position *= m_scale;
velocityMax *= m_scale;
// position *= m_scale;
// velocityMax *= m_scale;
to_motor_pos(position);
to_motor_vel(velocityMax);
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
writeInt(TMC5130_VMAX, velocityMax);
writeInt(TMC5130_XTARGET, position);
@ -205,4 +208,26 @@ int32_t TMC5130::readInt(uint8_t address) {
return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
}
#endif
#endif
int32_t TMC5130::to_motor_acc(int32_t acc) { //
int32_t val = acc / 60.0 * 51200;
return val;
}
int32_t TMC5130::to_motor_vel(int32_t vel) { //
int32_t val = vel / 60.0 * 51200;
return val;
} // rpm
int32_t TMC5130::to_motor_pos(int32_t pos) { //
int32_t val = pos * 1.0 / m_scale * 51200.0;
return val;
} //
int32_t TMC5130::to_user_pos(int32_t pos) { //
int32_t val = pos / 51200.0 * m_scale;
return 0;
} //
int32_t TMC5130::to_user_vel(int32_t vel) { //
int32_t val = vel * 60.0 / 51200.0;
return 0;
}

13
components/tmc/ic/ztmc5130.hpp
View File

@ -76,10 +76,10 @@ class TMC5130 : public IStepperMotor {
cfg_t m_cfg;
ZGPIO *m_csnpin = NULL;
ZGPIO *m_ennpin = NULL;
ZGPIO *m_csnpin = NULL;
ZGPIO *m_ennpin = NULL;
ZGPIO *m_spi_mode_select_gpio = NULL;
SPI_HandleTypeDef *m_hspi = NULL;
SPI_HandleTypeDef *m_hspi = NULL;
// uint8_t m_channel;
uint32_t m_lastCallPeriodicJobTick;
@ -149,6 +149,13 @@ class TMC5130 : public IStepperMotor {
private:
uint32_t haspassedms(uint32_t now, uint32_t last);
int32_t to_motor_acc(int32_t acc); // rpm/s^2
int32_t to_motor_vel(int32_t vel); // rpm
int32_t to_motor_pos(int32_t pos); //
int32_t to_user_pos(int32_t pos); //
int32_t to_user_vel(int32_t vel);
};
} // namespace iflytop
#endif
Write Preview
Loading…
Cancel
Save