public TimeSpan PivotDuration(AngleDelta angle, double axialDistance) { double dist = (Math.PI * axialDistance) / 360 * Math.Abs(angle.InDegrees); TimeSpan accel, maxSpeed, braking; return(DistanceDuration((int)dist, PivotAcceleration, PivotSpeed, PivotDeceleration, out accel, out maxSpeed, out braking)); }
/// <summary> /// Retourne une ligne qui est tournée de l'angle donné /// </summary> /// <param name="angle">Angle de rotation</param> /// <param name="rotationCenter">Centre de rotation, si null (0, 0) est utilisé</param> /// <returns>Droite tournée de l'angle donné</returns> public Line Rotation(AngleDelta angle, RealPoint rotationCenter = null) { RealPoint p1, p2; if (rotationCenter == null) { rotationCenter = Barycenter; } if (_c == 1) { p1 = new RealPoint(0, _a * 0 + _b); p2 = new RealPoint(1, _a * 1 + _b); p1 = p1.Rotation(angle, rotationCenter); p2 = p2.Rotation(angle, rotationCenter); } else { p1 = new RealPoint(_b, 0); p2 = new RealPoint(_b, 1); } return(new Line(p1, p2)); }
private List <RealPoint> ValuesToPositions(List <double> measures, bool limitOnTable, int minDistance, int maxDistance, Position refPosition) { List <RealPoint> positions = new List <RealPoint>(); AngleDelta resolution = _scanRange / _pointsCount; for (int i = 0; i < measures.Count; i++) { AnglePosition angle = resolution * i; if (measures[i] > minDistance && (measures[i] < maxDistance || maxDistance == -1)) { AnglePosition anglePoint = new AnglePosition(angle.InPositiveRadians - refPosition.Angle.InPositiveRadians - _scanRange.InRadians / 2 - Math.PI / 2, AngleType.Radian); RealPoint pos = new RealPoint(refPosition.Coordinates.X - anglePoint.Sin * measures[i], refPosition.Coordinates.Y - anglePoint.Cos * measures[i]); int marge = 20; // Marge en mm de distance de detection à l'exterieur de la table (pour ne pas jeter les mesures de la bordure qui ne collent pas parfaitement) if (!limitOnTable || (pos.X > -marge && pos.X < GameBoard.Width + marge && pos.Y > -marge && pos.Y < GameBoard.Height + marge)) { positions.Add(pos); } } } return(positions); }
public LidarSimu() : base() { _distances = Enumerable.Repeat(1500d, 360 * 3).ToList(); _scanRange = 360; _pointsCount = 360 * 3; _noise = 5; _rand = new Random(); }
public void TestAngleDeltaDegRad() { AngleDelta deg = new AngleDelta(720, AngleType.Degre); AngleDelta rad = new AngleDelta(Math.PI * 4, AngleType.Radian); Assert.AreEqual(deg, rad); Assert.AreEqual(deg, 720, AngleDelta.PRECISION); }
public ActionVirage(Robot r, int dist, AngleDelta a, SensAR ar, SensGD gd) { robot = r; distance = dist; angle = a; sensAR = ar; sensGD = gd; }
/// <summary> /// Retourne un cercle qui est tourné de l'angle donné /// </summary> /// <param name="angle">Angle de rotation</param> /// <param name="rotationCenter">Centre de rotation, si null le barycentre est utilisé</param> /// <returns>Cercle tourné de l'angle donné</returns> public Circle Rotation(AngleDelta angle, RealPoint rotationCenter = null) { if (rotationCenter == null) { rotationCenter = Barycenter; } return(new Circle(_center.Rotation(angle, rotationCenter), _radius)); }
/// <summary> /// Retourne un segment qui est tournée de l'angle donné /// </summary> /// <param name="angle">Angle de rotation</param> /// <param name="rotationCenter">Centre de rotation, si null (0, 0) est utilisé</param> /// <returns>Segment tourné de l'angle donné</returns> public new Segment Rotation(AngleDelta angle, RealPoint rotationCenter = null) { if (rotationCenter == null) { rotationCenter = Barycenter; } return(new Segment(_startPoint.Rotation(angle, rotationCenter), _endPoint.Rotation(angle, rotationCenter))); }
public void TestAngleDeltaTrigo() { double a = Math.PI / 2.54; // Arbitraire AngleDelta deg = new AngleDelta(a, AngleType.Radian); Assert.AreEqual(Math.Cos(a), deg.Cos, AngleDelta.PRECISION); Assert.AreEqual(Math.Sin(a), deg.Sin, AngleDelta.PRECISION); Assert.AreEqual(Math.Tan(a), deg.Tan, AngleDelta.PRECISION); }
public void TestAnglePositionSub1() { AnglePosition a30 = new AnglePosition(30); AnglePosition a350 = new AnglePosition(350); AngleDelta da30 = new AngleDelta(30); AngleDelta da350 = new AngleDelta(350); Assert.AreEqual(40, (a30 - da350).InDegrees, AnglePosition.PRECISION); Assert.AreEqual(-40, (a350 - da30).InDegrees, AnglePosition.PRECISION); }
public virtual void Pivot(AngleDelta angle, bool waitEnd = true) { if (angle > 0) { PivotLeft(angle, waitEnd); } else { PivotRight(-angle, waitEnd); } }
static public Frame Pivot(SensGD sens, AngleDelta angle, Robot robot) { byte[] tab = new byte[7]; tab[0] = (byte)robot.AsservBoard; tab[1] = (byte)UdpFrameFunction.Pivot; tab[2] = (byte)sens; tab[3] = ByteDivide((int)(angle * 100.0), true); tab[4] = ByteDivide((int)(angle * 100.0), false); Frame retour = new Frame(tab); return(retour); }
public void TestAnglePositionAdd() { AnglePosition a10 = new AnglePosition(10); AnglePosition a190 = new AnglePosition(190); AnglePosition a350 = new AnglePosition(350); AngleDelta da10 = new AngleDelta(10); AngleDelta da80 = new AngleDelta(80); AngleDelta da20 = new AngleDelta(20); Assert.AreEqual(0, (a350 + da10).InDegrees, AnglePosition.PRECISION); Assert.AreEqual(10, (a350 + da20).InDegrees, AnglePosition.PRECISION); Assert.AreEqual(-90, (a190 + da80).InDegrees, AnglePosition.PRECISION); Assert.AreEqual(20, (a10 + da10).InDegrees, AnglePosition.PRECISION); }
static public Frame Virage(SensAR sensAr, SensGD sensGd, int rayon, AngleDelta angle, Robot robot) { byte[] tab = new byte[8]; tab[0] = (byte)robot.AsservBoard; tab[1] = (byte)UdpFrameFunction.Virage; tab[2] = (byte)sensAr; tab[3] = (byte)sensGd; tab[4] = (byte)ByteDivide(rayon, true); tab[5] = (byte)ByteDivide(rayon, false); tab[6] = (byte)ByteDivide((int)(angle * 100), true); tab[7] = (byte)ByteDivide((int)(angle * 100), false); Frame retour = new Frame(tab); return(retour); }
/// <summary> /// Retourne un polygone qui est tourné de l'angle donné /// </summary> /// <param name="angle">Angle de rotation</param> /// <param name="rotationCenter">Centre de rotation, si null le barycentre est utilisé</param> /// <returns>Polygone tourné de l'angle donné</returns> public Polygon Rotation(AngleDelta angle, RealPoint rotationCenter = null) { if (rotationCenter == null) { rotationCenter = Barycenter; } Polygon output = new Polygon(Points.ConvertAll(p => p.Rotation(angle, rotationCenter)), false); if (_barycenter.Computed && _barycenter.Value == rotationCenter) { output._barycenter.Value = new RealPoint(_barycenter.Value); } return(output); }
public void GoToAngle(AnglePosition angle, double marge = 0) { AngleDelta diff = angle - Position.Angle; if (Math.Abs(diff.InDegrees) > marge) { if (diff.InDegrees > 0) { PivotRight(diff.InDegrees); } else { PivotLeft(-diff.InDegrees); } } }
public override void PivotRight(AngleDelta angle, bool wait = true) { base.PivotRight(angle, wait); angle = Math.Round(angle, 2); Historique.AjouterAction(new ActionPivot(this, angle, SensGD.Droite)); _destination = new Position(Position.Angle + angle, new RealPoint(Position.Coordinates.X, Position.Coordinates.Y)); _sensPivot = SensGD.Droite; if (wait) { while (Position.Angle != _destination.Angle) { Thread.Sleep(10); } } }
public override void Turn(SensAR sensAr, SensGD sensGd, int radius, AngleDelta angle, bool wait = true) { if (wait) { _lockFrame[UdpFrameFunction.FinDeplacement] = new Semaphore(0, int.MaxValue); } Frame frame = UdpFrameFactory.Virage(sensAr, sensGd, radius, angle, this); _asserConnection.SendMessage(frame); Historique.AjouterAction(new ActionVirage(this, radius, angle, sensAr, sensGd)); if (wait) { _lockFrame[UdpFrameFunction.FinDeplacement].WaitOne(); } }
public override void PivotRight(AngleDelta angle, bool wait = true) { base.PivotRight(angle, wait); if (wait) { _lockFrame[UdpFrameFunction.FinDeplacement] = new Semaphore(0, int.MaxValue); } Frame frame = UdpFrameFactory.Pivot(SensGD.Droite, angle, this); _asserConnection.SendMessage(frame); Historique.AjouterAction(new ActionPivot(this, angle, SensGD.Droite)); if (wait) { //if (!SemaphoresTrame[FrameFunction.FinDeplacement].WaitOne((int)SpeedConfig.PivotDuration(angle, Entraxe).TotalMilliseconds)) // Thread.Sleep(1000); // Tempo de secours, on a jamais reçu la fin de trajectoire après la fin du délai théorique _lockFrame[UdpFrameFunction.FinDeplacement].WaitOne(); } }
private List <RealPoint> Detection(Position refPosition) { List <RealPoint> positions = new List <RealPoint>(); AngleDelta resolution = _scanRange / _pointsCount; int dist = 10000; List <IShape> obstacles = new List <IShape>(); obstacles.AddRange(GameBoard.ObstaclesLidarGround); for (int i = 0; i < _pointsCount; i++) { AnglePosition angle = resolution * i; AnglePosition anglePoint = new AnglePosition(angle.InPositiveRadians - refPosition.Angle.InPositiveRadians - _scanRange.InRadians / 2 - Math.PI / 2, AngleType.Radian); RealPoint pos = new RealPoint(refPosition.Coordinates.X - anglePoint.Sin * dist, refPosition.Coordinates.Y - anglePoint.Cos * dist); positions.Add(GetDetectionLinePoint(new Segment(refPosition.Coordinates, pos), obstacles)); } return(positions); }
private void IdentifyModel() { SendMessage(_frameDetails); String response = GetResponse(); List <String> details = response.Split(new char[] { '\n', ':', ';' }).ToList(); _romVendor = details[3]; _romProduct = details[6]; _romFirmware = details[9]; _romProtocol = details[12]; _romSerialNumber = details[15]; SendMessage(_frameSpecif); response = GetResponse(); details = response.Split(new char[] { '\n', ':', ';' }).ToList(); _romModel = details[3]; _romMinDistance = int.Parse(details[6]); _romMaxDistance = int.Parse(details[9]); _romDivisions = int.Parse(details[12]); _romFirstMeasure = int.Parse(details[15]); _romLastMeasure = int.Parse(details[18]); _romTotalMeasures = int.Parse(details[21]) * 2 + 1; _romMotorSpeed = int.Parse(details[24]); _usefullMeasures = _romTotalMeasures - (_romTotalMeasures - (_romLastMeasure + 1)) - _romFirstMeasure; _resolution = 360.0 / _romDivisions; _scanRange = _resolution * _usefullMeasures; _keepFrom = _romFirstMeasure; _keepTo = _romFirstMeasure + _usefullMeasures - 1; _frameMeasure = "MS" + _keepFrom.ToString("0000") + _keepTo.ToString("0000") + "00001\n"; }
public abstract void Turn(SensAR sensAr, SensGD sensGd, int radius, AngleDelta angle, bool waitEnd = true);
public virtual void PivotRight(AngleDelta angle, bool waitEnd = true) { AsserStats.RightsRotations.Add(angle); }
public ActionPivot(Robot r, AngleDelta a, SensGD s) { robot = r; angle = a; sens = s; }
public virtual void PivotLeft(AngleDelta angle, bool waitEnd = true) { AsserStats.LeftRotations.Add(angle); }
private void Asservissement() { _linkAsserv.RegisterName(); // Calcul du temps écoulé depuis la dernière mise à jour de la position double interval = 0; if (_asserChrono != null) { long currentTick = _asserChrono.ElapsedMilliseconds; interval = currentTick - _lastTimerTick; _lastTimerTick = currentTick; } else { _asserChrono = Stopwatch.StartNew(); } if (interval > 0) { _lockMove.WaitOne(); if (_currentPolarPoint >= 0) { double distanceBeforeNext = Position.Coordinates.Distance(_polarTrajectory[_currentPolarPoint]); double distanceBeforeEnd = distanceBeforeNext; distanceBeforeEnd += DistanceBetween(_polarTrajectory, _currentPolarPoint, _polarTrajectory.Count - 1); if (distanceBeforeEnd > GetCurrentAngleBreakDistance()) { _currentSpeed = Math.Min(SpeedConfig.LineSpeed, _currentSpeed + SpeedConfig.LineAcceleration / (1000.0 / interval)); } else { _currentSpeed = _currentSpeed - SpeedConfig.LineDeceleration / (1000.0 / interval); } double distanceToRun = _currentSpeed / (1000.0 / interval); double distanceTested = distanceBeforeNext; bool changePoint = false; while (distanceTested < distanceToRun && _currentPolarPoint < _polarTrajectory.Count - 1) { _currentPolarPoint++; distanceTested += _polarTrajectory[_currentPolarPoint - 1].Distance(_polarTrajectory[_currentPolarPoint]); changePoint = true; } Segment segment; Circle circle; if (changePoint) { segment = new Segment(_polarTrajectory[_currentPolarPoint - 1], _polarTrajectory[_currentPolarPoint]); circle = new Circle(_polarTrajectory[_currentPolarPoint - 1], distanceToRun - (distanceTested - _polarTrajectory[_currentPolarPoint - 1].Distance(_polarTrajectory[_currentPolarPoint]))); } else { segment = new Segment(Position.Coordinates, _polarTrajectory[_currentPolarPoint]); circle = new Circle(Position.Coordinates, distanceToRun); } RealPoint newPos = segment.GetCrossingPoints(circle)[0]; AngleDelta a = -Maths.GetDirection(newPos, _polarTrajectory[_currentPolarPoint]).angle; _currentPosition = new Position(new AnglePosition(a), newPos); OnPositionChanged(Position); if (_currentPolarPoint == _polarTrajectory.Count - 1) { _currentPolarPoint = -1; _destination.Copy(Position); } } else { bool needLine = _destination.Coordinates.Distance(Position.Coordinates) > 0; bool needAngle = Math.Abs(_destination.Angle - Position.Angle) > 0.01; if (needAngle) { AngleDelta diff = Math.Abs(_destination.Angle - Position.Angle); double speedWithAcceleration = Math.Min(SpeedConfig.PivotSpeed, _currentSpeed + SpeedConfig.PivotAcceleration / (1000.0 / interval)); double remainingDistanceWithAcceleration = CircleArcLenght(WheelSpacing, diff) - (_currentSpeed + speedWithAcceleration) / 2 / (1000.0 / interval); if (remainingDistanceWithAcceleration > DistanceFreinage(speedWithAcceleration)) { double distParcourue = (_currentSpeed + speedWithAcceleration) / 2 / (1000.0 / interval); AngleDelta angleParcouru = (360 * distParcourue) / (Math.PI * WheelSpacing); _currentSpeed = speedWithAcceleration; Position.Angle += (_sensPivot.Factor() * angleParcouru); } else if (_currentSpeed > 0) { double speedWithDeceleration = Math.Max(0, _currentSpeed - SpeedConfig.PivotDeceleration / (1000.0 / interval)); double distParcourue = (_currentSpeed + speedWithDeceleration) / 2 / (1000.0 / interval); AngleDelta angleParcouru = (360 * distParcourue) / (Math.PI * WheelSpacing); _currentSpeed = speedWithDeceleration; Position.Angle += (_sensPivot.Factor() * angleParcouru); } else { Position.Copy(_destination); } OnPositionChanged(Position); } else if (needLine) { double speedWithAcceleration = Math.Min(SpeedConfig.LineSpeed, _currentSpeed + SpeedConfig.LineAcceleration / (1000.0 / interval)); double remainingDistanceWithAcceleration = Position.Coordinates.Distance(_destination.Coordinates) - (_currentSpeed + speedWithAcceleration) / 2 / (1000.0 / interval); // Phase accélération ou déccélération if (remainingDistanceWithAcceleration > DistanceFreinage(speedWithAcceleration)) { double distance = (_currentSpeed + speedWithAcceleration) / 2 / (1000.0 / interval); _currentSpeed = speedWithAcceleration; Position.Move(distance * _sensMove.Factor()); } else if (_currentSpeed > 0) { double speedWithDeceleration = Math.Max(0, _currentSpeed - SpeedConfig.LineDeceleration / (1000.0 / interval)); double distance = Math.Min(_destination.Coordinates.Distance(Position.Coordinates), (_currentSpeed + speedWithDeceleration) / 2 / (1000.0 / interval)); _currentSpeed = speedWithDeceleration; Position.Move(distance * _sensMove.Factor()); } else { // Si on est déjà à l'arrêt on force l'équivalence de la position avec la destination. Position.Copy(_destination); IsInLineMove = false; } OnPositionChanged(Position); } } _lockMove.Release(); } }
/// <summary> /// Convertit la trajectoire en suite d'actions de déplacement à effectuer pour suivre la trajectoire /// </summary> /// <returns>Liste des déplacements correspondant</returns> public List <ITimeableAction> ConvertToActions(Robot robot) { List <ITimeableAction> actions = new List <ITimeableAction>(); AnglePosition angle = _startAngle; for (int i = 0; i < Points.Count - 1; i++) { RealPoint c1 = new RealPoint(Points[i].X, Points[i].Y); RealPoint c2 = new RealPoint(Points[i + 1].X, Points[i + 1].Y); Position p = new Position(angle, c1); Direction traj = Maths.GetDirection(p, c2); // Désactivation 2019 de la possibilité de faire des marches arrière pour conserver le LDIAR d'évitement dans le sens de déplacement du robot bool canReverse = false; // Teste si il est plus rapide (moins d'angle à tourner) de se déplacer en marche arrière avant la fin bool inverse = false; if (canReverse) { if (i < _points.Count - 2) { inverse = Math.Abs(traj.angle) > 90; } else { // On cherche à minimiser le tout dernier angle quand on fait l'avant dernier AnglePosition finalAngle = angle - traj.angle; inverse = Math.Abs(finalAngle - _endAngle) > 90; } if (inverse) { traj.angle = new AngleDelta(traj.angle - 180); } } traj.angle.Modulo(); if (traj.angle < 0) { actions.Add(new ActionPivot(robot, -traj.angle, SensGD.Droite)); angle -= traj.angle; } else if (traj.angle > 0) { actions.Add(new ActionPivot(robot, traj.angle, SensGD.Gauche)); angle -= traj.angle; } if (inverse) { actions.Add(new ActionRecule(robot, (int)(traj.distance))); } else { actions.Add(new ActionAvance(robot, (int)(traj.distance))); } } AngleDelta diff = angle - _endAngle; if (Math.Abs(diff) > 0.2) // Angle minimal en dessous duquel on considère qu'il n'y a pas besoin d'effectuer le pivot { if (diff < 0) { actions.Add(new ActionPivot(robot, -diff, SensGD.Droite)); } else { actions.Add(new ActionPivot(robot, diff, SensGD.Gauche)); } } return(actions); }
private double CircleArcLenght(double diameter, AngleDelta arc) { return(Math.Abs(arc.InDegrees) / 360 * Math.PI * diameter); }
public override void Turn(SensAR sensAr, SensGD sensGd, int rayon, AngleDelta angle, bool wait = true) { // TODO2018 }
public bool DoSearchBuoy(Color color, IShape containerZone = null) { List <RealPoint> pts = AllDevices.LidarGround.GetPoints(); pts = pts.Where(o => GameBoard.IsInside(o, 50)).ToList(); if (containerZone != null) { pts = pts.Where(o => containerZone.Contains(o)).ToList(); } List <List <RealPoint> > groups = pts.GroupByDistance(80); List <Tuple <Circle, Color> > buoys = new List <Tuple <Circle, Color> >(); bool output = false; for (int i = 0; i < groups.Count; i++) { if (groups[i].Count > 4) { RealPoint center = groups[i].GetBarycenter(); double var = Math.Sqrt(groups[i].Average(p => p.Distance(center) * p.Distance(center))) * 2; buoys.Add(Tuple.Create(new Circle(center, var), var > 35 ? Buoy.Green : Buoy.Red)); } } if (buoys.Count > 0 && buoys.Exists(b => b.Item2 == color)) { Circle buoy = buoys.OrderBy(b => b.Item1.Distance(Robots.MainRobot.Position.Coordinates)).First(b => b.Item2 == color).Item1; RealPoint entryFrontPoint = GetEntryFrontPoint(); RealPoint entryBackPoint = GetEntryBackPoint(); AngleDelta bestAngle = 0; double bestError = int.MaxValue; for (AngleDelta i = 0; i < 360; i++) { Segment inter = new Segment(entryBackPoint.Rotation(i, Robots.MainRobot.Position.Coordinates), buoy.Center); double error = inter.Distance(entryFrontPoint.Rotation(i, Robots.MainRobot.Position.Coordinates)); if (error < bestError) { bestError = error; bestAngle = i; } } bestAngle = -bestAngle.Modulo(); if (bestAngle < 0) { Robots.MainRobot.PivotRight(-bestAngle); } else { Robots.MainRobot.PivotLeft(bestAngle); } DoGrabOpen(); int dist = (int)GetEntryFrontPoint().Distance(buoy.Center) + 50; Robots.MainRobot.Move(dist); DoSequencePickupColor(color); // TODO Détecter la couleur au lidar ? //DoSequencePickup();// ... ou pas... DoGrabClose(); Robots.MainRobot.Move(-dist); //DoSearchBuoy(); output = true; if (Robots.Simulation) { GameBoard.RemoveVirtualBuoy(buoy.Center); } } else { output = false; } return(output); }