public TDx.TDxInput.Vector3D Suppimmer_Bruit_Translation(TDx.TDxInput.Vector3D translation)
{
if (translation.X > 500)
{
translation.Y = 0;
translation.Z = 0;
}
else if (translation.X < -500)
{
translation.Y = 0;
translation.Z = 0;
}
else if (translation.Y > 500)
{
translation.X = 0;
translation.Z = 0;
}
else if (translation.Y < -500)
{
translation.X = 0;
translation.Z = 0;
}
else if (translation.Z > 500)
{
translation.X = 0;
translation.Y = 0;
}
else if (translation.Z < -500)
{
translation.X = 0;
translation.Y = 0;
}
return(translation);
}
private volatile bool _shouldStop; // Attribut qui permet d'arreter le thread et accessible par d'autre thread (volatile)
#endregion
#region Constructeur
/// <summary>
/// Fonction qui initialise la classe
/// </summary>
public Mouse()
{
#region Mouse connection
Mouse6d = new TDx.TDxInput.Device();
if (Mouse6d != null)
{
Mouse6d.Connect();
}
#endregion
MoveByVector = new TDx.TDxInput.Vector3D(0.0, 0.0, 0.0);
RotateByVector = new TDx.TDxInput.Vector3D(0.0, 0.0, 0.0);
}
public RobotActions()
{
liste_temp = new List <CartesianPosition>();
/*init*/
vecteur = new TDx.TDxInput.Vector3D();
point_relatif = new CartesianPosition();
liste_aller_magasin = new List <CartesianPosition>();
liste_aller_plateau = new List <CartesianPosition>();
liste_placer_piece = new List <CartesianPosition>();
liste_retour_magasin = new List <CartesianPosition>();
plateau = new Plateau().GetPlateau();
}
// Fonction permettant d'utiliser les translations et rotations de la souris et de les envoyer au Kuka
public void Kuka_Move(TDx.TDxInput.Vector3D translation, TDx.TDxInput.AngleAxis rotation)
{
if (debug)
{
Console.WriteLine("je suis dans kuka move");
var Position = (new CartesianPosition
{
X = translation.X,
Y = translation.Y,
Z = translation.Z,
A = rotation.X,
B = rotation.Y,
C = rotation.Z,
});
Console.WriteLine(Position.X + " ; " + Position.Y + " ; " + Position.Z);
// on envoi ces valeurs au Kuka
robot.SetRelativeMovement(Position);
}
// on recupere les valeurs des translations et rotations de la souris 3D
}
/// <summary>
///
/// </summary>
public void Loop()
{
#region Variables
TDx.TDxInput.Vector3D VectorNorm = new TDx.TDxInput.Vector3D();
TDx.TDxInput.Vector3D Translation;
TDx.TDxInput.AngleAxis Rotation;
var Norm = Math.Sqrt(Math.Pow(MaxTransX, 2) + Math.Pow(MaxTransY, 2) + Math.Pow(MaxTransZ, 2));
#endregion
#region Loop which get the information from the mouse and convert to the movement of the robot
while (Mouse6d.IsConnected || !_shouldStop)
{
Translation = Mouse6d.Sensor.Translation;
Rotation = Mouse6d.Sensor.Rotation;
#region Normalization of the vector of the mouse
VectorNorm.X = Translation.X / Norm;
VectorNorm.Y = Translation.Y / Norm;
VectorNorm.Z = Translation.Z / Norm;
#endregion
#region Error due to a vector's component upper than 1.0
if (VectorNorm.X > 1.0 || VectorNorm.Y > 1.0 || VectorNorm.Z > 1.0)
{
Console.WriteLine("Error, vector > 1");
_shouldStop = true;
}
#endregion
#region Movement vector send to the robot
// Translation alone
if (Math.Abs(VectorNorm.X) > Treshold || Math.Abs(VectorNorm.Y) > Treshold || Math.Abs(VectorNorm.Z) > Treshold)
{
MoveByVector.X = VectorNorm.X * VitesseTranslation;
MoveByVector.Y = VectorNorm.Y * VitesseTranslation;
MoveByVector.Z = VectorNorm.Z * VitesseTranslation;
RotateByVector.X = 0.0;
RotateByVector.Y = 0.0;
RotateByVector.Z = 0.0;
}
// Rotation alone
else if (Math.Abs(Rotation.X) > Treshold || Math.Abs(Rotation.Y) > Treshold || Math.Abs(Rotation.Z) > Treshold)
{
MoveByVector.X = 0.0;
MoveByVector.Y = 0.0;
MoveByVector.Z = 0.0;
#region Rotation X first
if (Math.Abs(Rotation.X) > Math.Abs(Rotation.Y) && Math.Abs(Rotation.X) > Math.Abs(Rotation.Z))
{
if (Rotation.X > 0)
{
RotateByVector.X = Rotation.Angle * VitesseRotation;
}
else
{
RotateByVector.X = -Rotation.Angle * VitesseRotation;
}
RotateByVector.Y = 0.0;
RotateByVector.Z = 0.0;
}
#endregion
#region Rotation Y first
if (Math.Abs(Rotation.Y) > Math.Abs(Rotation.X) && Math.Abs(Rotation.Y) > Math.Abs(Rotation.Z))
{
RotateByVector.X = 0.0;
if (Rotation.Y > 0)
{
RotateByVector.Y = Rotation.Angle * VitesseRotation;
}
else
{
RotateByVector.Y = -Rotation.Angle * VitesseRotation;
}
RotateByVector.Z = 0.0;
}
#endregion
#region Rotation Z first
if (Math.Abs(Rotation.Z) > Math.Abs(Rotation.Y) && Math.Abs(Rotation.Z) > Math.Abs(Rotation.X))
{
RotateByVector.X = 0.0;
RotateByVector.Y = 0.0;
if (Rotation.Z > 0)
{
RotateByVector.Z = Rotation.Angle * VitesseRotation;
}
else
{
RotateByVector.Z = -Rotation.Angle * VitesseRotation;
}
}
#endregion
}
else
{
MoveByVector.X = 0.0;
MoveByVector.Y = 0.0;
MoveByVector.Z = 0.0;
RotateByVector.X = 0.0;
RotateByVector.Y = 0.0;
RotateByVector.Z = 0.0;
}
#endregion
Console.WriteLine("X: {0} | Y: {1} | Z: {2}", RotateByVector.X, RotateByVector.Y, RotateByVector.Z);
}
#endregion
}