Update diagnostics/BatteryDiagnostics.py

diagnostics/BatteryDiagnostics.py

@@ -0,0 +1,55 @@
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.tools import run_task, multitask
+from pybricks.tools import wait, StopWatch
+from pybricks.robotics import DriveBase
+from pybricks.hubs import PrimeHub
+import umath
+# Initialize hub and devices
+hub = PrimeHub()
+class BatteryDiagnostics:
+    def __init__(self):
+        self.voltage = 0
+        self.current = 0
+    def printVoltage(self):
+        self.voltage = hub.battery.voltage()
+        if self.voltage > 7800:
+            print(f"Battery voltage is sufficient: {self.voltage}")
+        elif self.voltage < 7800 :
+            print(f"Charging needed: {self.voltage}")
+    def printCurrent(self):
+        self.current = hub.battery.current()
+        print("Battery current:", self.current)
+    def printAll(self):
+        timeelapsed = 0
+        voltageList = []
+        currentList = []
+        while True:
+            print("\n\n\n\n\n")
+            self.printVoltage()
+            voltageList.append(self.voltage)
+            self.printCurrent()
+            currentList.append(self.current)
+            wait(50)
+            timeelapsed += 50
+            
+            if(timeelapsed >= 3000):
+                break
+        print("--------------FINAL RESULTS OF BATTERY DIAGNOSTICS---------------")
+        print("Voltage deviation:", self.stdev(voltageList))
+        print("Current deviation:", self.stdev(currentList))
+    def stdev(self, vals):
+        data = vals
+        if len(data) < 2:
+            return 0
+        # Calculate the mean
+        mean = sum(data) / len(data)
+
+        # Calculate the variance (sum of squared differences from the mean, divided by n-1 for sample standard deviation)
+        variance = sum([(x - mean) ** 2 for x in data]) / float(len(data) - 1)
+        
+        # Calculate the standard deviation (square root of the variance)
+        std_dev_manual = umath.sqrt(variance)
+        
+
+        return (std_dev_manual)