/* Bouton Mouvement UP / DOWN */
private void btn_up_Click(object sender, EventArgs e)
{
_AutomateComm.PushSendMessageToArduino(
MessageBuilder.createMoveMessage(true, 100, 0xf0),
_ArduinoManager.getArduinoBotById(_CurrentArduinoId)
);
ArduinoBotComm test = _ArduinoManager.getArduinoBotById(_CurrentArduinoId);
//g_Serial.addMessageToSend(g_MessageBuilder.createMoveMessage(true,0x50,0x50));
}
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);
}
}
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;
}
}
}
}
}