From 8dd33181606b2af5fac93ddf5b1d994e83eb26a7 Mon Sep 17 00:00:00 2001
From: MostlyTurquoise <107848704+MostlyTurquoise@users.noreply.github.com>
Date: Sat, 18 Nov 2023 11:00:26 +0000
Subject: [PATCH] Exposed enable_12v and enable_5v Also added an enabled_5v
 value that is tracked within green giant similar to the exisiting enabled_12v

---
 robot/greengiant.py |  2 ++
 robot/wrapper.py    | 29 ++++++++++++++++++++++++-----
 2 files changed, 26 insertions(+), 5 deletions(-)

diff --git a/robot/greengiant.py b/robot/greengiant.py
index 4087866..0be6738 100644
--- a/robot/greengiant.py
+++ b/robot/greengiant.py
@@ -181,6 +181,7 @@ def __init__(self, bus):
         self._bus = bus
         self._version = self.get_version()
         self.enabled_12v = False
+        self.enabled_5v = False
         self.set_motor_power(self.enabled_12v)
 
     def enable_motors(self,new_state):
@@ -222,6 +223,7 @@ def set_5v_acc_power(self, new_state):
             # for GG this is always on, perhaps we should IOerror, for now just ignore
             # this is likely only ever used internally
             self._bus.write_byte_data(_GG_I2C_ADDR, _GG_ENABLE_5V_ACC, int(new_state))
+            self.enabled_5v = new_state
 
 
 
diff --git a/robot/wrapper.py b/robot/wrapper.py
index d323fd9..1c070c8 100644
--- a/robot/wrapper.py
+++ b/robot/wrapper.py
@@ -62,7 +62,10 @@ def __init__(self,
                  wait_for_start=True,
                  camera=None,
                  max_motor_voltage=6,
-                 logging_level=logging.INFO):
+                 logging_level=logging.INFO,
+                 start_enable_12v = True,
+                 start_enable_5v = True,
+                 ):
 
         self.zone = marker_setup.TEAM.RUSSET
         self.mode = "competition"
@@ -84,7 +87,7 @@ def __init__(self,
         except IOError:
             pass
 
-        self.subsystem_init(camera)
+        self.subsystem_init(camera, start_enable_12v, start_enable_5v)
         self.report_hardware_status()
         self.enable_12v = True
         type(self)._initialised = True
@@ -101,7 +104,7 @@ def __init__(self,
                            "`robot.wait_start`. Robot will not wait for the "
                            "start button until `robot.wait_start` is called.")
 
-    def subsystem_init(self, camera):
+    def subsystem_init(self, camera, start_enable_12v, start_enable_5v):
         """Allows for initalisation of subsystems after instansating `Robot()`
         Can only be called once"""
         if type(self)._initialised:
@@ -114,8 +117,8 @@ def subsystem_init(self, camera):
         if self._gg_version >= 10:
             # enable power rails
             self._green_giant.set_motor_power(True)
-            self._green_giant.set_12v_acc_power(True)    # Not sure, should this be controlled by user?
-            self._green_giant.set_5v_acc_power(True)
+            self._green_giant.set_12v_acc_power(start_enable_12v)    # Not sure, should this be controlled by user?
+            self._green_giant.set_5v_acc_power(start_enable_5v)
             self._adc_max = 5
             # configure User IO Ports
             self.servos = GreenGiantGPIOPinList(self.bus, self._gg_version, self._adc_max, _GG_SERVO_GPIO_BASE, _GG_SERVO_PWM_BASE)
@@ -211,6 +214,22 @@ def enable_motors(self, on):
         """An nice alias for set_12v"""
         if self._version < 10:
             return self._green_giant.enable_motors(on)
+        
+    @property
+    def enable_12v(self):
+        return self._green_giant.enabled_12v
+
+    @enable_12v.setter
+    def enable_12v(self, on):
+        self._green_giant.set_12v_acc_power(on)
+
+    @property
+    def enable_5v(self):
+        return self._green_giant.enabled_5v
+    
+    @enable_5v.setter
+    def enable_5v(self, on):
+        self._green_giant.set_5v_acc_power(on)
 
     def stop(self):
         """Stops the robot and cuts power to the motors.