From b72801d6540a4ea24288c7a6e97bf7137fc2fca2 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Fri, 10 May 2019 14:19:55 +0200
Subject: [PATCH] Completed Robot model. Now really ;)

---
 databaseCreatorv2.py | 23 ++++++++++++++++++++---
 1 file changed, 20 insertions(+), 3 deletions(-)

diff --git a/databaseCreatorv2.py b/databaseCreatorv2.py
index 4161ae9..16515f2 100644
--- a/databaseCreatorv2.py
+++ b/databaseCreatorv2.py
@@ -11,6 +11,7 @@ class Robot:
             self.previous_servos = None
             self.total_angle_deg = None
             self.total_angle_rad = None
+            self.delta = None
 
         def add_previous_servos(self, previous_servos=None):
             self.previous_servos = previous_servos
@@ -18,6 +19,9 @@ class Robot:
             if previous_servos:
                 for servo in self.previous_servos:
                     self.total_angle_deg += servo.deg
+                self.delta = self.deg + previous_servos[-1].deg
+                if self.delta < 0:
+                    self.delta *= -1
             self.total_angle_rad = self._get_rad(self.total_angle_deg)
 
         def _get_rad(self, deg):
@@ -26,7 +30,7 @@ class Robot:
 
         def set_angle(self, angle):
             self.deg = angle
-            self.rad = self._get_rad()
+            self.rad = self._get_rad(self.deg)
 
         def _get_angle(self, ms):
             val_min = self.geometry.min
@@ -110,9 +114,11 @@ class Robot:
         self.arm2 = None
         self.arm3 = None
         self.data = self.Database()
+        self.x = None
+        self.y = None
+        self.efficency = None
+
 
-        # get position based on servos
-        # get efficency based on servos
 
     def init_depending(self, a1, a2, a3, s1_prev=None, s2_prev=None, s3_prev=None):
         self.arm1 = a1
@@ -121,11 +127,22 @@ class Robot:
         self.servo1.add_previous_servos(s1_prev)
         self.servo2.add_previous_servos(s2_prev)
         self.servo3.add_previous_servos(s3_prev)
+        self.x, self.y = self._get_position()
+        self.efficency = self._get_efficency()
 
     def _get_position(self):
+        x = 0
+        y = 0
+        for arm in [self.arm1, self.arm2, self.arm3]:
+            x += math.sin(arm.attachted_to.total_angle_rad) * arm.length + math.sin(arm.attachted_to.total_angle_rad + 1.5708) * arm.height
+            y += math.cos(arm.attachted_to.total_angle_rad) * arm.length + math.cos(arm.attachted_to.total_angle_rad + 1.5708) * arm.height
         return x, y
 
     def _get_efficency(self):
+        effi = 0
+        multipliers = [8, 4, 2]
+        for i, servo in enumerate([self.servo1, self.servo2, self.servo3]):
+            effi += servo.delta * multipliers[i]
         return effi