public void refreshAcceleration()
{
aceleration.X = ((analogRead.X * VoltsPerCount) - (VZEROG)) / Sensitivity;
aceleration.Y = ((analogRead.Y * VoltsPerCount) - (VZEROG)) / Sensitivity;
aceleration.Z = ((analogRead.Z * VoltsPerCount) - (VZEROG)) / Sensitivity;
acelerationUnit = Triplet.normalized(aceleration);
}
/// <summary>
/// Calcula el producto CRUZ entre los vectores recibidos como parámetros.
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public static Triplet crossProduct(Triplet A, Triplet B)
{
Triplet C = new Triplet();
C.X = ((A.Y * B.Z) - (A.Z * B.Y));
C.Y = ((A.Z * B.X) - (A.X * B.Z));
C.Z = ((A.X * B.Y) - (A.Y * B.X));
return(C);
}
/// <summary>
/// Calcula el producto PUNTO entre los vectores recibidos como parámetros.
/// </summary>
/// <param name="A"></param>
/// <param name="B"></param>
/// <returns></returns>
public static double dotProduct(Triplet A, Triplet B)
{
double respuesta;
respuesta = (A.X * B.X);
respuesta += (A.Y * B.Y);
respuesta += (A.Z * B.Z);
return(respuesta);
}
private void refreshZenithAngle()
{
double angleRad;
double dotProduct;
dotProduct = Triplet.dotProduct(this.aDEC.AcelerationUnit, Status.Zenith);
angleRad = Math.Acos(dotProduct);
this.zenithAngle = ((angleRad * 180.0) / Math.PI);
}
/// <summary>
/// Calcula la magnitud de este vector.
/// </summary>
/// <returns></returns>
public static double magnitude(Triplet V)
{
double magnitud;
magnitud = 0;
magnitud = Math.Sqrt(
(V.X * V.X) + (V.Y * V.Y) + (V.Z * V.Z)
);
return(magnitud);
}
/// <summary>
/// Calcula el producto matricial MxA
/// </summary>
/// <param name="M">La Matriz (esta matriz premultiplica.</param>
/// <param name="A">El vector a multiplicar.</param>
/// <returns></returns>
public static Triplet matrixProduct(Triplet[] M, Triplet A)
{
Triplet respuesta;
respuesta = new Triplet();
respuesta.X = Triplet.dotProduct(M[0], A);
respuesta.Y = Triplet.dotProduct(M[1], A);
respuesta.Z = Triplet.dotProduct(M[2], A);
return(respuesta);
}
/// <summary>
/// Crea el vector unitario del vector recibido como parametro
/// </summary>
/// <param name="V"></param>
/// <returns></returns>
public static Triplet normalized(Triplet V)
{
double magnitud;
magnitud = Triplet.magnitude(V);
Triplet respuesta;
if (magnitud == 0)
{
return(V);
}
respuesta.X = (V.X / magnitud);
respuesta.Y = (V.Y / magnitud);
respuesta.Z = (V.Z / magnitud);
return(respuesta);
}
/// <summary>
/// Calcula el octante del vector recibido utilizando la
/// convención "+=1"
/// </summary>
/// <param name="V"></param>
/// <returns></returns>
public static Byte Octant(Triplet V)
{
Byte respuesta;
respuesta = 0;
if (V.X >= 0)
{
respuesta += 1;
}
if (V.Y >= 0)
{
respuesta += 2;
}
if (V.Z >= 0)
{
respuesta += 4;
}
return(respuesta);
}
private void refreshCounterWeightAngle()
{
double angleRad;
double dotProduct;
Triplet ARcrossSouthPole;
Byte OctantARcrossSouthPole;
dotProduct = Triplet.dotProduct(this.aRA.AcelerationUnit, Status.SouthPole);
ARcrossSouthPole = Triplet.crossProduct(this.aRA.AcelerationUnit, Status.SouthPole);
OctantARcrossSouthPole = Triplet.Octant(ARcrossSouthPole);
//Console.WriteLine("ARcrossSouthPole=" + ARcrossSouthPole.ToString());
//Console.WriteLine("OctantARcrossSouthPole=" + OctantARcrossSouthPole.ToString());
angleRad = Math.Acos(dotProduct);
if (OctantARcrossSouthPole > 0)
{
angleRad *= -1.0;
}
angleRad += Math.PI;
this.counterWeightAngle = ((angleRad * 180.0) / Math.PI);
}
private void refreshDeclinationAngle()
{
double angleRad;
double dotProduct;
Triplet DECcrossSouthPole;
Byte OctantDECcrossSouthPole;
Triplet SouthPoleB;
SouthPoleB = Triplet.normalized(Triplet.matrixProduct(Status.RotationMatrix, this.aRA.AcelerationUnit));
dotProduct = Triplet.dotProduct(this.aDEC.AcelerationUnit, SouthPoleB);
DECcrossSouthPole = Triplet.crossProduct(this.aDEC.AcelerationUnit, SouthPoleB);
OctantDECcrossSouthPole = Triplet.Octant(DECcrossSouthPole);
Console.WriteLine("SouthPoleB=" + SouthPoleB.ToString());
Console.WriteLine("OctantDECcrossSouthPole=" + OctantDECcrossSouthPole.ToString());
angleRad = Math.Acos(dotProduct);
if (OctantDECcrossSouthPole < 6)
{
angleRad *= -1.0;
}
this.declinationAngle = ((angleRad * 180.0) / Math.PI);
Console.WriteLine("DEC_RA[º]=" + this.declinationAngle);
}
/// <summary>
/// Analiza una linea enviada por el microcontrolador de los Limits.
/// Esta linea contiene información sobre:
/// - Limit Switches
/// - Acelerómetros
/// - Cwa, Angulo del contrapeso
/// - Z, ángulo Zenital
/// - Indicador de protección de la montura
/// - Indicador de encendido de la montura
/// </summary>
public void Analiza()
{
String[] part;
part = this.linea.Split((" ").ToCharArray());
//for (int i = 0; i < part.Length; i++)
//{
// Console.WriteLine("part[" + i + "]=" + part[i]);
//}
try
{
this.flags = Int16.Parse(part[0]);
}
catch (FormatException exc)
{
logger.Error("linea=" + this.linea);
logger.Error(exc.Message);
return;
}
if (part.Length >= 6)
{
Triplet analogReadRA;
Triplet analogReadDEC;
try
{
analogReadRA = new Triplet(Double.Parse(part[1]), Double.Parse(part[2]), Double.Parse(part[3]));
analogReadDEC = new Triplet(Double.Parse(part[4]), Double.Parse(part[5]), Double.Parse(part[6]));
}
catch (FormatException exc)
{
logger.Error("linea=" + this.linea);
logger.Error(exc.Message);
return;
}
aRA.AnalogRead = analogReadRA;
aDEC.AnalogRead = analogReadDEC;
aRA.refreshAcceleration();
aDEC.refreshAcceleration();
//Console.WriteLine("Dec Unit= " + aDEC.AcelerationUnit.ToString());
this.refreshZenithAngle();
this.refreshCounterWeightAngle();
this.refreshDeclinationAngle();
}
if (part.Length >= 10)
{
try
{
this.counterWeightAngleArduino = Double.Parse(part[7]) * (180.0 / Math.PI);
this.zenithAngleArduino = Double.Parse(part[8]) * (180.0 / Math.PI);
this.zenithCounter = long.Parse(part[9]);
}
catch (FormatException exc)
{
logger.Error("linea=" + this.linea);
logger.Error(exc.Message);
return;
}
//Console.WriteLine("DeltaAngles = (" + (this.counterWeightAngleArduino - this.counterWeightAngle) + "," + (this.zenithAngleArduino - this.zenithAngle) + ")");
Console.WriteLine("Arduino Angles = (" + (this.counterWeightAngleArduino) + "," + (this.zenithAngleArduino) + ")");
}
#region analisis Flags (Limits, Status Arduino)
int valor;
valor = this.flags;
StringBuilder strBinary;
strBinary = new StringBuilder();
flag = new Boolean[10];
for (int i = 1; i < 10; i++)
{
flag[i - 1] = ((valor & (1 << i - 1)) != 0);
if (flag[i - 1])
{
strBinary.Append(1);
}
else
{
strBinary.Append(0);
}
}
this.decHome = flag[DEC_SWITCH];
this.raLimitEast = ((flag[1]) & flag[RA_SWITCH_WEST]);
if (!raLimitEast)
{
this.raHome = flag[RA_SWITCH_HOME];
this.raLimitWest = flag[RA_SWITCH_WEST];
}
this.monturaEncendida = flag[FLAG_MONTURA_ENCENDIDA];
this.monturaProtegida = flag[FLAG_MONTURA_PROTEGIDA];
#endregion
StringBuilder mensaje;
mensaje = new StringBuilder();
mensaje.Append("flags="); mensaje.Append(this.flags);
mensaje.Append("\t strBinary="); mensaje.Append(strBinary);
mensaje.Append("\t decHome="); mensaje.Append(this.decHome);
mensaje.Append("\t raLimitEast="); mensaje.Append(raLimitEast);
mensaje.Append("\t raLimitWest="); mensaje.Append(this.raLimitWest);
mensaje.Append("\t raHome="); mensaje.Append(this.raHome);
Console.WriteLine(mensaje.ToString());
}