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