public override void Stop(StopMode mode)
{
Historique.AjouterAction(new ActionStop(this, mode));
_lockMove.WaitOne();
if (mode == StopMode.Smooth)
{
Position nouvelleDestination = new Position(Position.Angle, new RealPoint(_currentPosition.Coordinates.X, _currentPosition.Coordinates.Y));
if (IsInLineMove)
{
if (_sensMove == SensAR.Avant)
{
nouvelleDestination.Move(GetCurrentLineBreakDistance());
}
else
{
nouvelleDestination.Move(-GetCurrentLineBreakDistance());
}
}
_destination = nouvelleDestination;
}
else if (mode == StopMode.Abrupt)
{
_currentSpeed = 0;
_destination = Position;
}
_lockMove.Release();
}
public override void MoveForward(int distance, bool wait = true)
{
base.MoveForward(distance, wait);
IsInLineMove = true;
if (distance > 0)
{
if (!_inRecalibration)
{
Historique.AjouterAction(new ActionAvance(this, distance));
}
_sensMove = SensAR.Avant;
}
else
{
if (!_inRecalibration)
{
Historique.AjouterAction(new ActionRecule(this, -distance));
}
_sensMove = SensAR.Arriere;
}
_destination = new Position(Position.Angle, new RealPoint(Position.Coordinates.X + distance * Position.Angle.Cos, Position.Coordinates.Y + distance * Position.Angle.Sin));
// TODO2018 attente avec un sémaphore ?
if (wait)
{
while ((Position.Coordinates.X != _destination.Coordinates.X ||
Position.Coordinates.Y != _destination.Coordinates.Y) && !Execution.Shutdown)
{
Thread.Sleep(10);
}
}
}
public override void MoveBackward(int distance, bool wait = true)
{
base.MoveBackward(distance, wait);
if (wait)
{
_lockFrame[UdpFrameFunction.FinDeplacement] = new Semaphore(0, int.MaxValue);
}
IsInLineMove = true;
Frame frame;
if (distance < 0)
{
frame = UdpFrameFactory.Deplacer(SensAR.Avant, -distance, this);
}
else
{
frame = UdpFrameFactory.Deplacer(SensAR.Arriere, distance, this);
}
_asserConnection.SendMessage(frame);
Historique.AjouterAction(new ActionRecule(this, distance));
if (wait)
{
//if (!SemaphoresTrame[FrameFunction.FinDeplacement].WaitOne((int)SpeedConfig.LineDuration(distance).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 void SpeedConfig_ParamChange(bool lineAccelChange, bool lineDecelChange, bool lineSpeedChange, bool pivotAccelChange, bool pivotDecelChange, bool pivotSpeedChange)
{
if (lineSpeedChange)
{
Frame frame = UdpFrameFactory.VitesseLigne(SpeedConfig.LineSpeed, this);
_asserConnection.SendMessage(frame);
Historique.AjouterAction(new ActionVitesseLigne(this, SpeedConfig.LineSpeed));
}
if (lineAccelChange || lineDecelChange)
{
Frame frame = UdpFrameFactory.AccelLigne(SpeedConfig.LineAcceleration, SpeedConfig.LineDeceleration, this);
_asserConnection.SendMessage(frame);
Historique.AjouterAction(new ActionAccelerationLigne(this, SpeedConfig.LineAcceleration, SpeedConfig.LineDeceleration));
}
if (pivotSpeedChange)
{
Frame frame = UdpFrameFactory.VitessePivot(SpeedConfig.PivotSpeed, this);
_asserConnection.SendMessage(frame);
Historique.AjouterAction(new ActionVitessePivot(this, SpeedConfig.PivotSpeed));
}
if (pivotAccelChange || pivotDecelChange)
{
Frame frame = UdpFrameFactory.AccelPivot(SpeedConfig.PivotAcceleration, this);
_asserConnection.SendMessage(frame);
Historique.AjouterAction(new ActionAccelerationPivot(this, SpeedConfig.PivotAcceleration, SpeedConfig.PivotDeceleration));
}
}
public virtual void SetMotorSpeed(MotorID moteur, SensGD sens, int vitesse)
{
if (MotorState.ContainsKey(moteur))
{
MotorState[moteur] = vitesse == 0 ? false : true;
}
Historique.AjouterAction(new ActionMoteur(this, vitesse, moteur));
}
public override void SetActuatorOnOffValue(ActuatorOnOffID actuator, bool on)
{
ActuatorOnOffState[actuator] = on;
Frame frame = UdpFrameFactory.ActionneurOnOff(_boardActuatorOnOff[actuator], actuator, on);
Connections.UDPBoardConnection[_boardActuatorOnOff[actuator]].SendMessage(frame);
Historique.AjouterAction(new ActionOnOff(this, actuator, on));
}
public override void Init()
{
Historique = new Historique(IDRobot);
PositionsHistorical = new List <Position>();
Position = new Position(GoBot.Recalibration.StartPosition);
PositionTarget = null;
_linkAsserv.StartInfiniteLoop(new TimeSpan(0, 0, 0, 0, _highResolutionAsservissement ? 1 : 16));
_linkLogPositions.StartInfiniteLoop(new TimeSpan(0, 0, 0, 0, 100));
}
public override void Stop(StopMode mode = StopMode.Smooth)
{
AsserEnable = (mode != StopMode.Freely);
IsInLineMove = false;
Frame frame = UdpFrameFactory.Stop(mode, this);
_asserConnection.SendMessage(frame);
Historique.AjouterAction(new ActionStop(this, mode));
}
public bool RunTrajectory(Trajectory traj)
{
TrajectoryRunning = traj;
TimeSpan estimatedDuration = traj.GetDuration(this);
Stopwatch sw = Stopwatch.StartNew();
TrajectoryCutOff = false;
TrajectoryFailed = false;
foreach (IAction action in traj.ConvertToActions(this))
{
if (!Execution.Shutdown)
{
action.Executer();
if (TrajectoryCutOff || TrajectoryFailed)
{
break;
}
if (action is ActionAvance || action is ActionRecule)
{
Historique.Log("Noeud atteint " + TrajectoryRunning.Points[0].X.ToString("0") + ":" + TrajectoryRunning.Points[0].Y.ToString("0"), TypeLog.PathFinding);
TrajectoryRunning.RemovePoint(0);
}
}
}
if (!Execution.Shutdown)
{
TrajectoryRunning = null;
if (!TrajectoryCutOff && !TrajectoryFailed)
{
Historique.Log("Trajectoire parcourue en " + (sw.ElapsedMilliseconds / 1000.0).ToString("0.0") + "s (durée théorique : " + (estimatedDuration.TotalSeconds).ToString("0.0") + "s)", TypeLog.PathFinding);
return(true);
}
if (TrajectoryFailed)
{
Historique.Log("Echec du parcours de la trajectoire (dérapage, blocage...)", TypeLog.PathFinding);
return(false);
}
}
return(false);
}
public bool GoToPosition(Position dest)
{
Historique.Log("Lancement pathfinding pour aller en " + dest.ToString(), TypeLog.PathFinding);
Trajectory traj = PathFinder.ChercheTrajectoire(Graph, GameBoard.ObstaclesAll, GameBoard.ObstaclesOpponents, Position, dest, Radius, Robots.MainRobot.Width / 2);
if (traj == null)
{
return(false);
}
RunTrajectory(traj);
return(!TrajectoryCutOff && !TrajectoryFailed);
}
public override void Recalibration(SensAR sens, bool wait = true, bool sendOffset = false)
{
_inRecalibration = true;
Historique.AjouterAction(new ActionRecallage(this, sens));
if (wait)
{
RecalProcedure(sens);
}
else
{
ThreadManager.CreateThread(link => RecalProcedure(sens)).StartThread();
}
base.Recalibration(sens, wait, sendOffset);
}
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);
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 Init()
{
Historique = new Historique(IDRobot);
Connections.ConnectionCan.FrameReceived += ReceptionCanMessage;
_asserConnection.FrameReceived += ReceptionUdpMessage;
if (this == Robots.MainRobot)
{
Connections.ConnectionIO.FrameReceived += ReceptionUdpMessage;
}
//Connections.ConnectionsCan[CanBoard.CanAlim].FrameReceived += ReceptionCanMessage;
if (this == Robots.MainRobot)
{
if (GameBoard.MyColor == GameBoard.ColorLeftBlue)
{
Position = new Position(0, new RealPoint(240, 1000));
}
else
{
Position = new Position(180, new RealPoint(3000 - 240, 1000));
}
}
else
{
if (GameBoard.MyColor == GameBoard.ColorLeftBlue)
{
Position = new Position(0, new RealPoint(480, 1000));
}
else
{
Position = new Position(180, new RealPoint(3000 - 480, 1000));
}
}
PositionTarget = null; //TODO2018 Init commun à la simu
PositionsHistorical = new List <Position>();
_asserConnection.SendMessage(UdpFrameFactory.DemandePositionContinue(50, this));
}
public override void Recalibration(SensAR sens, bool wait = true, bool sendOffset = false)
{
if (wait)
{
_lockFrame[UdpFrameFunction.FinDeplacement] = new Semaphore(0, int.MaxValue);
}
Frame frame = UdpFrameFactory.Recallage(sens, this);
_asserConnection.SendMessage(frame);
Historique.AjouterAction(new ActionRecallage(this, sens));
if (wait)
{
_lockFrame[UdpFrameFunction.FinDeplacement].WaitOne();
}
base.Recalibration(sens, wait, sendOffset);
}
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();
}
}
public virtual void SetMotorAcceleration(MotorID moteur, int acceleration)
{
Historique.AjouterAction(new ActionMoteur(this, acceleration, moteur));
}
public bool OpponentsTrajectoryCollision(IEnumerable <IShape> opponents)
{
bool ok = true;
if (TrajectoryCutOff)
{
ok = false;
}
if (ok)
{
try
{
// Teste si le chemin en cours de parcours est toujours franchissable
if (TrajectoryRunning != null && TrajectoryRunning.Lines.Count > 0)
{
List <Segment> segmentsTrajectoire = new List <Segment>();
// Calcule le segment entre nous et notre destination (permet de ne pas considérer un obstacle sur un tronçon déjà franchi)
Segment toNextPoint = new Segment(Position.Coordinates, new RealPoint(TrajectoryRunning.Lines[0].EndPoint));
segmentsTrajectoire.Add(toNextPoint);
for (int iSegment = 1; iSegment < TrajectoryRunning.Lines.Count; iSegment++)
{
segmentsTrajectoire.Add(TrajectoryRunning.Lines[iSegment]);
}
foreach (IShape opponent in opponents)
{
foreach (Segment segment in segmentsTrajectoire)
{
// Marge de 30mm pour être plus permissif sur le passage et ne pas s'arreter dès que l'adversaire approche
if (!IsFarEnough(toNextPoint, opponent, -30))
{
// Demande de génération d'une nouvelle trajectoire
Historique.Log("Trajectoire coupée, annulation", TypeLog.PathFinding);
TrajectoryCutOff = true;
TrajectoryRunning = null;
if (IsInLineMove)
{
Stop();
}
ok = false;
break;
}
}
if (!ok)
{
break;
}
}
}
}
catch (Exception)
{
ok = false;
}
}
return(ok);
}
public override void Init()
{
Couleur = Color.Purple;
Historique = new Historique(IDRobot);
DateRefreshPos = DateTime.Now;
//Enchainement = new Enchainements.HomologationEnchainement();
Connexion.NouvelleTrameRecue += new ConnexionUDP.ReceptionDelegate(ReceptionMessage);
if (this == Robots.GrosRobot)
Connexions.ConnexionIO.NouvelleTrameRecue += new ConnexionUDP.ReceptionDelegate(ReceptionMessage);
if (this == Robots.GrosRobot)
{
if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet)
Position = new Calculs.Position(new Angle(0, AnglyeType.Degre), new PointReel(240, 1000));
else
Position = new Calculs.Position(new Angle(180, AnglyeType.Degre), new PointReel(3000 - 240, 1000));
}
else
{
if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet)
Position = new Calculs.Position(new Angle(0, AnglyeType.Degre), new PointReel(480, 1000));
else
Position = new Calculs.Position(new Angle(180, AnglyeType.Degre), new PointReel(3000 - 480, 1000));
}
PositionCible = null;
HistoriqueCoordonnees = new List<Position>();
Connexion.SendMessage(TrameFactory.DemandePositionContinue(100, this));
}
public override void Init()
{
Historique = new Historique(IDRobot);
HistoriqueCoordonnees = new List<Position>();
if (this == Robots.GrosRobot)
{
if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet)
Position = new Calculs.Position(new Angle(0, AnglyeType.Degre), new PointReel(240, 1000));
else
Position = new Calculs.Position(new Angle(180, AnglyeType.Degre), new PointReel(3000 - 240, 1000));
}
else
{
if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet)
Position = new Calculs.Position(new Angle(0, AnglyeType.Degre), new PointReel(480, 1000));
else
Position = new Calculs.Position(new Angle(180, AnglyeType.Degre), new PointReel(3000 - 480, 1000));
}
PositionCible = null;
}
public override void SetActuatorOnOffValue(ActuatorOnOffID actuator, bool on)
{
// TODO
Historique.AjouterAction(new ActionOnOff(this, actuator, on));
}
