From e73bedc346e6e3f35c2e5f367edd9e62914af51d Mon Sep 17 00:00:00 2001 From: SoulSurvivor-Boon <62327041+SoulSurvivor-Boon@users.noreply.github.com> Date: Tue, 21 Dec 2021 06:58:51 +0300 Subject: [PATCH] docs(deprications) Written by: V.S. Boon Committed on: 1-12-2021 Version: 1.0.0 --- raspberry_pi/classes.py | 19 ++++++++++++------- raspberry_pi/control/control.py | 2 +- raspberry_pi/experiments/hold_position.py | 16 +++++++++++----- 3 files changed, 24 insertions(+), 13 deletions(-) diff --git a/raspberry_pi/classes.py b/raspberry_pi/classes.py index 8968e91..cc057df 100644 --- a/raspberry_pi/classes.py +++ b/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]. diff --git a/raspberry_pi/control/control.py b/raspberry_pi/control/control.py index 7b0f5ae..c553277 100644 --- a/raspberry_pi/control/control.py +++ b/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 diff --git a/raspberry_pi/experiments/hold_position.py b/raspberry_pi/experiments/hold_position.py index 87a2920..a1aef40 100644 --- a/raspberry_pi/experiments/hold_position.py +++ b/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__()