WeldingSpotPerformance/OptAlgorithm/OptAlgorithm.py

from OptAlgorithm.OptTimeCalculator import OptTimeCalculator
from OptAlgorithm.AutoConfigClass import AutoConfigClass
from OptAlgorithm.ConstantCalculator import ConstantCalculator

from numpy import cos, sin, sqrt, cbrt, arcsin


class OptAlgorithm(AutoConfigClass):
    
    params_list = []

    def __init__(self, operator_config : dict, system_config : dict):
        cCalculator = ConstantCalculator(operator_config, system_config)
        super().__init__(OptAlgorithm.params_list, operator_config, system_config, cCalculator.calc())

        calc = OptTimeCalculator(operator_config, system_config)

        self.Ts = calc.T(self.dist_open_start_1,
                         self.dist_open_start_2,
                         self.dist_open_after_1,
                         self.dist_open_after_2,
                         self.dist_open_end_1,
                         self.dist_open_end_2)

        self.x1Contact = self.dist_open_start_1 + self.position_start_1
        self.x2Contact = self.dist_open_start_2 + self.position_start_2
    
    def V1Close(self, t: float):
        if t < self.Ts["tclose_1_acc"]:
            return self.a_max_1 * t
        else:
            return self.a_max_1 * self.Ts["tclose_1_acc"]
            
    def X1Close(self, t: float):
        t1 = min(t, self.Ts["tclose_1_acc"])
        x0 =  self.a_max_1 * t1 * t1 / 2
        
        t2 = max(t - self.Ts["tclose_1_acc"], 0)
        x1 = self.a_max_1 * self.Ts["tclose_1_acc"] * t2
        
        return x0 + x1 + self.position_start_1
        
    def V2Close(self, t: float):
        if t < self.Ts["tclose_2_acc"]:
            return self.a_max_2 * t
        t -= self.Ts["tclose_2_acc"]
        if t < self.Ts["tclose_2_speed"]:
            return self.a_max_2 * self.Ts["tclose_2_acc"]
        t -= self.Ts["tclose_2_speed"]
        return self.a_max_2 * self.Ts["tclose_2_acc"]  - self.a_max_2 * t
    
    def X2Close(self, t: float):
        t1 = min(t, self.Ts["tclose_2_acc"])
        x0 =  self.a_max_2 * t1 * t1 / 2
        
        t2 = max(min(t - self.Ts["tclose_2_acc"], self.Ts["tclose_2_speed"]), 0)
        x1 = self.a_max_2 * self.Ts["tclose_2_acc"] * t2
        
        t3 = max(min(t - self.Ts["tclose_2_speed"]- self.Ts["tclose_2_acc"], self.Ts["tclose_2_acc"]), 0)
        x2 = self.a_max_2 * self.Ts["tclose_2_acc"] * t3 - self.a_max_2 * t3 * t3 / 2
        
        return x0 + x1 + x2 + self.position_start_2
        
    def FClose(self, t: float):
        return 0
        
    def V1Grow(self, t: float):
        F = self.FGrow(t)
        dF0 = self.a_max_1 * self.Ts["tclose_1_acc"] * self.k_hardness_1
    
        dFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow)
        if t < self.Ts["tspeed"]:
            dF = sqrt(self.k_hardness_1 / self.mass_1) * self.eff_control * sin(sqrt(self.k_hardness_1 / self.mass_1) * t) + dF0 * cos(sqrt(self.k_hardness_1 / self.mass_1) * t)
            return dF / self.k_hardness_1
        t -= self.Ts["tspeed"]
        if t < self.Ts["tmeet"]:
            return dFmax / self.k_hardness_1
        t -= self.Ts["tmeet"]
        if t < self.Ts["tend"]:
            return sqrt(self.k_hardness_1/self.mass_1)*sqrt((self.force_target-self.Fprop)**2 - F**2) / self.k_hardness_1
        v0 = sqrt(self.k_hardness_1/self.mass_1)*sqrt((self.force_target-self.Fprop)**2 - self.Fstart_prop**2)
        b = (1/3 * v0 / cbrt((self.force_target - self.Fstart_prop))**2)**3
        dF = 3.0*b*cbrt((F -self.force_target)/b)**2
        return dF / self.k_hardness_1
            
    def X1Grow(self, t: float):
        F = self.FGrow(t)
        x = F / self.k_hardness_1
        return x + self.x1Contact
        
    def V2Grow(self, t: float):
        """
        Считается, что верхний электрод не влияет на набор усилия,
        функция не реализована!, возвращает 0. Устанавливайте kturn = 0
        """
        return 0
    
    def X2Grow(self, t: float):
        """
        Считается, что верхний электрод не влияет на набор усилия,
        функция не реализована!, возвращает 0. Устанавливайте kturn = 0
        """
        return self.x2Contact
        
    def FGrow(self, t: float):
        v0 = self.a_max_1 * self.Ts["tclose_1_acc"]
        dF0 = self.a_max_1 * self.Ts["tclose_1_acc"] * self.k_hardness_1
        
        dFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow)
        self.Fmeet =  1/ self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - dFmax**2)
        tspeed = self.Ts["tspeed"]
        Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1/self.freq * dF0 * sin(self.freq * tspeed)
        if t < self.Ts["tspeed"]:
            return - self.eff_control * cos(self.freq * t) + self.eff_control + 1/self.freq * dF0 * sin(self.freq * t)
        t -= self.Ts["tspeed"]
        if t < self.Ts["tmeet"]:
            return Fspeed + t * dFmax
        t -= self.Ts["tmeet"]
        if t < self.Ts["tend"]:
            tm = arcsin(self.Fmeet/(self.Ftogrow)) * 1/self.freq
            return self.Ftogrow * sin(self.freq*(t + tm))
        t -= self.Ts["tend"]
        
        #tprop
        a_max_1 = self.force_target
        v0 = self.freq*sqrt((self.force_target-self.Fprop)**2 - self.Fstart_prop**2)
        b = (1/3 * v0 / cbrt((self.force_target - self.Fstart_prop))**2)**3
        q = self.Fstart_prop
        
        return 3 * t**2 * cbrt(b*b*(q-a_max_1)) + 3 * t * cbrt(q-a_max_1)**2 * cbrt(b) + b * t**3 + q 
    
    def V1Open(self, t: float):
        if t < self.Ts["topen_1_acc"]:
            return -self.a_max_1 * t
        t -= self.Ts["topen_1_acc"]
        if t < self.Ts["topen_1_speed"]:
            return -self.a_max_1 * self.Ts["topen_1_acc"]
        t -= self.Ts["topen_1_speed"]
        if t < self.Ts["topen_1_acc"]:
            return -self.a_max_1 * self.Ts["topen_1_acc"]  + self.a_max_1 * t
        return 0
        
    def X1Open(self, t: float):
        xm = self.force_target / self.k_hardness_1
        t1 = min(t, self.Ts["topen_1_acc"])
        x0 =  -self.a_max_1 * t1 * t1 / 2
        
        t2 = max(min(t - self.Ts["topen_1_acc"], self.Ts["topen_1_speed"]), 0)
        x1 = -self.a_max_1 * self.Ts["topen_1_acc"] * t2
        
        t3 = max(min(t - self.Ts["topen_1_speed"]- self.Ts["topen_1_acc"], self.Ts["topen_1_acc"]), 0)
        x2 = -self.a_max_1 * self.Ts["topen_1_acc"] * t3 + self.a_max_1 * t3 * t3 / 2
        
        return xm + x0 + x1 + x2 + self.x1Contact
        
    def V2Open(self, t: float):
        
        t = max(t-self.Ts["topen_2_offset"] , 0)
        if t < self.Ts["topen_2_acc"]:
            return -self.a_max_2 * t
        t -= self.Ts["topen_2_acc"]
        if t < self.Ts["topen_2_speed"]:
            return -self.a_max_2 * self.Ts["topen_2_acc"]
        t -= self.Ts["topen_2_speed"]
        if t < self.Ts["topen_2_acc"]:
            return -self.a_max_2 * self.Ts["topen_2_acc"]  + self.a_max_2 * t
        return 0
    
    def X2Open(self, t: float):
        t = max(t-self.Ts["topen_2_offset"] , 0)
        t1 = min(t, self.Ts["topen_2_acc"])
        x0 =  -self.a_max_2 * t1 * t1 / 2
        
        t2 = max(min(t - self.Ts["topen_2_acc"], self.Ts["topen_2_speed"]), 0)
        x1 = -self.a_max_2 * self.Ts["topen_2_acc"] * t2
        
        t3 = max(min(t - self.Ts["topen_2_speed"]- self.Ts["topen_2_acc"], self.Ts["topen_2_acc"]), 0)
        x2 = -self.a_max_2 * self.Ts["topen_2_acc"] * t3 + self.a_max_2 * t3 * t3 / 2
        
        return x0 + x1 + x2 + self.x2Contact
        
    def FOpen(self, t: float):
        x1 = self.X1Open(t)
        x2 = self.X2Open(t)
        F = self.k_hardness_1 * max(0, (x1 - self.x1Contact))
        return F
    
    def calcPhaseClose(self, t: float):
        """
        Получить значения X1, X2, V1, V2, F в момент времени t для фазы смыкания

        Args:
            t (float): Момент времени

        Returns:
            (float, float, float, float, float): X1, X2, V1, V2, F
        """
        return self.X1Close(t), self.X2Close(t), self.V1Close(t), self.V2Close(t), self.FClose(t)
        
    def calcPhaseGrow(self, t: float):
        """
        Получить значения X1, X2, V1, V2, F в момент времени t для фазы роста усилия

        Args:
            t (float): Момент времени

        Returns:
            (float, float, float, float, float): X1, X2, V1, V2, F
        """
        return self.X1Grow(t), self.X2Grow(t), self.V1Grow(t), self.V2Grow(t), self.FGrow(t)
        
    def calcPhaseOpen(self, t: float):
        """
        Получить значения X1, X2, V1, V2, F в момент времени t для фазы раксрытия

        Args:
            t (float): Момент времени

        Returns:
            (float, float, float, float, float): X1, X2, V1, V2, F
        """
        return self.X1Open(t), self.X2Open(t), self.V1Open(t), self.V2Open(t), self.FOpen(t)
    
    def getSpecific(self, param : str, phase : str, t : float):
        """
        Получить значение величины в определенную фазу в момент времени t (с начала фазы)

        Args:
            param (str): Значение из списка X1 | X2 | V1 | V2 | F
            phase (str): Значение из списка: Close | Grow | Open
            t (float): Время

        Returns:
            Значение величины
        """
        funcName = param + phase
        try:
            func = getattr(self, funcName)
        except:
            print("Wrong param or phase name")
            return 0
        return func(t)
    
    def getVar(self, param : str, t : float):
        """
        Получить значение величины в момент времени t (БЕЗ УЧЕТА СВАРКИ!)

        Args:
            param (str): Значение из списка X1 | X2 | V1 | V2 | F
            t (float): Время

        Returns:
            Значение величины
        """
        if t < self.Ts["tclose"]:
            return  self.getSpecific(param, "Close", t)
        t -= self.Ts["tclose"]
        if t < self.Ts["tgrow"] :
            return  self.getSpecific(param, "Grow", t)
        t -= self.Ts["tgrow"]
        return  self.getSpecific(param, "Open", t)