/// <summary>
/// returns the calculated angles
/// </summary>
/// <returns>Angles angles</returns>
public Model.Angles GetCalculatedAngles()
{
Model.Angles angles = new Model.Angles();
double R = CalculateR();
double D = Math.Sqrt((handLenght - baseHeight) * (handLenght - baseHeight) + R * R);
angles.PanAngle = CalculatePanAngle(R, D);
angles.TiltAngle = CalculateTiltAngle(R);
angles.ElbowAngle = CalculateElbowAngle(R, D);
angles.HandAngle = CalculateHandAngle(angles.PanAngle, angles.ElbowAngle);
angles.ElbowAngle = (byte)(180 - angles.ElbowAngle);
return(angles);
}
}/// <summary>
/// Sends the calculated positions to the arduino
/// </summary>
/// <param name="angles"></param>
public void WriteAnglesToSerial(Model.Angles angles)
{
port.Open();
port.Write("h");//hand
PutZerosBeforeNumberIfNeceserry(angles.HandAngle, port);
port.Write(Convert.ToString(angles.HandAngle));
port.Write("e");//elbow
PutZerosBeforeNumberIfNeceserry(angles.ElbowAngle, port);
port.Write(Convert.ToString(angles.ElbowAngle));
port.Write("p"); //shoulderPan
PutZerosBeforeNumberIfNeceserry(angles.PanAngle, port);
port.Write(Convert.ToString(angles.PanAngle));
port.Write("t");//shoulderTilt
PutZerosBeforeNumberIfNeceserry(angles.TiltAngle, port);
port.Write(Convert.ToString(angles.TiltAngle));
port.Close();
}
