diff --git a/README.md b/README.md index 735dc9c..e13f4a4 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ M5 stack library for cybergear * M5Stack Basic V2.7 -## H/W Components +## H/W Components (MCP2515) * [Xiaomi Cybergear](https://www.mi.com/cyber-gear) * [M5Stack Basic V2.7](https://shop.m5stack.com/collections/m5-controllers/products/esp32-basic-core-lot-development-kit-v2-7) @@ -20,10 +20,16 @@ M5 stack library for cybergear * [XT30(2+2)-F](https://www.china-amass.com/product/contain/1Yf5h7G4u1927079) * [Grove Cable](https://www.seeedstudio.com/Grove-Universal-4-Pin-Buckled-20cm-Cable-5-PCs-pack.html) -## H/W Connection - ![image](https://github.com/project-sternbergia/cybergear_m5/assets/147309062/c36d82cf-e91a-45da-ac53-a79e8d8fc730) +## H/W Components (ESP32 + CAN Transceiver Unit) + +* [Xiaomi Cybergear](https://www.mi.com/cyber-gear) +* [M5Stack Basic V2.7](https://shop.m5stack.com/collections/m5-controllers/products/esp32-basic-core-lot-development-kit-v2-7) +* [LAN Module W5500 with PoE V12](https://shop.m5stack.com/products/lan-module-w5500-with-poe-v12) or [CANBus Unit(CA-IS3050G)](https://shop.m5stack.com/products/canbus-unitca-is3050g) +* [XT30(2+2)-F](https://www.china-amass.com/product/contain/1Yf5h7G4u1927079) +* [Grove Cable](https://www.seeedstudio.com/Grove-Universal-4-Pin-Buckled-20cm-Cable-5-PCs-pack.html) + ## How to use Official GUI tool This software requires a specific CAN to USB module. @@ -47,6 +53,7 @@ Tested(for reference): cd ~/Arduino/libraries git clone https://github.com/coryjfowler/MCP_CAN_lib.git git clone https://github.com/Locoduino/RingBuffer.git +git clone git@github.com:project-sternbergia/arduino-CAN.git git clone https://github.com/project-sternbergia/cybergear_m5.git ``` @@ -84,7 +91,7 @@ After that write [cybergear_m5/examples/cybergear_bilateral.ino](https://github. ## References -* Xiaomi Cybergear 微电机使用说明书 +* [Xiaomi Cybergear 微电机使用说明书](https://web.vip.miui.com/page/info/mio/mio/detail?postId=40233100) ## LICENSE diff --git a/examples/dump_ram_test/dump_ram_test.ino b/examples/dump_ram_test/dump_ram_test.ino new file mode 100644 index 0000000..8ce69ca --- /dev/null +++ b/examples/dump_ram_test/dump_ram_test.ino @@ -0,0 +1,72 @@ +#include +#include +#include "cybergear_driver.hh" + +#define USE_ESP32_CAN +#ifdef USE_ESP32_CAN +#include "cybergear_can_interface_esp32.hh" +#else +#include "cybergear_can_interface_mcp.hh" +#endif + +// setup master can id and motor can id (default cybergear can id is 0x7F) +uint8_t MASTER_CAN_ID = 0x00; +uint8_t MOT_CAN_ID = 0x7F; + +// init cybergeardriver +CybergearDriver driver = CybergearDriver(MASTER_CAN_ID, MOT_CAN_ID); + +#ifdef USE_ESP32_CAN +CybergearCanInterfaceEsp32 interface; +#else +CybergearCanInterfaceMcp interface; +#endif + +void setup() +{ + M5.begin(true, false, true); + + // init cybergear driver + M5.Lcd.printf("Start read write motor param test\n"); + + interface.init(); + driver.init(&interface); + driver.init_motor(MODE_POSITION); + delay(1000); + driver.enable_motor(); +} + +void loop() +{ + M5.update(); + + // get motor parameter + driver.set_position_ref(0.0f); + driver.dump_motor_param(); + driver.process_packet(); + + MotorParameter param = driver.get_motor_param(); + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setCursor(0, 0); + M5.Lcd.printf("Received motor param\n"); + M5.Lcd.printf(" stamp : %u\n", param.stamp_usec); + M5.Lcd.printf(" run_mode : %d\n", param.run_mode); + M5.Lcd.printf(" iq_ref : %f\n", param.iq_ref); + M5.Lcd.printf(" spd_ref : %f\n", param.spd_ref); + M5.Lcd.printf(" limit_torque : %f\n", param.limit_torque); + M5.Lcd.printf(" cur_kp : %f\n", param.cur_kp); + M5.Lcd.printf(" cur_ki : %f\n", param.cur_ki); + M5.Lcd.printf(" cur_filt_gain : %f\n", param.cur_filt_gain); + M5.Lcd.printf(" loc_ref : %f\n", param.loc_ref); + M5.Lcd.printf(" limit_spd : %f\n", param.limit_spd); + M5.Lcd.printf(" limit_cur : %f\n", param.limit_cur); + M5.Lcd.printf(" mech_pos : %f\n", param.mech_pos); + M5.Lcd.printf(" iqf : %f\n", param.iqf); + M5.Lcd.printf(" mech_vel : %f\n", param.mech_vel); + M5.Lcd.printf(" vbus : %f\n", param.vbus); + M5.Lcd.printf(" rotation : %d\n", param.rotation); + M5.Lcd.printf(" loc_kp : %f\n", param.loc_kp); + M5.Lcd.printf(" spd_kp : %f\n", param.spd_kp); + M5.Lcd.printf(" spd_ki : %f\n", param.spd_ki); + delay(5000); +} diff --git a/src/cybergear_can_interface_esp32.cpp b/src/cybergear_can_interface_esp32.cpp index 5fa6124..5ec23d9 100644 --- a/src/cybergear_can_interface_esp32.cpp +++ b/src/cybergear_can_interface_esp32.cpp @@ -56,6 +56,7 @@ bool CybergearCanInterfaceEsp32::init(uint8_t rx_pin, uint8_t tx_pin) bool CybergearCanInterfaceEsp32::send_message(uint32_t id, const uint8_t *data, uint8_t len, bool ext) { + CG_DEBUG_FUNC // change packet type if (ext) { CAN.beginExtendedPacket(id); @@ -71,6 +72,7 @@ bool CybergearCanInterfaceEsp32::send_message(uint32_t id, const uint8_t *data, bool CybergearCanInterfaceEsp32::read_message(unsigned long& id, uint8_t *data, uint8_t& len) { + CG_DEBUG_FUNC // check empty if (buffer.isEmpty()) return false; @@ -87,5 +89,6 @@ bool CybergearCanInterfaceEsp32::read_message(unsigned long& id, uint8_t *data, bool CybergearCanInterfaceEsp32::available() { + CG_DEBUG_FUNC return (buffer.isEmpty() == false); } diff --git a/src/cybergear_can_interface_mcp.cpp b/src/cybergear_can_interface_mcp.cpp index a153236..c94d75d 100644 --- a/src/cybergear_can_interface_mcp.cpp +++ b/src/cybergear_can_interface_mcp.cpp @@ -14,14 +14,14 @@ bool CybergearCanInterfaceMcp::init(uint8_t cs_pin, uint8_t int_pin) { if (pcan_ != nullptr) { - CG_DEBUG_PRINLN("Failed to open can intetrface. CAN interface has already opened."); + CG_DEBUG_PRINTLN("Failed to open can intetrface. CAN interface has already opened."); return false; } pcan_ = new MCP_CAN(cs_pin); if (!pcan_->begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ) == CAN_OK) { - CG_DEBUG_PRINLN("Failed to open can intetrface."); + CG_DEBUG_PRINTLN("Failed to open can intetrface."); return false; } diff --git a/src/cybergear_controller.cpp b/src/cybergear_controller.cpp index 34b93e6..02ad500 100644 --- a/src/cybergear_controller.cpp +++ b/src/cybergear_controller.cpp @@ -3,7 +3,6 @@ #include "cybergear_driver.hh" #include "cybergear_driver_defs.hh" #include "cybergear_driver_utils.hh" -#include CybergearController::CybergearController(uint8_t master_can_id) : can_(NULL) diff --git a/src/cybergear_driver.cpp b/src/cybergear_driver.cpp index 58c4c4d..c1df53e 100644 --- a/src/cybergear_driver.cpp +++ b/src/cybergear_driver.cpp @@ -1,6 +1,7 @@ #include "cybergear_driver.hh" #include "cybergear_driver_defs.hh" #include "cybergear_driver_utils.hh" +#include #include #include @@ -90,7 +91,6 @@ void CybergearDriver::set_current_kp(float kp) void CybergearDriver::set_current_ki(float ki) { - // NOT SUPPOTED (no information about limit on manual) // write_float_data(target_can_id_, ADDR_CURRENT_KI, ki, 0.0f, KI_MAX); } @@ -119,6 +119,56 @@ void CybergearDriver::set_velocity_ki(float ki) write_float_data(target_can_id_, ADDR_SPD_KI, ki, 0.0f, 200.0f); } +void CybergearDriver::get_mech_position() +{ + read_ram_data(ADDR_MECH_POS); +} + +void CybergearDriver::get_mech_velocity() +{ + read_ram_data(ADDR_MECH_VEL); +} + +void CybergearDriver::get_vbus() +{ + read_ram_data(ADDR_VBUS); +} + +void CybergearDriver::get_rotation() +{ + read_ram_data(ADDR_ROTATION); +} + +void CybergearDriver::dump_motor_param() +{ + const std::vector index_array = { + ADDR_RUN_MODE, + ADDR_IQ_REF, + ADDR_SPEED_REF, + ADDR_LIMIT_TORQUE, + ADDR_CURRENT_KP, + ADDR_CURRENT_KI, + ADDR_CURRENT_FILTER_GAIN, + ADDR_LOC_REF, + ADDR_LIMIT_SPEED, + ADDR_LIMIT_CURRENT, + ADDR_MECH_POS, + ADDR_IQF, + ADDR_MECH_VEL, + ADDR_VBUS, + ADDR_ROTATION, + ADDR_LOC_KP, + ADDR_SPD_KP, + ADDR_SPD_KI + }; + + for (auto index : index_array) + { + read_ram_data(index); + delay(1); + } +} + void CybergearDriver::set_position_ref(float position) { write_float_data(target_can_id_, ADDR_LOC_REF, position, P_MIN, P_MAX); @@ -167,18 +217,24 @@ uint8_t CybergearDriver::get_motor_id() const bool CybergearDriver::process_packet() { + CG_DEBUG_FUNC bool check_update = false; while (true) { - uint8_t ret = receive_motor_data(motor_status_); - if (ret == RET_CYBERGEAR_OK) check_update = true; - else if (ret == RET_CYBERGEAR_MSG_NOT_AVAIL) break; + if (receive_motor_data(motor_status_)) + { + check_update = true; + } + else + { + break; + } } return check_update; } bool CybergearDriver::update_motor_status(unsigned long id, const uint8_t * data, unsigned long len) { - // if id is not mine + CG_DEBUG_FUNC uint8_t receive_can_id = id & 0xff; if ( receive_can_id != master_can_id_ ) { return false; @@ -191,29 +247,22 @@ bool CybergearDriver::update_motor_status(unsigned long id, const uint8_t * data // check packet type uint8_t packet_type = (id & 0x3F000000) >> 24; - if ( packet_type != CMD_RESPONSE ) { - return false; - } + if (packet_type == CMD_RESPONSE) { + process_motor_packet(data, len); - motor_status_.raw_position = data[1] | data[0] << 8; - motor_status_.raw_velocity = data[3] | data[2] << 8; - motor_status_.raw_effort = data[5] | data[4] << 8; - motor_status_.raw_temperature = data[7] | data[6] << 8; + } else if (packet_type == CMD_RAM_READ){ + process_read_parameter_packet(data, len); - // convert motor data - motor_status_.stamp_usec = micros(); - motor_status_.motor_id = motor_can_id; - motor_status_.position = uint_to_float(motor_status_.raw_position, P_MIN, P_MAX); - motor_status_.velocity = uint_to_float(motor_status_.raw_velocity, V_MIN, V_MAX); - motor_status_.effort = uint_to_float(motor_status_.raw_effort, T_MIN, T_MAX); - motor_status_.temperature = motor_status_.raw_temperature; + } else if (packet_type == CMD_GET_MOTOR_FAIL) { + // NOT IMPLEMENTED - return true; -} + } else { + CG_DEBUG_PRINTF("invalid command response [0x%x]\n", packet_type); + print_can_packet(id, data, len); + return false; + } -MotorStatus CybergearDriver::get_motor_status() const -{ - return motor_status_; + return true; } void CybergearDriver::write_float_data(uint8_t can_id, uint16_t addr, float value, float min, float max) @@ -251,54 +300,150 @@ void CybergearDriver::send_command(uint8_t can_id, uint8_t cmd_id, uint16_t opti ++send_count_; } -uint8_t CybergearDriver::receive_motor_data(MotorStatus & mot) +bool CybergearDriver::receive_motor_data(MotorStatus & mot) { // receive data unsigned long id; uint8_t len; if (!can_->read_message(id, receive_buffer_, len)) { - return RET_CYBERGEAR_MSG_NOT_AVAIL; + CG_DEBUG_PRINTLN("received data is not available"); + return false; } // if id is not mine uint8_t receive_can_id = id & 0xff; if ( receive_can_id != master_can_id_ ) { CG_DEBUG_PRINTF("Invalid master can id. Expected=[0x%02x] Actual=[0x%02x] Raw=[%x]\n", master_can_id_, receive_can_id, id); - return RET_CYBERGEAR_INVALID_CAN_ID; + return false; } uint8_t motor_can_id = (id & 0xff00) >> 8; if ( motor_can_id != target_can_id_ ) { CG_DEBUG_PRINTF("Invalid target can id. Expected=[0x%02x] Actual=[0x%02x] Raw=[%x]\n", target_can_id_, motor_can_id, id); - return RET_CYBERGEAR_INVALID_CAN_ID; + return false; } // parse packet -------------- + return update_motor_status(id, receive_buffer_, len); +} - // check packet type - uint8_t packet_type = (id & 0x3F000000) >> 24; - if ( packet_type != CMD_RESPONSE ) { - CG_DEBUG_PRINLN("invalid command response"); - return RET_CYBERGEAR_INVALID_PACKET; - } - - mot.raw_position = receive_buffer_[1] | receive_buffer_[0] << 8; - mot.raw_velocity = receive_buffer_[3] | receive_buffer_[2] << 8; - mot.raw_effort = receive_buffer_[5] | receive_buffer_[4] << 8; - mot.raw_temperature = receive_buffer_[7] | receive_buffer_[6] << 8; +void CybergearDriver::process_motor_packet(const uint8_t * data, unsigned long len) +{ + motor_status_.raw_position = data[1] | data[0] << 8; + motor_status_.raw_velocity = data[3] | data[2] << 8; + motor_status_.raw_effort = data[5] | data[4] << 8; + motor_status_.raw_temperature = data[7] | data[6] << 8; // convert motor data - mot.stamp_usec = micros(); - mot.motor_id = motor_can_id; - mot.position = uint_to_float(mot.raw_position, P_MIN, P_MAX); - mot.velocity = uint_to_float(mot.raw_velocity, V_MIN, V_MAX); - mot.effort = uint_to_float(mot.raw_effort, T_MIN, T_MAX); - mot.temperature = mot.raw_temperature; + motor_status_.stamp_usec = micros(); + motor_status_.motor_id = target_can_id_; + motor_status_.position = uint_to_float(motor_status_.raw_position, P_MIN, P_MAX); + motor_status_.velocity = uint_to_float(motor_status_.raw_velocity, V_MIN, V_MAX); + motor_status_.effort = uint_to_float(motor_status_.raw_effort, T_MIN, T_MAX); + motor_status_.temperature = motor_status_.raw_temperature; +} - return RET_CYBERGEAR_OK; +void CybergearDriver::process_read_parameter_packet(const uint8_t * data, unsigned long len) +{ + uint16_t index = data[1] << 8 | data[0]; + + uint8_t uint8_data; + memcpy(&uint8_data, &data[4], sizeof(uint8_t)); + + int16_t int16_data; + memcpy(&int16_data, &data[4], sizeof(int16_t)); + + float float_data; + memcpy(&float_data, &data[4], sizeof(float)); + + bool is_updated = true; + + switch (index) + { + case ADDR_RUN_MODE: + motor_param_.run_mode = uint8_data; + CG_DEBUG_PRINTF("Receive ADDR_RUN_MODE = [0x%02x]\n", uint8_data); + break; + case ADDR_IQ_REF: + motor_param_.iq_ref = float_data; + CG_DEBUG_PRINTF("Receive ADDR_IQ_REF = [%f]\n", float_data); + break; + case ADDR_SPEED_REF: + motor_param_.spd_ref = float_data; + CG_DEBUG_PRINTF("Receive ADDR_SPEED_REF = [%f]\n", float_data); + break; + case ADDR_LIMIT_TORQUE: + motor_param_.limit_torque = float_data; + CG_DEBUG_PRINTF("Receive ADDR_LIMIT_TORQUE = [%f]\n", float_data); + break; + case ADDR_CURRENT_KP: + motor_param_.cur_kp = float_data; + CG_DEBUG_PRINTF("Receive ADDR_CURRENT_KP = [%f]\n", float_data); + break; + case ADDR_CURRENT_KI: + motor_param_.cur_ki = float_data; + CG_DEBUG_PRINTF("Receive ADDR_CURRENT_KI = [%f]\n", float_data); + break; + case ADDR_CURRENT_FILTER_GAIN: + motor_param_.cur_filt_gain = float_data; + CG_DEBUG_PRINTF("Receive ADDR_CURRENT_FILTER_GAIN = [%f]\n", float_data); + break; + case ADDR_LOC_REF: + motor_param_.loc_ref = float_data; + CG_DEBUG_PRINTF("Receive ADDR_LOC_REF = [%f]\n", float_data); + break; + case ADDR_LIMIT_SPEED: + motor_param_.limit_spd = float_data; + CG_DEBUG_PRINTF("Receive ADDR_LIMIT_SPEED = [%f]\n", float_data); + break; + case ADDR_LIMIT_CURRENT: + motor_param_.limit_cur = float_data; + CG_DEBUG_PRINTF("Receive ADDR_LIMIT_CURRENT = [%f]\n", float_data); + break; + case ADDR_MECH_POS: + motor_param_.mech_pos = float_data; + CG_DEBUG_PRINTF("Receive ADDR_MECH_POS = [%f]\n", float_data); + break; + case ADDR_IQF: + motor_param_.iqf = float_data; + CG_DEBUG_PRINTF("Receive ADDR_IQF = [%f]\n", float_data); + break; + case ADDR_MECH_VEL: + motor_param_.mech_vel = float_data; + CG_DEBUG_PRINTF("Receive ADDR_MECH_VEL = [%f]\n", float_data); + break; + case ADDR_VBUS: + motor_param_.vbus = float_data; + CG_DEBUG_PRINTF("Receive ADDR_VBUS = [%f]\n", float_data); + break; + case ADDR_ROTATION: + motor_param_.rotation = int16_data; + CG_DEBUG_PRINTF("Receive ADDR_ROTATION = [%d]\n", int16_data); + break; + case ADDR_LOC_KP: + motor_param_.loc_kp = float_data; + CG_DEBUG_PRINTF("Receive ADDR_LOC_KP = [%f]\n", float_data); + break; + case ADDR_SPD_KP: + motor_param_.spd_kp = float_data; + CG_DEBUG_PRINTF("Receive ADDR_SPD_KP = [%f]\n", float_data); + break; + case ADDR_SPD_KI: + motor_param_.spd_ki = float_data; + CG_DEBUG_PRINTF("Receive ADDR_SPD_KI = [%f]\n", float_data); + break; + default: + CG_DEBUG_PRINTF("Unknown parameter value index=[0x%04x]\n", index); + is_updated = false; + break; + } + + if (is_updated) { + motor_param_.stamp_usec = micros(); + } } -void CybergearDriver::print_can_packet(uint32_t id, uint8_t *data, uint8_t len) +void CybergearDriver::print_can_packet(uint32_t id, const uint8_t *data, uint8_t len) { Serial.print("Id : "); Serial.print(id, HEX); diff --git a/src/cybergear_driver.hh b/src/cybergear_driver.hh index 31bce0a..f2a2f64 100644 --- a/src/cybergear_driver.hh +++ b/src/cybergear_driver.hh @@ -1,6 +1,7 @@ #ifndef CYBER_GEAR_DRIVER_H #define CYBER_GEAR_DRIVER_H +#include #include "cybergear_driver_defs.hh" #include "cybergear_can_interface.hh" @@ -22,6 +23,41 @@ struct MotorStatus }; +struct MotorFault +{ + bool encoder_not_calibrated; + bool over_current_phase_a; + bool over_current_phase_b; + bool over_voltage; + bool under_voltage; + bool driver_chip; + bool motor_over_tempareture; +}; + + +struct MotorParameter +{ + unsigned long stamp_usec; + uint16_t run_mode; + float iq_ref; + float spd_ref; + float limit_torque; + float cur_kp; + float cur_ki; + float cur_filt_gain; + float loc_ref; + float limit_spd; + float limit_cur; + float mech_pos; + float iqf; + float mech_vel; + float vbus; + int16_t rotation; + float loc_kp; + float spd_kp; + float spd_ki; +}; + /** * @brief Cybergear driver class */ @@ -130,10 +166,53 @@ public: */ void set_limit_torque(float torque); + /** + * @brief Set the position kp gain + * + * @param kp kp gain for position control. + */ void set_position_kp(float kp); + + /** + * @brief Set the velocity kp gain + * + * @param kp kp gain for velocity control. + */ void set_velocity_kp(float kp); + + /** + * @brief Set the velocity ki gain + * + * @param ki ki gain for velocity control. + */ void set_velocity_ki(float ki); + /** + * @brief Requeset mech position. This request response will store motor param struct. + */ + void get_mech_position(); + + /** + * @brief Requeset mech velocity. This request response will store motor param struct. + */ + void get_mech_velocity(); + + /** + * @brief Requeset vbus. This request response will store motor param struct. + */ + void get_vbus(); + + /** + * @brief Requeset rotation. This request response will store motor param struct. + */ + void get_rotation(); + + /** + * @brief Request motor parameters. This funciton takes param x 1msec for can response. please use this as a debug function. + * This request response will store motor param struct. + */ + void dump_motor_param(); + /** * @brief Set position reference for position control mode * @@ -213,8 +292,20 @@ public: * * @return MotorStatus motor status */ - MotorStatus get_motor_status() const; + MotorStatus get_motor_status() const { return motor_status_; } + /** + * @brief Get the motor param object + * + * @return MotorParameter motor param + */ + MotorParameter get_motor_param() const { return motor_param_; } + + /** + * @brief Get send count + * + * @return unsigned long total send count from this driver class. + */ unsigned long send_count() const { return send_count_; } private: @@ -254,7 +345,30 @@ private: */ void send_command(uint8_t can_id, uint8_t cmd_id, uint16_t option, uint8_t len, uint8_t * data); - uint8_t receive_motor_data(MotorStatus& mot); + /** + * @brief Receive motor data from can interface + * + * @param mot motor status + * @return true success to receive + * @return false failed to receive + */ + bool receive_motor_data(MotorStatus& mot); + + /** + * @brief process motor response packet + * + * @param data received data + * @param len data length + */ + void process_motor_packet(const uint8_t * data, unsigned long len); + + /** + * @brief process read parameter packet + * + * @param data received data + * @param len data length + */ + void process_read_parameter_packet(const uint8_t * data, unsigned long len); /** * @brief Print byte array as serial output @@ -262,7 +376,7 @@ private: * @param data data * @param len length */ - void print_can_packet(uint32_t id, uint8_t *data, uint8_t len); + void print_can_packet(uint32_t id, const uint8_t *data, uint8_t len); CybergearCanInterface *can_; //!< can connection instance @@ -271,7 +385,8 @@ private: uint8_t run_mode_; //!< run mode uint8_t receive_buffer_[64]; //!< receive buffer MotorStatus motor_status_; //!< current motor status - unsigned long send_count_; + MotorParameter motor_param_; //!< motor parameter + unsigned long send_count_; //!< send count }; #endif // !CYBER_GEAR_DRIVER_H diff --git a/src/cybergear_driver_defs.hh b/src/cybergear_driver_defs.hh index b2882ca..55777eb 100644 --- a/src/cybergear_driver_defs.hh +++ b/src/cybergear_driver_defs.hh @@ -11,9 +11,6 @@ #define CMD_RAM_WRITE 18 #define CMD_GET_MOTOR_FAIL 21 -#define ADDR_SPD_KP 0x2014 -#define ADDR_SPD_KI 0x2015 -#define ADDR_LOC_KP 0x2016 #define ADDR_RUN_MODE 0x7005 #define ADDR_IQ_REF 0x7006 #define ADDR_SPEED_REF 0x700A @@ -24,6 +21,14 @@ #define ADDR_LOC_REF 0x7016 #define ADDR_LIMIT_SPEED 0x7017 #define ADDR_LIMIT_CURRENT 0x7018 +#define ADDR_MECH_POS 0x7019 +#define ADDR_IQF 0x701A +#define ADDR_MECH_VEL 0x701B +#define ADDR_VBUS 0x701C +#define ADDR_ROTATION 0x701D +#define ADDR_LOC_KP 0x701E +#define ADDR_SPD_KP 0x701F +#define ADDR_SPD_KI 0x7020 #define MODE_MOTION 0x00 #define MODE_POSITION 0x01 @@ -45,12 +50,26 @@ #define CURRENT_FILTER_GAIN_MIN 0.0f #define CURRENT_FILTER_GAIN_MAX 1.0f +#define IQ_REF_MAX 23.0f +#define IQ_REF_MIN -23.0f +#define SPD_REF_MAX 30.0f +#define SPD_REF_MIN -30.0f +#define LIMIT_TORQUE_MAX 0.0f +#define LIMIT_TORQUE_MIN 12.0f +#define CUR_KP_MAX 200.0f +#define CUR_KP_MIN 0.0f +#define CUR_KI_MAX 200.0f +#define CUR_KI_MIN 0.0f +#define LIMIT_SPD_MAX 30.0f +#define LIMIT_SPD_MIN 0.0f + + #define DEFAULT_CURRENT_KP 0.125f #define DEFAULT_CURRENT_KI 0.0158f #define DEFAULT_CURRENT_FINTER_GAIN 0.1f #define DEFAULT_POSITION_KP 30.0f #define DEFAULT_VELOCITY_KP 2.0f -#define DEFAULT_VELOCITY_KI 0.021f +#define DEFAULT_VELOCITY_KI 0.002f #define DEFAULT_VELOCITY_LIMIT 2.0f #define DEFAULT_CURRENT_LIMIT 27.0f #define DEFAULT_TORQUE_LIMIT 12.0f diff --git a/src/cybergear_driver_utils.hh b/src/cybergear_driver_utils.hh index 9eb79aa..6611376 100644 --- a/src/cybergear_driver_utils.hh +++ b/src/cybergear_driver_utils.hh @@ -3,14 +3,16 @@ // #define CG_DEBUG #ifndef CG_DEBUG -#define CG_DEBUG_FUNC() +#define CG_DEBUG_FUNC #define CG_DEBUG_PRINTF(fmt, ...) -#define CG_DEBUG_PRINLN(msg) +#define CG_DEBUG_PRINTLN(msg) #else #include +#undef min +#undef max #define CG_DEBUG_FUNC Serial.printf("l%d %s\n", __LINE__, __func__); #define CG_DEBUG_PRINTF(fmt, ...) Serial.printf(fmt, __VA_ARGS__); -#define CG_DEBUG_PRINLN(msg) Serial.println(msg); +#define CG_DEBUG_PRINTLN(msg) Serial.println(msg); #endif #endif // CYBERGEAR_DRIVER_UTILS_HH