#!/usr/stsci/pyssg/Python-2.5.4/bin/python
#
#NAME:
#  SimpleControlLoop.py 
#
#PURPOSE:
#  This function determines the output torque for a very simple control 
#  loop with only a P term in the classical PID loop.
#
#INPUTS:
#  w_m: vec3 float
#    The measured angular velocity vector
#  w_d: float
#    The desired angular velocity vector
#  I_m: mat3
#    The 3x3 moment of intertia tensor
#  k: float
#    The proportional factor for the control loop
#  t_max: float
#    The maximum control torque that can be applied to any axis
#
#RETURNS:
#  t : vec3
#    The control torque that will be applied
#
##########################################################################
from pylab import sign

def SimpleControlLoop(w_m, w_d, I_m, k, t_max):
  
  #Calculate the torque necessary
  t_x = I_m[0,0]*k*(w_d.x-w_m.x) + (I_m[2,2]-I_m[1,1])*w_m.y*w_m.z
  t_y = I_m[1,1]*k*(w_d.y-w_m.y) + (I_m[0,0]-I_m[2,2])*w_m.x*w_m.z
  t_z = I_m[2,2]*k*(w_d.z-w_m.z) + (I_m[1,1]-I_m[0,0])*w_m.x*w_m.z

  #Now apply upper limits to the torque
  if abs(t_x) > t_max : t_x = sign(t_x)*t_max
  if abs(t_y) > t_max : t_y = sign(t_y)*t_max
  if abs(t_z) > t_max : t_z = sign(t_z)*t_max
 
  #Form the three vector and return it
  return t_x, t_y, t_z
