bug fix

RobotLib.py

@@ -31,7 +31,7 @@ class Robot:
             rad = deg * math.pi / 180
             return rad
 
-        def _calc_previous(self):
+        def calc_previous(self):
             if self.previous_servos:
                 self.total_angle_deg = 0
                 for servo in self.previous_servos:
@@ -44,11 +44,12 @@ class Robot:
                 self.total_angle_deg = self.deg
                 if self.delta < 0:
                     self.delta *= -1
+            self.total_angle_rad = self._get_rad(self.total_angle_deg)
 
         def set_angle(self, angle):
             self.deg = angle
             self.rad = self._get_rad(self.deg)
-            self._calc_previous()
+            self.calc_previous()
 
         def _get_angle(self, ms):
             val_min = self.geometry.min
@@ -67,7 +68,7 @@ class Robot:
             self.ms = ms
             self.deg = self._get_angle(self.ms)
             self.rad = self._get_rad(self.deg)
-            self._calc_previous()
+            self.calc_previous()
 
         def get_ms(self, calc=False):
             if calc:
@@ -200,6 +201,8 @@ class Robot:
         return effi
 
     def calculate(self):
+        for servo in [self.servo1, self.servo2, self.servo3]:
+            servo.calc_previous()
         self.x, self.y = self._get_position()
         self.efficency = self._get_efficency()
         self.x = self._round(self.x)