Added intial version of iSV57 communication files

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<int16_t, 3> 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
+
+  }
+}

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(" "); 
+  }
+
+}

Arduino/Esp32/Main/isv57communication.h

@@ -0,0 +1,61 @@
+//#include <SoftwareSerial.h>
+#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;
+
+
+
+}