/// <summary>
/// Moving window distance calculator. returns the distance between two coordinates given their lat and long values
/// </summary>
/// <param name="latitude1">Latitude Point1</param>
/// <param name="longitude2">Longitude Point1</param>
/// <param name="latitude3">Latitude Point2</param>
/// <param name="longitude4">Longitude Point2</param>
/// <returns></returns>
public float DistanceWindow(float latitude1, float longitude2, float latitude3, float longitude4)
{
distance = UtilsMath.CalculateDistance(latitude1, longitude2, latitude3, longitude4);
float distanceFloat = (float)(Math.Round((double)distance, 2));
return(distanceFloat);
}
// Trouve l'objectif le plus près de la position (objectif non effectué )
private Objectif findObjectifNear(PositionElement p)
{
Objectif sortie = null;
double MinDist = -1;
foreach (Objectif ob in _Cubes)
{
// Objectif non fait et non en cours de traitement
if (ob.Done == false && ob.Robot == null)
{
double dist = UtilsMath.DistanceManhattan(p, ob.position);
if (sortie == null) // On a pas encore d'objectif ?
{
sortie = ob;
MinDist = dist;
}
else
{
if (MinDist > dist)
{
sortie = ob;
MinDist = dist;
}
}
}
}
return(sortie);
}
private AStar CreerAstarDepose(ArduinoBotIA robot, Zone Depose)
{
PositionElement PositionCentreZone = UtilsMath.CentreRectangle(Depose.position);
AStar AS = new AStar(PositionCentreZone, robot.Position, _ZoneTravail);
// Ajout des autres cubes en obstacles
foreach (Objectif o in _Cubes)
{
if (o.Robot != null)
{
AS.AddObstacle(o.position);
}
}
// Ajout des Zones en Obstacles
foreach (Zone o in _ZonesDepose)
{
if (o.id != Depose.id)
{
AS.AddObstacle(UtilsMath.CentreRectangle(o.position));
}
}
return(AS);
}
// Verifie si un robot est bien proche du tracé qu'il doit effectuer
private bool checkProximiteTrace(byte idRobot)
{
// On a bien un robot avec ce numéro
if (_ListArduino.Exists(ArduinoBotIA.ById(idRobot)))
{
ArduinoBotIA Robot = _ListArduino.Find(ArduinoBotIA.ById(idRobot));
// Le robot a bien un tracé définit
if (Robot.Trace != null)
{
Track tr = Robot.Trace;
double minDistance = -1;
for (int i = 0; i < tr.Positions.Count - 1; i++)
{
// Trouve la distance la plus petite entre la position actuelle et les traits du tracé
double tmp = UtilsMath.FindDistanceToSegment(Robot.Position, tr.Positions[i], tr.Positions[i + 1]);
if (minDistance == -1 || minDistance > tmp)
{
minDistance = tmp;
}
}
if (minDistance >= _DistanceMaximumRecal)
{
Logger.GlobalLogger.info("Robot trop éloigné de son tracé, Recalcul de l'itinéraire ");
return(true);
}
}
}
return(false);
}
// Verifie si le robot est proche d'un point de l'itinéraire pour changer passer a la suite
private bool checkSuiteItineraire(byte idRobot, out PositionElement NearestPositionTroncon)
{
NearestPositionTroncon = new PositionElement();
// On a bien un robot avec ce numéro
if (_ListArduino.Exists(ArduinoBotIA.ById(idRobot)))
{
ArduinoBotIA Robot = _ListArduino.Find(ArduinoBotIA.ById(idRobot));
// Le robot a bien un tracé définit
if (Robot.Trace != null)
{
Track tr = Robot.Trace;
for (int i = tr.Positions.Count - 1; i >= 0; i--) // On parcours le tracé depuis la fin pour trouver le plus proche de lobjectif
{
// On est assez proche du point, on passe à la suite
if (UtilsMath.DistanceEuclidienne(tr.Positions[i], Robot.Position) < _RadiusNextItineraire)
{
NearestPositionTroncon = tr.Positions[i];
return(true);
}
}
}
}
return(false);
}
public void AccermanFunctionTest()
{
Assert.That(4, Is.EqualTo(UtilsMath.AccermanFunction(1, 2)));
Assert.That(2, Is.EqualTo(UtilsMath.AccermanFunction(0, 1)));
Assert.That(1, Is.EqualTo(UtilsMath.AccermanFunction(0, 0)));
Assert.That(7, Is.EqualTo(UtilsMath.AccermanFunction(2, 2)));
}
public void GetShortestWordsTest()
{
var aProgressionTrue = new[] { 2, 4, 6, 8 };
var aProgressionFalse = new[] { 2, 5, 6, 8 };
var gProgressionTrue = new[] { 2, 4, 8, 16, 32, 64, 128, 256 };
var gProgressionFalse = new[] { 2, 4, 16, 32 };
Assert.That((true, false), Is.EqualTo(UtilsMath.IsProgression(aProgressionTrue)));
Assert.That((false, false), Is.EqualTo(UtilsMath.IsProgression(aProgressionFalse)));
Assert.That((false, true), Is.EqualTo(UtilsMath.IsProgression(gProgressionTrue)));
Assert.That((false, false), Is.EqualTo(UtilsMath.IsProgression(gProgressionFalse)));
}
/// <summary>
/// Calculates the distancs between two coordinates using an external formula. (Haversine)
/// </summary>
/// <param name="infraLatitude">Latitude of the infrastructure</param>
/// <param name="infraLongitude">Longitude of the infrastructure</param>
/// <returns></returns>
public float Location(float infraLatitude, float infraLongitude)
{
//overwrite current lat and lon everytime
currentLatitude = GPS.Instance.latitude;
currentLongitude = GPS.Instance.longitude;
//calculate the distance between where the player was when the app started and where they are now.
//Debug.LogWarning("[Location Handler]: lat and long: " + currentLatitude + " - " + currentLongitude);
distance = UtilsMath.CalculateDistance(GPS.Instance.latitude, GPS.Instance.longitude, infraLatitude, infraLongitude);
float distanceFloat = (float)(Math.Round((double)distance, 2));
return(distanceFloat);
}
// Calcul de l'objectif en fonction de l'etat du robot
public Track CalculerObjectif(ArduinoBotIA robot)
{
Logger.GlobalLogger.debug("Creation d'une trajectoire pour le robot " + robot.ID + " etat Saisie : " + robot.Saisie, 2);;
if (robot.Saisie) // Si le robot a saisi un cube , retour a la base
{
if (robot.Cube != null)
{
Zone o = _ZonesDepose.Find(Zone.ById(robot.Cube.idZone));
//Track t = CreerAstarDepose(robot, o).CalculerTrajectoire();
Track t = new Track();
t.ajouterPoint(robot.Position);
t.ajouterPoint(UtilsMath.CentreRectangle(o.position));
robot.SetZoneDepose(o);
robot.SetTrace(t);
return(t);
}
else
{
return(null);
}
}
else
{
Objectif o;
if (robot.Cube != null) // On a deja un objectif
{
o = robot.Cube;
}
else // Trouver un objectif
{
o = findObjectifNear(robot.Position);
}
//Track t = CreerAstarCube(robot, o).CalculerTrajectoire();
Track t = new Track();
t.ajouterPoint(robot.Position);
t.ajouterPoint(o.position);
robot.SetObjectif(o);
o.Robot = robot;
robot.SetTrace(t);
return(t);
}
}
// Retourne la différence entre l'orientation du robot et l'axe du tracé (premier et second point)
private double diffOrientation(ArduinoBotIA robot, Track tr)
{
if (tr.Positions.Count < 2)
{
return(0);
}
double angleTrace = 0;
/* Vecteur de comparaison */
PointF A = new PointF(0, 10);
PointF B = new PointF(0, 0);
/* Vecteur de mouvement */
PointF Start = new PointF(tr.Positions[0].X, tr.Positions[0].Y);
PointF Stop = new PointF(tr.Positions[1].X, tr.Positions[1].Y);
angleTrace = UtilsMath.TrueAngleBetweenVectors(A, B, Start, Stop);
Logger.GlobalLogger.debug("Angle déplacement :" + angleTrace + "robot.Angle :" + robot.Angle);
return(UtilsMath.diffAngle(angleTrace, robot.Angle));
}
// Proximité de l'objectif ou de la zone
private bool checkProximiteObjectif(ArduinoBotIA Robot)
{
if (Robot.Saisie) // le robot à un cube de saisie
{
Zone Depose = Robot.DeposeZone;
if (UtilsMath.DistanceEuclidienne(Robot.Position, UtilsMath.CentreRectangle(Depose.position)) < _seuilProximiteObjectif)
{
Logger.GlobalLogger.info("Robot proche de la zone de dépose ! id : " + Robot.ID);
return(true);
}
}
else
{
Objectif Cube = Robot.Cube;
if (UtilsMath.DistanceEuclidienne(Robot.Position, Cube.position) < _seuilProximiteObjectif)
{
Logger.GlobalLogger.info("Robot proche du cube ! id : " + Robot.ID);
return(true);
}
}
return(false);
}
public void UpdateListAffichage()
{
// Update Robots
listAffichageArduino.Items.Clear();
//foreach (ArduinoBotIA Robot in _Follower.ListArduino)
for (int i = 0; i < _Follower.ListArduino.Count; i++)
{
ArduinoBotIA Robot = _Follower.ListArduino[i];
if (_ArduinoManager != null)
{
ArduinoBotComm RobotComm = _ArduinoManager.getArduinoBotById(Robot.ID);
ListViewItem master = new ListViewItem(Robot.ID + "");
if (RobotComm != null)
{
master.SubItems.Add(RobotComm.Connected + "");
master.SubItems.Add(RobotComm.stateComm + "");
}
else
{
master.SubItems.Add("N/A");
master.SubItems.Add("N/A");
}
master.SubItems.Add(Robot.LastAction + "");
master.SubItems.Add(Robot.Position.X + "");
master.SubItems.Add(Robot.Position.Y + "");
master.SubItems.Add(Robot.Angle + "");
listAffichageArduino.Items.Add(master);
}
}
// Update Cubes
listAffichageCubes.Items.Clear();
for (int i = 0; i < _Follower.TrackMaker.Cubes.Count; i++)
{
Objectif cube = _Follower.TrackMaker.Cubes[i];
ListViewItem master = new ListViewItem(cube.id + "");
master.SubItems.Add(cube.idZone + "");
master.SubItems.Add(cube.Done + "");
if (cube.Robot != null)
{
master.SubItems.Add(cube.Robot.ID + "");
}
else
{
master.SubItems.Add("N/A");
}
master.SubItems.Add(cube.position.X + "");
master.SubItems.Add(cube.position.Y + "");
listAffichageCubes.Items.Add(master);
}
// Update Zones
listAffichageZones.Items.Clear();
for (int i = 0; i < _Follower.TrackMaker.ZonesDepose.Count; i++)
{
Zone zone = _Follower.TrackMaker.ZonesDepose[i];
ListViewItem master = new ListViewItem(zone.id + "");
master.SubItems.Add("A : " + zone.position.A.ToString() + " B : " + zone.position.B.ToString() + " C : " + zone.position.C.ToString() + " D : " + zone.position.D.ToString());
master.SubItems.Add(UtilsMath.CentreRectangle(zone.position).ToString());
listAffichageZones.Items.Add(master);
}
}
public void UpdateImageDebug()
{
if (_Follower.TrackMaker != null && _Follower.TrackMaker.ZoneTravail.B.X != 0)
{
List <PolyligneDessin> ListePoly = new List <PolyligneDessin>(); // Liste envoyé a l'image
Bitmap bitmap = new Bitmap(_Follower.TrackMaker.ZoneTravail.B.X, _Follower.TrackMaker.ZoneTravail.B.Y);
//dessinerTrack(bitmap, tr);
// Dessiner Cubes
// foreach (Objectif obstacle in _Follower.TrackMaker.Cubes)
for (int i = 0; i < _Follower.TrackMaker.Cubes.Count; i++)
{
Objectif obstacle = _Follower.TrackMaker.Cubes[i];
PolyligneDessin p = new PolyligneDessin(Color.LimeGreen);
for (int x = obstacle.position.X - 5; x <= obstacle.position.X + 5; x++)
{
for (int y = obstacle.position.Y - 5; y <= obstacle.position.Y + 5; y++)
{
p.addPoint(new PointDessin(x, y));
}
}
ListePoly.Add(p);
dessinerPoint(bitmap, obstacle.position, Brushes.Gray);
}
foreach (QuadrillageCoord q in _Follower.TrackMaker.CreerAstarQuadriallage().CalculerQuadrillage())
{
PolyligneDessin p = new PolyligneDessin(Color.Gray);
p.addPoint(new PointDessin(q.A.X, q.A.Y));
p.addPoint(new PointDessin(q.B.X, q.B.Y));
ListePoly.Add(p);
dessinerLigne(bitmap, q.A, q.B, Color.Gray, 1);
}
for (int i = 0; i < _Follower.TrackMaker.ZonesDepose.Count; i++)
{
Zone z = _Follower.TrackMaker.ZonesDepose[i];
PositionElement pos = UtilsMath.CentreRectangle(z.position);
PolyligneDessin p = new PolyligneDessin(Color.Pink);
for (int x = pos.X - 5; x <= pos.X + 5; x++)
{
for (int y = pos.Y - 5; y <= pos.Y + 5; y++)
{
p.addPoint(new PointDessin(x, y));
}
}
ListePoly.Add(p);
dessinerPoint(bitmap, pos, Brushes.Pink);
}
for (int ab = 0; ab < _Follower.ListArduino.Count; ab++)
//foreach (ArduinoBotIA robot in _Follower.ListArduino)
{
ArduinoBotIA robot = _Follower.ListArduino[ab];
PolyligneDessin p = new PolyligneDessin(Color.Purple);
PositionElement pos = robot.Position;
for (int x = pos.X - 5; x <= pos.X + 5; x++)
{
for (int y = pos.Y - 5; y <= pos.Y + 5; y++)
{
p.addPoint(new PointDessin(x, y));
}
}
ListePoly.Add(p);
dessinerPoint(bitmap, pos, Brushes.Turquoise);
if (robot.Trace != null)
{
dessinerTrack(bitmap, robot.Trace, _Follower.TrackMaker.CreerAstarQuadriallage());
PolyligneDessin p2 = new PolyligneDessin(Color.Red);
for (int i = 0; i < robot.Trace.Positions.Count; i++)
{
p.addPoint(new PointDessin(robot.Trace.Positions[i].X, robot.Trace.Positions[i].Y));
}
ListePoly.Add(p2);
}
}
imageDebug.Image = bitmap;
if (DrawPolylineEvent != null)
{
DrawPolylineEventArgs args = new DrawPolylineEventArgs(ListePoly);
DrawPolylineEvent(this, args);
}
}
}
private void tickIA() // Fonction principale
{
if (_bInfosPosition && _bInfosCubes && _bInfosZone && _bInfosZoneTravail) // On a recus tout le necessaire depuis l'image
{
// foreach (ArduinoBotIA Robot in _ListArduino)
for (int indexRobot = 0; indexRobot < _ListArduino.Count; indexRobot++)
{
ArduinoBotIA Robot = _ListArduino[indexRobot];
ArduinoBotComm RobotComm = _ArduinoManager.getArduinoBotById(Robot.ID);
if (RobotComm != null && RobotComm.Connected) // Le robot est déja connecté
{
// Faire des verification et envoi des messages
if (Robot.PositionValide) // On a bien une position valide
{
if (Robot.LastAction == ActionRobot.ROBOT_AUTONOME)
{
continue; // Le robot est en mode autonome (on devrais verifier si il ne séloigne pas de l'objectif )
}
if (Robot.Trace != null) // On a un tracé attribué au robot
{
if (!Robot.Saisie && !_TrackMaker.Cubes.Exists(Objectif.ById(Robot.Cube.id))) // Le cube n'existe plus ( téléportation), et on ne l'as pas saisis
{
// Reinit pour recalcul de trajectoire
Robot.SetZoneDepose(null);
Robot.SetObjectif(null);
Robot.SetTrace(null);
Robot.Saisie = false;
}
if (checkProximiteObjectif(Robot)) // Proximité de l'objectif ?
{
Logger.GlobalLogger.debug("Proximité detectée ! ");
if (Robot.Saisie) // On a un cube ? si oui on le dépose
{
MessageProtocol mess = MessageBuilder.createOpenClawMessage();
_AutomateComm.PushSendMessageToArduino(mess, RobotComm);
Robot.LastAction = ActionRobot.ROBOT_PINCE;
Robot.SetZoneDepose(null);
Robot.SetObjectif(null);
Robot.SetTrace(null);
Robot.Saisie = false;
}
else
{
// Passer en Autonome
MessageProtocol mess = MessageBuilder.createModeAutoMessage();
_AutomateComm.PushSendMessageToArduino(mess, RobotComm);
Robot.LastAction = ActionRobot.ROBOT_AUTONOME;
}
Robot.LastActionTime = DateTime.Now;
}
else
{
Logger.GlobalLogger.debug("Proximité non detectée ! ");
PositionElement NearestPositionTroncon;
if (checkSuiteItineraire(Robot.ID, out NearestPositionTroncon)) // On est proche du point de passage ?
{
Logger.GlobalLogger.debug("Point Suivant ");
removeBeforePoint(Robot.Trace, NearestPositionTroncon); // On supprime les anciens points
}
if (!checkProximiteTrace(Robot.ID)) // On est proche du tracé ?
{
// Calcul de la différence d'orientation
if (Math.Abs(diffOrientation(Robot, Robot.Trace)) > _differenceMaxOrientation) // Différence suppérieur de 15 degreé entre le robot et l'angle de la droite
{
Logger.GlobalLogger.debug("Orientation différente ");
if (Robot.LastAction != ActionRobot.ROBOT_TOURNER || (DateTime.Now - Robot.LastActionTime) > TimeSpan.FromSeconds(5)) // On etait pas en train de tourner ou ça fait plus de 5 secondes
{
// Faire trouner le robot
double angle = diffOrientation(Robot, Robot.Trace);
if (angle < 0) // Si inférieur a 0° alors tourner a gauche
{
MessageProtocol mess = MessageBuilder.createTurnMessage(true, (byte)Math.Abs(angle));
_AutomateComm.PushSendMessageToArduino(mess, RobotComm);
}
else // sinon touner a droite
{
MessageProtocol mess = MessageBuilder.createTurnMessage(false, (byte)Math.Abs(angle));
_AutomateComm.PushSendMessageToArduino(mess, RobotComm);
}
Logger.GlobalLogger.info("Changement d'angle : " + angle);
Robot.LastAction = ActionRobot.ROBOT_TOURNER;
Robot.LastActionTime = DateTime.Now;
}
// Tourner pour se placer dans le bon sens
}
else
{
// Si oui on continue a se deplacer
foreach (ArduinoBotIA RobotProche in _ListArduino) // tester la presence d'autre robot a proximité
{
if (RobotProche.ID == Robot.ID) // Soi meme
{
continue; // suivant
}
if (UtilsMath.DistanceEuclidienne(Robot.Position, RobotProche.Position) < _seuilProximiteRobot) // On est trop proche d'un autre robot
{
if (RobotProche.LastAction != ActionRobot.ROBOT_ARRET) // L'autre robot n'est pas en arret
{
// nous arreter
MessageProtocol mess = MessageBuilder.createMoveMessage(true, (byte)0, (byte)0); // STOP
_AutomateComm.PushSendMessageToArduino(mess, RobotComm);
Robot.LastAction = ActionRobot.ROBOT_ARRET;
Robot.LastActionTime = DateTime.Now;
break; // sortie de boucle
}
}
}
if (Robot.LastAction != ActionRobot.ROBOT_DEPLACER || (DateTime.Now - Robot.LastActionTime) > TimeSpan.FromSeconds(5)) // On etait pas en train de se deplacer ou ça fait plus de 10 secondes
{
double distance = UtilsMath.DistanceEuclidienne(Robot.Position, Robot.Trace.Positions[1]);
//Logger.GlobalLogger.debug("Distance : " + (byte)(distance / _ConversionUnit), 5);
MessageProtocol mess = MessageBuilder.createMoveMessage(true, (byte)100, (byte)(distance / _ConversionUnit)); // Avancer a 50% de vitesse
_AutomateComm.PushSendMessageToArduino(mess, RobotComm);
Robot.LastAction = ActionRobot.ROBOT_DEPLACER;
Robot.LastActionTime = DateTime.Now;
}
else
{
// ne pas renvoyer d'ordre, le robot est en train de se déplacer
}
}
}
else
{
// Si non on recalcule
_TrackMaker.CalculerObjectif(Robot);
}
}
}
else
{
// Calcul d'un itineraire
_TrackMaker.CalculerObjectif(Robot);
}
}
else
{// Pas de position valide
// Envoyer un stop pour ne pas bouger
MessageProtocol mess = MessageBuilder.createMoveMessage(true, 0x00, 0x00);
_AutomateComm.PushSendMessageToArduino(mess, RobotComm);
Robot.LastAction = ActionRobot.ROBOT_ARRET;
Robot.LastActionTime = DateTime.Now;
}
}
}
}
}
