Clear servo alarms at startup

ChrGri · Aug 2, 2024 · fcde7c4 · fcde7c4
1 parent 2dc60c0
commit fcde7c4
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 0 deletions.
diff --git a/ESP32/include/isv57communication.h b/ESP32/include/isv57communication.h
@@ -53,6 +53,7 @@ class isv57communication {
     void readServoStates();
     bool checkCommunication();
     bool findServosSlaveId();
+    bool clearServoAlarms();
 
 
 

diff --git a/ESP32/src/Main.cpp b/ESP32/src/Main.cpp
@@ -313,6 +313,9 @@ void setup()
     ESP.restart();
   }
 
+  // reset iSV57 alarms
+  bool servoAlarmsCleared = isv57.clearServoAlarms();
+  delay(500);
 
   Serial.print("iSV57 communication state:  ");
   Serial.println(isv57LifeSignal_b);

diff --git a/ESP32/src/isv57communication.cpp b/ESP32/src/isv57communication.cpp
@@ -207,3 +207,18 @@ void isv57communication::readServoStates() {
   }
 
 }
+
+
+
+bool isv57communication::clearServoAlarms() {
+
+  // read the alarm list
+  int8_t numberOfRegistersToRead_u8 = 0;
+  // Alarm register address: 0x02
+  //int bytesReceived_i = modbus.requestFrom(slaveId, 0x03, 0x02, numberOfRegistersToRead_u8);
+
+  // clear alarm list
+  modbus.holdingRegisterWrite(slaveId, 0x019a, 0x7777); 
+
+  return 1;
+}