add stopIfFault from code and put in library

functions belong in the library not the code (even if they work in the code)
photodude · Apr 18, 2015 · 580ed06 · 580ed06
1 parent eacbb0b
commit 580ed06
Showing 1 changed file with 25 additions and 0 deletions.
diff --git a/DualVNH5019MotorShieldMod3/DualVNH5019MotorShieldMod3.cpp b/DualVNH5019MotorShieldMod3/DualVNH5019MotorShieldMod3.cpp
@@ -403,3 +403,28 @@ unsigned char DualVNH5019MotorShieldMod3::getM4Fault()
 {
   return !digitalRead(_EN4DIAG4);
 }
+
+//stop if fault methods
+void stopIfFault()
+{
+  if (md.getM1Fault())
+  {
+    Serial.println("M1 fault");
+    while(1);
+  }
+  if (md.getM2Fault())
+  {
+    Serial.println("M2 fault");
+    while(1);
+  }
+    if (md.getM3Fault())
+  {
+    Serial.println("M3 fault");
+    while(1);
+  }
+    if (md.getM4Fault())
+  {
+    Serial.println("M4 fault");
+    while(1);
+  }
+}