From 0f2754af73df64ae0f40b2ff728e396b13296c8c Mon Sep 17 00:00:00 2001
From: Ram Reddy <69733662+ramnreddy15@users.noreply.github.com>
Date: Sun, 25 Jul 2021 15:58:30 -0400
Subject: [PATCH] Finish stuff

---
 .../controllers/olympian/Propioception.py     | 193 +++++++++++++++++-
 1 file changed, 186 insertions(+), 7 deletions(-)

diff --git a/simulation/controllers/olympian/Propioception.py b/simulation/controllers/olympian/Propioception.py
index 7074121..7acc458 100644
--- a/simulation/controllers/olympian/Propioception.py
+++ b/simulation/controllers/olympian/Propioception.py
@@ -1,12 +1,56 @@
-
+"""
+This class keeps track of all the motors, sensors and import variables needed for the robot.
+"""
 class Propioception:
-    def __init__(self):
+    """
+    Standard kwargs to give for this function
+
+    data = {
+        # Motors (all units are in degrees)
+        'left_hip_pitch': 0,
+        'left_hip_yaw': 0,
+        'left_hip_roll': 0,
+        'left_knee_pitch': 0,
+        'left_ankle_pitch': 0,
+        'left_ankle_yaw': 0,
+        'left_ankle_roll': 0,
+        'right_hip_pitch': 0,
+        'right_hip_yaw': 0,
+        'right_hip_roll': 0,
+        'right_knee_pitch': 0,
+        'right_ankle_pitch': 0,
+        'right_ankle_yaw': 0,
+        'right_ankle_roll': 0,
+        'left_shoulder_pitch': 0,
+        'left_shoulder_yaw': 0,
+        'left_shoulder_roll': 0,
+        'left_elbow_pitch': 0,
+        'right_shoulder_pitch': 0,
+        'right_shoulder_yaw': 0,
+        'right_shoulder_roll': 0,
+        'right_elbow_pitch': 0,
+        'torso_pitch': 0,
+        'torso_yaw': 0,
+        'torso_roll': 0,
+        'neck_pitch': 0,
+        'neck_yaw': 0,
+        'neck_roll': 0,
+
+        # Sensors
+        # Add later
+    }
+
+    This will also have the constants
+    """
+    def __init__(self, **kwargs):
         self.data = {
-            'locationX' = 0,
-            'locationY' = 0,
-            'locationZ' = 0,
-            ''
+            # Add defualt later
         }
+        self.constants = {
+            # Add some constants here later
+        }
+        for key, value in kwargs.items():
+            self.data[str(key)] = value
 
     def update(self, partsAndValues):
         """
@@ -18,6 +62,7 @@ class Propioception:
         1 is success
         -1 is the error for NoneType variables
         -2 is for incorrect datatypes
+        -3 is for when the key is not in the dictionary
         """
 
         if partsAndValues == None:
@@ -27,4 +72,138 @@ class Propioception:
             return -2
 
         for key in partsAndValues.keys():
-            self.data[key] = partsAndValues[key]
+            if key in self.data:
+                self.data[str(key)] = partsAndValues[key]
+            else:
+                return -3
+            return 1
+
+    def add(self, partsAndValues):
+        """
+        This function will add a part to the system if needed
+
+        arg artsAndValues is {string --> int or float}
+
+        Error codes:
+        1 is success
+        -1 is the error for NoneType variables
+        -2 is for incorrect datatypes
+        -3 is for when the key is in the dictionary
+        """
+
+        if partsAndValues == None:
+            return -1
+
+        if type(partsAndValues) != dict:
+            return -2
+
+        for key in partsAndValues.keys():
+            if key in self.data:
+                return -3
+            else:
+                self.data[str(key)] = partsAndValues[key]
+            return 1
+
+    def remove(self, partsAndValues):
+        """
+        This function will remove a part to the system if needed
+
+        arg artsAndValues is [string]
+
+        Error codes:
+        1 is success
+        -1 is the error for NoneType variables
+        -2 is for incorrect datatypes
+        -3 is for when the key is not in the dictionary
+        """
+
+        if partsAndValues == None:
+            return -1
+            
+
+        if type(partsAndValues) != list:
+            return -2
+
+        for key in partsAndValues:
+            if key in self.data:
+                self.data.pop(str(key))
+            else:
+                return -3
+            return 1
+        
+
+# Test
+stuff = Propioception(
+        left_hip_pitch= 0,
+        left_hip_yaw= 0,
+        left_hip_roll= 0,
+        left_knee_pitch= 0,
+        left_ankle_pitch= 0,
+        left_ankle_yaw= 0,
+        left_ankle_roll= 0,
+        right_hip_pitch= 0,
+        right_hip_yaw= 0,
+        right_hip_roll= 0,
+        right_knee_pitch= 0,
+        right_ankle_pitch= 0,
+        right_ankle_yaw= 0,
+        right_ankle_roll= 0,
+        left_shoulder_pitch= 0,
+        left_shoulder_yaw= 0,
+        left_shoulder_roll= 0,
+        left_elbow_pitch= 0,
+        right_shoulder_pitch= 0,
+        right_shoulder_yaw= 0,
+        right_shoulder_roll= 0,
+        right_elbow_pitch= 0,
+        torso_pitch= 0,
+        torso_yaw= 0,
+        torso_roll= 0,
+        neck_pitch= 0,
+        neck_yaw= 0,
+        neck_roll= 0
+        )
+        
+"""Update cases"""
+# Success case
+print(stuff.data['neck_roll'])        
+stuff.update({'neck_roll': 21})
+print(stuff.data['neck_roll'])
+
+# -1 case
+print(stuff.update(None))
+
+# -2 case
+print(stuff.update([]))
+
+# -3 case
+print(stuff.update({'test': 21}))
+
+"""Remove cases"""
+# Success case
+print(stuff.data['neck_roll'])        
+print(stuff.remove(['neck_roll']))
+
+# -1 case
+print(stuff.remove(None))
+
+# # -2 case
+print(stuff.remove({}))
+
+# -3 case
+print(stuff.remove(['test']))
+
+"""Add cases"""
+# Success case
+print('test' in stuff.data.keys())        
+stuff.add({'test': 21})
+print(stuff.data['test'])
+
+# -1 case
+print(stuff.add(None))
+
+# -2 case
+print(stuff.add([]))
+
+# -3 case
+print(stuff.add({'test': 21}))