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

fix some bug

master
zhaohe 2 years ago
parent
13227d8499
commit
372a2f158e
4 changed files with 48 additions and 15 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. 57
      components/mini_servo_motor/feite_servo_motor.cpp
  2. 2
      components/mini_servo_motor/feite_servo_motor.hpp
  3. 2
      components/zcancmder/zcanreceiver_master.cpp
  4. 2
      components/zcancmder/zcanreceiver_master.hpp

57
components/mini_servo_motor/feite_servo_motor.cpp
View File

@ -26,8 +26,10 @@ void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hd
m_uart = uart;
m_hdma_rx = hdma_rx;
m_hdma_tx = hdma_tx;
m_mutex.init();
}
bool FeiTeServoMotor::ping(uint8_t id) {
zlock_guard l(m_mutex);
ping_cmd_t ping_cmd;
ping_resp_t ping_resp;
ping_cmd.header = 0xffff;
@ -53,8 +55,14 @@ static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) {
return calibrate;
}
bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) { return write_u8(id, kRegServoRunMode, (uint8_t)runmode); }
bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) { return read_s16(id, kRegServoCalibration, 11, poscalibration); }
bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) {
zlock_guard l(m_mutex);
return write_u8(id, kRegServoRunMode, (uint8_t)runmode);
}
bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) {
zlock_guard l(m_mutex);
return read_s16(id, kRegServoCalibration, 11, poscalibration);
}
run_mode_e FeiTeServoMotor::getmode(uint8_t id) {
uint8_t data = 0;
@ -67,22 +75,33 @@ run_mode_e FeiTeServoMotor::getmode(uint8_t id) {
}
bool FeiTeServoMotor::getmode(uint8_t id, run_mode_e& runmode) {
uint8_t data = 0;
bool suc = read_u8(id, kRegServoRunMode, data);
runmode = (run_mode_e)data;
zlock_guard l(m_mutex);
uint8_t data = 0;
bool suc = read_u8(id, kRegServoRunMode, data);
runmode = (run_mode_e)data;
return suc;
}
bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) { return write_u8(id, kRegServoTorqueSwitch, on ? 1 : 0); }
bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) {
zlock_guard l(m_mutex);
return write_u8(id, kRegServoTorqueSwitch, on ? 1 : 0);
}
bool FeiTeServoMotor::getTorqueSwitch(uint8_t id, bool& on) {
uint8_t data = 0;
bool suc = read_u8(id, kRegServoTorqueSwitch, data);
on = data;
zlock_guard l(m_mutex);
uint8_t data = 0;
bool suc = read_u8(id, kRegServoTorqueSwitch, data);
on = data;
return suc;
}
bool FeiTeServoMotor::getNowPos(uint8_t id, int16_t& pos) { return read_s16(id, kRegServoCurrentPos, 15, pos); }
bool FeiTeServoMotor::setTargetPos(uint8_t id, int16_t pos) { return write_s16(id, kRegServoTargetPos, 15, pos); }
bool FeiTeServoMotor::getNowPos(uint8_t id, int16_t& pos) {
zlock_guard l(m_mutex);
return read_s16(id, kRegServoCurrentPos, 15, pos);
}
bool FeiTeServoMotor::setTargetPos(uint8_t id, int16_t pos) {
zlock_guard l(m_mutex);
return write_s16(id, kRegServoTargetPos, 15, pos);
}
bool FeiTeServoMotor::triggerAysncWrite(uint8_t id) {
cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf;
@ -97,6 +116,7 @@ bool FeiTeServoMotor::triggerAysncWrite(uint8_t id) {
}
bool FeiTeServoMotor::rotate(uint8_t id, int16_t speed, uint16_t torque) {
zlock_guard l(m_mutex);
DO(setmode(id, kMotorMode));
if (torque == 0) torque = 1000;
DO(write_u16(id, kRegServoTorqueLimit, torque));
@ -105,6 +125,7 @@ bool FeiTeServoMotor::rotate(uint8_t id, int16_t speed, uint16_t torque) {
}
bool FeiTeServoMotor::moveTo(uint8_t id, int16_t pos, int16_t speed, uint16_t torque) {
zlock_guard l(m_mutex);
/**
* @brief ÉèÖÃŤ¾Ø
*/
@ -117,7 +138,7 @@ bool FeiTeServoMotor::moveTo(uint8_t id, int16_t pos, int16_t speed, uint16_t to
DO(setTargetPos(id, pos));
return true;
}
uint16_t abs16(int16_t val) {
static uint16_t abs16(int16_t val) {
if (val < 0) {
return -val;
} else {
@ -125,6 +146,7 @@ uint16_t abs16(int16_t val) {
}
}
bool FeiTeServoMotor::moveWithTorque(uint8_t id, int16_t torque) {
zlock_guard l(m_mutex);
DO(setmode(id, kOpenMotorMode));
if (torque == 0) torque = 1000;
DO(write_u16(id, kRegServoTorqueLimit, abs16(torque)));
@ -144,13 +166,15 @@ static int16_t tosign16(uint16_t* d, int signoff) {
bool FeiTeServoMotor::read_status(uint8_t id, status_t* status) {
// kRegServoCurrentPos
bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)status, sizeof(status_t));
status->vel = tosign16((uint16_t*)&status->vel, 15);
zlock_guard l(m_mutex);
bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)status, sizeof(status_t));
status->vel = tosign16((uint16_t*)&status->vel, 15);
if (!suc) return false;
return true;
}
bool FeiTeServoMotor::read_detailed_status(uint8_t id, detailed_status_t* detailed_status) {
bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)detailed_status, sizeof(*detailed_status));
zlock_guard l(m_mutex);
bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)detailed_status, sizeof(*detailed_status));
if (!suc) return false;
detailed_status->vel = tosign16((uint16_t*)&detailed_status->vel, 15);
detailed_status->torque = tosign16((uint16_t*)&detailed_status->torque, 10);
@ -270,6 +294,7 @@ bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbi
}
bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) { //
// ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len);
zlock_guard l(m_mutex);
cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf;
receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf;
@ -300,6 +325,8 @@ bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* da
}
bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) {
// return false;
zlock_guard l(m_mutex);
cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf;
receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf;
cmd_header->header = 0xffff;

2
components/mini_servo_motor/feite_servo_motor.hpp
View File

@ -125,6 +125,8 @@ class FeiTeServoMotor {
uint8_t m_txbuf[256] = {0};
uint8_t m_rxbuf[256] = {0};
zmutex m_mutex;
public:
void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx);
bool ping(uint8_t id);

2
components/zcancmder/zcanreceiver_master.cpp
View File

@ -33,6 +33,7 @@ void ZCanCommnaderMaster::init(CFG *cfg) {
HAL_StatusTypeDef hal_status;
m_config = cfg;
m_on_packet_map_lock.init();
txlock.init();
/**
* @brief ³õʼ»¯CAN
@ -279,6 +280,7 @@ uint16_t ZCanCommnaderMaster::generateFreeIndex() {
}
void ZCanCommnaderMaster::sendPacket(uint8_t *packet, size_t len) {
zlock_guard txlock_guard(txlock);
/**
* @brief
*/

2
components/zcancmder/zcanreceiver_master.hpp
View File

@ -87,6 +87,8 @@ class ZCanCommnaderMaster : public ZCanIRQListener {
zmutex m_on_packet_map_lock;
uint16_t m_index_off = 0;
zmutex txlock;
public:
ZCanCommnaderMaster() {}
CFG *createCFG();

Write Preview
Loading…
Cancel
Save