templates.py

import numpy as np
import matplotlib.pyplot as plt


# SHAPE FUNCTIONS --------------------------------------------------
def gaussian(x, mu, sigma, amp):
    return (np.radians(amp))*(np.exp(-np.power(x - mu, 2.) / (2 * np.power(sigma, 2.))))

def sigmoid(x,shift,amp,k):
    return ((np.radians(amp))/(1 + np.exp(k*(-x +shift)))) 

def straight(x,slope,b):
    return (slope*x+b)


# PLOT FUNCTION SHAPES -----------------------------------------------
def bellcurve_vis(time, amp):
    x_val = np.arange(0, time, 0.01)
    path, = plt.plot(x_val, gaussian(x_val, 5, 2, amp))
    plt.grid(True, which='both')
    plt.axhline(y=0, color='k')
    plt.xlabel('time')
    plt.ylabel('steering angle (in degrees)')
    plt.title('Bell Curve to Represent Steering Profile')
    bellcurve_vis.xdata = path.get_xdata()
    bellcurve_vis.ydata = path.get_ydata()
    plt.show()


def straight_vis(time):
    plt.hlines(y = 0, xmin = 1, xmax = time, label = "black line")
    plt.grid(True, which='both')
    plt.axhline(y=0, color='k')
    plt.xlabel('time')
    plt.ylabel('steering angle (in degrees)')
    plt.title('Line to Represent Steering Profile')
    straight_vis.xdata = np.arange(0,time,0.1)
    straight_vis.ydata = np.full(np.size(straight_vis.xdata),0)
    plt.show()

def sigmoid_vis(time,amp,shift,slope):
    x = np.arange(0, time, 0.1)
    z = sigmoid(x,shift,amp,slope)
    path, = plt.plot(x, z)
    plt.grid(True, which='both')
    plt.axhline(y=0, color='k')
    plt.xlabel('time')
    plt.ylabel('steering angle (in degrees)')
    plt.title('S Curve to Represent Steering Profile')
    sigmoid_vis.xdata = path.get_xdata()
    sigmoid_vis.ydata = path.get_ydata()
    plt.show()


# TEST CODE -------------------------------------------------------