two_point_steering_model.py

import numpy as np
import math

# CONTROL LAW FUNCTIONS ==========================================================================

def control_law(i,dictionary,Agent,Simulation):
# CALCULATE DIFFERENCE BETWEEN CURRENT ANGLE AND PREVIOUS ANGLE
    current_near_point_angle = dictionary[i-1]['nearPoint_angle']
    current_far_point_angle = dictionary[i-1]['farPoint_angle']
    prev_near_point_angle = dictionary[i-2]['nearPoint_angle']
    prev_far_point_angle = dictionary[i-2]['farPoint_angle']

    farPoint_loc = dictionary[i-1]['farPoint_x'],dictionary[i-1]['farPoint_y']
    agent_loc = dictionary[i-1]['agent_x_loc'],dictionary[i-1]['agent_y_loc']

    farPoint_to_agent_distance = math.dist(farPoint_loc,agent_loc)
    farPoint_angle_dot = -1*((Agent.velocity*Simulation.simulation_resolution)*np.sin(current_far_point_angle))/farPoint_to_agent_distance

    #print("FP angle dot",farPoint_angle_dot)

    nearPoint_angle_delta = current_near_point_angle - prev_near_point_angle
    farPoint_angle_delta = current_far_point_angle - prev_far_point_angle

    #print("delta FP angle",farPoint_angle_delta)
    
    delta_alpha = Agent.proportional_nearPoint_gain*current_near_point_angle*Simulation.delta_time + Agent.derivative_nearPoint_gain*nearPoint_angle_delta + Agent.derivative_farPoint_gain*farPoint_angle_delta
    #delta_alpha = Agent.proportional_nearPoint_gain*current_near_point_angle*Simulation.delta_time + Agent.derivative_nearPoint_gain*nearPoint_angle_delta + Agent.derivative_farPoint_gain*farPoint_angle_dot
    return(delta_alpha)


# X,Y, ALPHA, AND PHI CALCULATION ========================================================================

# CALCULATE PHI, X AND Y VALUES (PHI IS CALCULATED BY TAKING THE DERIVATIVE OF ALPHA)
def calculate_alpha(i,dictionary,Simulation,delta_alpha):
    prev_alpha = dictionary[i-1]['alpha']
    alpha = prev_alpha + delta_alpha*Simulation.delta_time
    return(alpha)

def calculate_phi(i,dictionary,Agent,Simulation,alpha):
    prev_phi = dictionary[i-1]['phi']
    phi = prev_phi + Agent.velocity*Agent.phi_gain*Simulation.delta_time*alpha
    return(phi)

def calculate_x_y(i,dictionary,Simulation,Agent,phi):
    pastAgent_x = dictionary[i-1]['agent_x_loc']
    pastAgent_y = dictionary[i-1]['agent_y_loc']

    x = pastAgent_x + np.sin(phi)*Simulation.delta_time*Agent.velocity
    y = pastAgent_y + np.cos(phi)*Simulation.delta_time*Agent.velocity
    return(x,y)