//--------------------------------------------------------------------------- //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; }