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()); }