//---------------------------------------------------------------------------
//void LancerMouvement( tModeDeMouvement MM, tPoint2DPlus &PF)
public static void LancerMouvement(uPanel.tModeDeMouvement MM, uPanel.tPoint2DPlus PF) // (* envoyer lorsque prêt... *)
{
P0RobotFutur = PF;
uPanel.Mouvement.ModeDeMouvement = MM;
uMTasks.Work[uPanel.NTMouvementSpat].TaskStatus = uMTasks.tPhase.Demandee;
return;
}
//---------------------------------------------------------------------------
//void Diff2DPlus(tPoint2DPlus &P0, tPoint2DPlus &PF,
// tPoint2DPlus &resultat)
public static void Diff2DPlus(uPanel.tPoint2DPlus P0, uPanel.tPoint2DPlus PF, ref uPanel.tPoint2DPlus resultat)
{
resultat.x = PF.x - P0.x;
resultat.y = PF.y - P0.y;
resultat.ThetaRobotDegres = PF.ThetaRobotDegres - P0.ThetaRobotDegres;
return;
}
//---------------------------------------------------------------------------
public static void MiseAJourPosition()
{
uPanel.tPoint2DPlus PRobotRobotFutur;
switch (uPanel.Mouvement.ModeDeMouvement)
{
//(* XY:; R :; MDMds : ; ... *)
case
uPanel.tModeDeMouvement.XY:
P0Robot = uMouv.P0RobotFutur;
break; // (* XY: *)
case
uPanel.tModeDeMouvement.Rot:
P0Robot.ThetaRobotDegres = P0RobotFutur.ThetaRobotDegres;
// P0Robot.ThetaRobotDegres=P0RobotTemp.ThetaRobotDegres
// +DThetaRobotDegresTemp; // 005.12.20 JDZ
break; // (* R: *)
case
uPanel.tModeDeMouvement.MDMds: // (* "code": ds=P0RobotFutur.x *)
P0Robot.x = P0RobotTemp.x + Convert.ToSingle(P0RobotFutur.ds * Math.Cos(DegresARadians(uPiaget.Round(P0Robot.ThetaRobotDegres))));
P0Robot.y = P0RobotTemp.y + Convert.ToSingle(P0RobotFutur.ds * Math.Sin(DegresARadians(uPiaget.Round(P0Robot.ThetaRobotDegres))));
break; // (* MDMds: *)
case
uPanel.tModeDeMouvement.RotExc:
// (* Point Futur dans le repère "robot" *)
PRobotRobotFutur.x = -Convert.ToSingle(P0RobotFutur.RayonTrajectoire * Math.Sin(DegresARadians(uPiaget.Round(P0RobotFutur.AngleTrajectoire))));
PRobotRobotFutur.y = -Convert.ToSingle(P0RobotFutur.RayonTrajectoire * (1 - Math.Cos(DegresARadians(uPiaget.Round(P0RobotFutur.AngleTrajectoire)))));
// (* Point Futur dans le repère "terrain" *)
P0Robot.x = P0RobotTemp.x + PRobotRobotFutur.x * Convert.ToSingle(Math.Cos(DegresARadians(uPiaget.Round(P0RobotTemp.ThetaRobotDegres)))
- PRobotRobotFutur.y * Math.Sin(DegresARadians(uPiaget.Round(P0RobotTemp.ThetaRobotDegres))));
P0Robot.y = P0RobotTemp.y + Convert.ToSingle(PRobotRobotFutur.x * Math.Sin(DegresARadians(uPiaget.Round(P0RobotTemp.ThetaRobotDegres)))
+ PRobotRobotFutur.y * Math.Cos(DegresARadians(uPiaget.Round(P0RobotTemp.ThetaRobotDegres))));
// (* Mise … jour des orientations *)
P0Robot.ThetaRobotDegres = P0RobotTemp.ThetaRobotDegres + P0RobotFutur.AngleTrajectoire;
break; //
case
uPanel.tModeDeMouvement.RXY:
P0Robot.ThetaRobotDegres = P0RobotFutur.ThetaRobotDegres;
break; // (* RXY: *)
case
uPanel.tModeDeMouvement.Lat: // (* "code": ds=P0RobotFutur.x *)
P0Robot.x = P0RobotTemp.x + Convert.ToSingle(P0RobotFutur.ds *
Math.Cos(DegresARadians(uPiaget.Round(P0Robot.ThetaRobotDegres + 90))));
P0Robot.y = P0RobotTemp.y + Convert.ToSingle(P0RobotFutur.ds *
Math.Sin(DegresARadians(uPiaget.Round(P0Robot.ThetaRobotDegres + 90))));
break; // Lat:
} // (* case *)
return;
}
public static void MatriceAPoint(uPanel.tMatriceTransf m, ref uPanel.tPoint2DPlus pt)
{
// (* changement du vecteur position *)
pt.x = m.p.x;
pt.y = m.p.y;
// (* angle *)
pt.ThetaRobotDegres =
uMouv.RadiansADegres(uMouv.Atan24(m.u.y, m.u.x));
pt.ThetaRobotDegres =
uMouv.DeballerDegres(uPiaget.Round(pt.ThetaRobotDegres));
} // end MatriceAPoint
public static void PointAMatrice(uPanel.tPoint2DPlus pt, ref uPanel.tMatriceTransf m)
{
float ThetaTemp, CTemp, STemp;
ThetaTemp = uMouv.DegresARadians(uPiaget.Round(pt.ThetaRobotDegres));
CTemp = Convert.ToSingle(Math.Cos(ThetaTemp));
STemp = Convert.ToSingle(Math.Sin(ThetaTemp));
// (* changement du vecteur position *)
m.p.x = pt.x;
m.p.y = pt.y;
// (* sous-matrice de rotation *)
m.u.x = CTemp; m.v.x = -STemp;
m.u.y = STemp; m.v.y = CTemp;
}