From 82bd75bea1affe841ab4b3d71eecd57721b65f8f Mon Sep 17 00:00:00 2001 From: ChrGri Date: Mon, 2 Oct 2023 15:40:02 +0200 Subject: [PATCH] Added intial version of iSV57 communication files --- Arduino/Esp32/Main/Main.ino | 54 ++++++++++ Arduino/Esp32/Main/isv57communication.cpp | 116 ++++++++++++++++++++++ Arduino/Esp32/Main/isv57communication.h | 61 ++++++++++++ 3 files changed, 231 insertions(+) create mode 100644 Arduino/Esp32/Main/isv57communication.cpp create mode 100644 Arduino/Esp32/Main/isv57communication.h diff --git a/Arduino/Esp32/Main/Main.ino b/Arduino/Esp32/Main/Main.ino index f4b25197..1a4984cb 100644 --- a/Arduino/Esp32/Main/Main.ino +++ b/Arduino/Esp32/Main/Main.ino @@ -1,4 +1,17 @@ #define ESTIMATE_LOADCELL_VARIANCE +//#define ISV_COMMUNICATION +//#define PRINT_SERVO_STATES + + + +#ifdef ISV_COMMUNICATION + #include "isv57communication.h" + isv57communication isv57; +#endif + + + + #include "Main.h" @@ -72,8 +85,13 @@ ForceCurve_Interpolated forceCurve; #define STACK_SIZE_FOR_TASK_1 0.2 * (configTOTAL_HEAP_SIZE / 4) #define STACK_SIZE_FOR_TASK_2 0.2 * (configTOTAL_HEAP_SIZE / 4) + TaskHandle_t Task1; TaskHandle_t Task2; +#ifdef ISV_COMMUNICATION + #define STACK_SIZE_FOR_TASK_3 0.2 * (configTOTAL_HEAP_SIZE / 4) + TaskHandle_t Task3; +#endif static SemaphoreHandle_t semaphore_updateConfig=NULL; bool configUpdateAvailable = false; // semaphore protected data @@ -293,6 +311,24 @@ void setup() +#ifdef ISV_COMMUNICATION + isv57::init(); + isv57::setupServoStateReading(); + isv57::sendTunedServoParameters(); + + xTaskCreatePinnedToCore( + servoCommunicationTask, + "servoCommunicationTask", + 10000, + //STACK_SIZE_FOR_TASK_2, + NULL, + 1, + &Task2, + 1); + delay(500); +#endif + + Serial.println("Setup end!"); @@ -728,3 +764,21 @@ void serialCommunicationTask( void * pvParameters ) } } + + + + + + +void servoCommunicationTask( void * pvParameters ) +{ + for(;;){ + isv57::readServoStates(); + + #ifdef PRINT_SERVO_STATES + static RTDebugOutput rtDebugFilter({ "servo_pos_given_p", "servo_pos_error_p", "servo_current_percent"}); + rtDebugFilter.offerData({ isv57.servo_pos_given_p, isv57.servo_pos_error_p, isv57.servo_current_percent}); + #endif + + } +} diff --git a/Arduino/Esp32/Main/isv57communication.cpp b/Arduino/Esp32/Main/isv57communication.cpp new file mode 100644 index 00000000..dd610895 --- /dev/null +++ b/Arduino/Esp32/Main/isv57communication.cpp @@ -0,0 +1,116 @@ +#include "isv57communication.h" + + +// declare variables +byte raw[200]; +uint8_t len; + + + +// initialize the communication +void isv57communication::init() +{ + Modbus modbus(Serial1); + Serial1.begin(38400, SERIAL_8N1, RXPIN, TXPIN, true); // Modbus serial + modbus.init(MODE); +} + + +// send tuned servo parameters +void isv57communication::setupServoStateReading() { + + // The iSV57 has four registers (0x0191, 0x0192, 0x0193, 0x0194) in which we can write, which values we want to obtain cyclicly + // These registers can be obtained by sending e.g. the command: 0x63, 0x03, 0x0191, target_sate, CRC + // tell the modbus slave, which registers will be read cyclicly + modbus.holdingRegisterWrite(slaveId, 0x0191, reg_add_position_given_p); + delay(50); + modbus.holdingRegisterWrite(slaveId, 0x0192, reg_add_position_error_p); + delay(50); + modbus.holdingRegisterWrite(slaveId, 0x0193, reg_add_velocity_current_feedback_percent); + delay(50); + modbus.holdingRegisterWrite(slaveId, 0x0194, reg_add_velocity_current_given_percent); + delay(50); + +} + + + +// send tuned servo parameters +void isv57communication::sendTunedServoParameters() { + + // servo config update + modbus.holdingRegisterWrite(slaveId, pr_0_00+2, 0); // deactivate auto gain + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_0_00+3, 10); // machine stiffness + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_0_00+4, 80); // ratio of inertia + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_0_00+8, 1600); // microsteps + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_0_00+14, 500); // position deviation setup + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_1_00+0, 20); // 1st position gain + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_1_00+1, 20); // 1st velocity loop gain + delay(50); + modbus.holdingRegisterWrite(slaveId, pr_1_00+15, 0); // control switching mode + delay(50); + + + + // store the settings to servos NVM + if (0) + { + modbus.holdingRegisterWrite(slaveId, 0x019A, 0x5555); // store the settings to servos NVM + delay(2000); + } + + + +} + + + + +// read servo states +void isv57communication::readServoStates() { + + // read the four registers + for (uint8_t regIdx = 0; regIdx < 4; regIdx++) + { + regArray[regIdx] = modbus.holdingRegisterRead(slaveId, ref_cyclic_read_0 + regIdx, 2); + + if(modbus.requestFrom(slaveId, 0x03, ref_cyclic_read_0 + regIdx, 1) > 0) + { + modbus.RxRaw(raw, len); + regArray[regIdx] = modbus.uint16(0); + } + delay(5); + } + + // write to public variables + servo_pos_given_p = regArray[0]; + servo_pos_error_p = regArray[1]; + servo_current_percent = regArray[2]; + + + + // print registers + if (0) + { + Serial.print("Pos_given:"); + Serial.print(regArray[0]); + + Serial.print(",Pos_error:"); + Serial.print(regArray[1]); + + Serial.print(",Cur_given:"); + Serial.print(regArray[2]); + + Serial.print(",Cur_fb:"); + Serial.print(regArray[3]); + + Serial.println(" "); + } + +} diff --git a/Arduino/Esp32/Main/isv57communication.h b/Arduino/Esp32/Main/isv57communication.h new file mode 100644 index 00000000..25c75c7a --- /dev/null +++ b/Arduino/Esp32/Main/isv57communication.h @@ -0,0 +1,61 @@ +//#include +#include "Modbus.h" + +// define modbus stuff +#define TXPIN 27 //17 +#define RXPIN 26 // 16 +#define MODE 5 + + + + + +// servo states register addresses +#define reg_add_position_given_p 0x0001 // checked +#define reg_add_position_feedback_p 0x0002 // checked +#define reg_add_position_error_p 0x0003 // checked +#define reg_add_command_position_given_p 0x0004 // checked +#define reg_add_position_relative_error_p 0x0005 // checked +#define reg_add_velocity_given_rpm 0x0040 // checked +#define reg_add_velocity_feedback_rpm 0x0041 // checked +#define reg_add_velocity_error_rpm 0x0042 // checked +#define reg_add_velocity_feedback_no_filt_rpm 0x0048 // checked +#define reg_add_position_command_velocity_rpm 0x0049 // checked +#define reg_add_velocity_current_given_percent 0x0080 // checked +#define reg_add_velocity_current_feedback_percent 0x0081 // checked + +#define ref_cyclic_read_0 0x01F3 +#define ref_cyclic_read_1 0x01F4 +#define ref_cyclic_read_2 0x01F5 +#define ref_cyclic_read_3 0x01F6 + +// servo parameter addresses +#define pr_0_00 0x0000 // reserved parameter +#define pr_1_00 0x0000 + 25 // 1st position gain +#define pr_2_00 pr_1_00 + 40 // adaptive filter mode setup +#define pr_3_00 pr_2_00 + 30 // velocity control +#define pr_4_00 pr_3_00 + 30 // velocity torque control +#define pr_5_00 pr_4_00 + 50 // extension settings +#define pr_6_00 pr_5_00 + 40 // special settings + + +#define slaveId 63 + + +class isv57communication { + + public: + void init(); + void setupServoStateReading(); + void sendTunedServoParameters(); + void readServoStates(); + + int16_t regArray[4]; + + int16_t servo_pos_given_p = 0; + int16_t servo_pos_error_p = 0; + int16_t servo_current_percent = 0; + + + +}