docs(deprications)

Written by: V.S. Boon
Committed on: 1-12-2021
Version: 1.0.0

Author: VSBoon

raspberry_pi/classes.py

@@ -127,19 +127,20 @@ def __init__(self, lenData: int, joints: List[Joint], desAngles:
         should be removed and code refactored.
         """
         self.lenData = lenData
-        self.desAngle = desAngles
+        self.desAngle = desAngles #Depricated
         self.joints = joints
         self.totCount = [0 for i in range(lenData)]
         self.rotDirCurr = [None for i in range(lenData)]
         self.current = [None for i in range(lenData)]
         self.homing = [None for i in range(lenData)]
+        #Often used in the form SerialData.currAngle[:-1]
         self.currAngle = [0. for i in range(lenData)]
         self.prevAngle = [0. for i in range(lenData)]
         self.mSpeed = [0. for i in range(lenData)]
         self.rotDirDes = [0 for i in range(lenData)]
-        self.dataOut = ['0|0|0' for i in range(lenData)]
-        self.maxDeltaAngle = maxDeltaAngle
-        self.angleTol = angleTol
+        self.dataOut = ['0|0' for i in range(lenData)]
+        self.maxDeltaAngle = maxDeltaAngle #Depricated
+        self.angleTol = angleTol #Depricated
         self.limBool = [False for i in range(lenData)]
 
     def ExtractVars(self, dataPacket: List[str]):
@@ -153,7 +154,7 @@ def ExtractVars(self, dataPacket: List[str]):
         """
         for i in range(self.lenData):
             args = dataPacket[i].split('|')
-            if len(args) == 3:
+            if len(args) >= 3:
                 self.homing[i] = args[2]
                 self.homing[i] = int(self.homing[i])
             if len(args) == 4:
@@ -163,7 +164,6 @@ def ExtractVars(self, dataPacket: List[str]):
             self.totCount[i] = int(self.totCount[i])
             self.rotDirCurr[i] = int(self.rotDirCurr[i])
             self.prevAngle[i] = self.currAngle[i]
-            #TODO: CHECK!!!
             if i == self.lenData-1:
                     #Gripper doesn't have an 'angle'
                     self.currAngle[i] = self.totCount[i]
@@ -194,6 +194,7 @@ def ExtractVars(self, dataPacket: List[str]):
 
     def Dtheta2Mspeed(self, dtheta: "np.ndarray[float]", 
                       dthetaMax: List[float], PWMMin: int, PWMMax: int):
+        #DEPRICATED!
         """Translates desired motor velocities in rad/s to PWM.
         :param dtheta: Numpy array of motor velocities in [rad/s].
         :param dthetaMax: Angular velocity of each motor at a PWM of 255.
@@ -208,6 +209,7 @@ def Dtheta2Mspeed(self, dtheta: "np.ndarray[float]",
             for i in range(self.lenData-1)] #Rudimentary solution, PID will help!
 
     def CheckCommFault(self) -> bool:
+        #DEPRICATED
         """Checks if data got corrupted using a maximum achievable 
         change in angle between two timesteps.
         :return commFault: Boolean indicating if new angle is 
@@ -229,6 +231,7 @@ def CheckCommFault(self) -> bool:
         return commFault
 
     def CheckTolAng(self) -> bool:
+        #DEPRICATED
         """Determines if the desired angle has been reached within the 
         given tolerance, and if the desired angle is reachable.
         If the desired angle is unreachable, the closest angle is chosen.
@@ -267,6 +270,7 @@ def CheckJointLim(self):
 
 
     def GetDir(self):
+        #DEPRICATED
         """Gives the desired direction of rotation.
         """
         for i in range(self.lenData):
@@ -279,7 +283,8 @@ def GetDir(self):
                 self.rotDirDes[i] = self.rotDirCurr[i]
 
     def PControl1(self, i: int, mSpeedMax: int, mSpeedMin: int):
-        """Gives motor speed commands proportional to the angle error.
+        #DEPRICATED
+        """Gives motor speed commands propor tional to the angle error.
         :param i: Current iteration number.
         :param mSpeedMax: Maximum rotational speed, 
                           portrayed in PWM [0, 255].

raspberry_pi/control/control.py

@@ -88,7 +88,7 @@ def PosControl(sConfig: Union[np.ndarray, List], eConfig: Union[np.ndarray, List
     timeScaling=5)
     print(f"Total estimated time for trajectory: {round(dt*traj[:,0].size, 2)} s")
     traj, velTraj, accTraj = TrajDerivatives(traj, method, robot, dt)
-    g = np.array([0,0,-9.81]) #EXPERIMENTAL!
+    g = np.array([0,0,-9.81])
     FTip = np.zeros(6) #Position control, --> assume no end-effector force.
     tauPID = np.zeros(traj[0,:].size)
     n = -1 #iterator

raspberry_pi/experiments/hold_position.py

@@ -11,7 +11,7 @@
 import pygame
 import matplotlib.pyplot as plt
 from dynamics.dynamics_funcs import FeedForward
-from robot_init import robotFric as Pegasus
+from robot_init import robot as Pegasus
 from settings import sett
 from typing import Tuple
 from classes import SerialData, Robot, PID
@@ -20,6 +20,7 @@
 from control.control import PosControl
 from main import HoldPos
 
+dataMat = np.zeros((1,8)) #First three for encoder readings, second three for output torques
 serial = SerialData(6, Pegasus.joints)
 #port = FindSerial(askInput=True)[0]
 Teensy = StartComms('COM13') #TEMPORARY, REPLACE WITH port
@@ -31,26 +32,31 @@
     dtAct = sett['dtFF']*0.5
     dtComm = sett['dtComm']
     D = sett['D'] #Absolute rotational damper
-    serial = SerialData(6, Pegasus.joints)
+    SReadAndParse(serial, Teensy)
     thetaStart = np.array(serial.currAngle[:-1])
     #thetaDes = np.array([0*np.pi,0,0.5*np.pi,-0.5*np.pi,0.5*np.pi])
-    thetaDes = np.array([0.2*np.pi,0,0,0,0])
+    thetaDes = np.array([0.*np.pi,0.*np.pi,0*np.pi,0*np.pi,0])
     pygame.init()
     screen = pygame.display.set_mode([700, 500])
     background = pygame.image.load(os.path.join(parent,'control_overview.png'))
-    PosControl(thetaStart, thetaDes, Pegasus, serial, 0.1, 0.1, 0.2, PIDObj, dtComm, dtAct, sett['dtFrame'], Teensy, screen, background)
+    PosControl(thetaStart, thetaDes, Pegasus, serial, 0.1, 0.1, 0.1, PIDObj, dtComm, dtAct, sett['dtFrame'], Teensy, screen, background)
     lastHold = -100 #Last hold should run first
     lastComm = time.perf_counter()
     lastFrame = time.perf_counter()
     dtHold = sett['dtFF']
     dtFrame = sett['dtFrame']
     PWM = serial.mSpeed[:-1]
+    newRow = np.zeros((1,8)) #TEMP
     print("Start holding")
     while True:
         if time.perf_counter() - lastHold >= dtHold:
-            lastComm, lastFrame = HoldPos(serial, Teensy, Pegasus, PIDObj, thetaDes, lastComm, lastFrame, dtComm, dtHold, dtFrame, screen, background)
+            lastComm, lastFrame, tau = HoldPos(serial, Teensy, Pegasus, PIDObj, thetaDes, lastComm, lastFrame, dtComm, dtHold, dtFrame, screen, background)
             lastHold = time.perf_counter()
+            newRow[0,0:4] = np.array([np.array(serial.currAngle[1:-1]) - thetaDes[1:]])
+            newRow[0,4:] = tau[1:]
+            dataMat = np.vstack((dataMat, newRow))
 finally:
+    np.savetxt("PIDDataLowAngle4.csv", dataMat, fmt="%.4f", delimiter=',')
     serial.dataOut = [f"{0|0}" for i in range(serial.lenData)]
     Teensy.write(f"{serial.dataOut}\n".encode('utf-8'))
     Teensy.__del__()