public override List <int>[] DiagnosticLine(int distance, SensAR sens)
{
_lastPidTest = new List <int> [2];
_lastPidTest[0] = new List <int>();
_lastPidTest[1] = new List <int>();
_lockFrame[UdpFrameFunction.FinDeplacement] = new Semaphore(0, int.MaxValue);
Frame frame = UdpFrameFactory.Deplacer(sens, distance, this);
_asserConnection.SendMessage(frame);
frame = UdpFrameFactory.DemandePositionsCodeurs(this);
while (!_lockFrame[UdpFrameFunction.FinDeplacement].WaitOne(0))
{
_asserConnection.SendMessage(frame);
Thread.Sleep(30);
}
List <int>[] output = new List <int> [2];
output[0] = new List <int>(_lastPidTest[0]);
output[1] = new List <int>(_lastPidTest[1]);
return(output);
}
private void RecalProcedure(SensAR sens)
{
int realAccel = SpeedConfig.LineAcceleration;
int realDecel = SpeedConfig.LineAcceleration;
SpeedConfig.LineAcceleration = 50000;
SpeedConfig.LineDeceleration = 50000;
IShape contact = GetBounds();
double lenght = sens == SensAR.Arriere ? LenghtBack : LenghtFront;
while (Position.Coordinates.X - lenght > 0 &&
Position.Coordinates.X + lenght < GameBoard.Width &&
Position.Coordinates.Y - lenght > 0 &&
Position.Coordinates.Y + lenght < GameBoard.Height)// &&
//!GameBoard.ObstaclesAll.ToList().Exists(o => o.Cross(contact))) // TODO ça marche pas on dirait le test de recallage sur les obstacles
{
if (sens == SensAR.Arriere)
{
MoveBackward(1);
}
else
{
MoveForward(1);
}
contact = GetBounds();
}
SpeedConfig.LineAcceleration = realAccel;
SpeedConfig.LineDeceleration = realDecel;
_inRecalibration = false;
}
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 ActionVirage(Robot r, int dist, AngleDelta a, SensAR ar, SensGD gd)
{
robot = r;
distance = dist;
angle = a;
sensAR = ar;
sensGD = gd;
}
public ActionVirage(Robot r, int dist, int a, SensAR ar, SensGD gd)
{
robot = r;
distance = dist;
angle = a;
sensAR = ar;
sensGD = gd;
}
public override void PolarTrajectory(SensAR sens, List <RealPoint> points, bool wait = true)
{
_polarTrajectory = points;
_currentPolarPoint = 0;
while (wait && _currentPolarPoint != -1)
{
Thread.Sleep(10);
}
}
static public Frame Recallage(SensAR sens, Robot robot)
{
byte[] tab = new byte[3];
tab[0] = (byte)robot.AsservBoard;
tab[1] = (byte)UdpFrameFunction.Recallage;
tab[2] = (byte)sens;
Frame retour = new Frame(tab);
return(retour);
}
static public Frame EnvoiConsigneBrute(int consigne, SensAR sens, Robot robot)
{
byte[] tab = new byte[5];
tab[0] = (byte)robot.AsservBoard;
tab[1] = (byte)UdpFrameFunction.AsserEnvoiConsigneBrutePosition;
tab[2] = (byte)sens;
tab[3] = (byte)ByteDivide(consigne, true);
tab[4] = (byte)ByteDivide(consigne, false);
Frame retour = new Frame(tab);
return(retour);
}
static public Frame Deplacer(SensAR sens, int distance, Robot robot)
{
byte[] tab = new byte[5];
tab[0] = (byte)robot.AsservBoard;
tab[1] = (byte)UdpFrameFunction.Deplace;
tab[2] = (byte)sens;
tab[3] = ByteDivide(distance, true);
tab[4] = ByteDivide(distance, false);
Frame retour = new Frame(tab);
return(retour);
}
public override List <int>[] DiagnosticPID(int steps, SensAR sens, int pointsCount)
{
List <int>[] output = new List <int> [2];
output[0] = new List <int>();
output[1] = new List <int>();
for (double i = 0; i < pointsCount; i++)
{
output[0].Add((int)(Math.Sin((i + DateTime.Now.Millisecond) / 100.0 * Math.PI) * steps * 10000000 / (i * i * i)));
output[1].Add((int)(Math.Sin((i + DateTime.Now.Millisecond) / 100.0 * Math.PI) * steps * 10000000 / (i * i * i) + 10));
}
return(output);
}
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);
}
public override void PolarTrajectory(SensAR sens, List <RealPoint> points, bool wait = true)
{
if (wait)
{
_lockFrame[UdpFrameFunction.FinDeplacement] = new Semaphore(0, int.MaxValue);
}
Frame frame = UdpFrameFactory.TrajectoirePolaire(sens, points, this);
_asserConnection.SendMessage(frame);
if (wait)
{
_lockFrame[UdpFrameFunction.FinDeplacement].WaitOne();
}
}
public RobotSimu(IDRobot idRobot) : base(idRobot, "Robot Simu")
{
_highResolutionAsservissement = true;
_rand = new Random();
_linkAsserv = ThreadManager.CreateThread(link => Asservissement());
_linkLogPositions = ThreadManager.CreateThread(link => LogPosition());
_lockMove = new Semaphore(1, 1);
_sensMove = SensAR.Avant;
_inRecalibration = false;
_asserChrono = null;
_currentPolarPoint = -1;
BatterieVoltage = 25;
}
public virtual void Recalibration(SensAR sens, bool waitEnd = true, bool sendOffset = false)
{
int angle = 0;
double toleranceXY = 30;
double toleranceTheta = 3;
double dist = (sens == SensAR.Avant ? Robots.MainRobot.LenghtFront : Robots.MainRobot.LenghtBack);
if (waitEnd && sendOffset)
{
if (Position.Angle.IsOnArc(0 - toleranceTheta, 0 + toleranceTheta))
{
angle = 0;
}
else if (Position.Angle.IsOnArc(90 - toleranceTheta, 90 + toleranceTheta))
{
angle = 90;
}
else if (Position.Angle.IsOnArc(180 - toleranceTheta, 180 + toleranceTheta))
{
angle = 180;
}
else if (Position.Angle.IsOnArc(270 - toleranceTheta, 270 + toleranceTheta))
{
angle = 270;
}
if (Position.Coordinates.X < dist + toleranceXY && Position.Coordinates.X > dist - toleranceTheta)
{
SetAsservOffset(new Position(angle, new RealPoint(dist, Position.Coordinates.Y)));
}
else if (Position.Coordinates.X > 3000 - dist)
{
SetAsservOffset(new Position(angle, new RealPoint(3000 - dist, Position.Coordinates.Y)));
}
else if (Position.Coordinates.Y < dist)
{
SetAsservOffset(new Position(angle, new RealPoint(Position.Coordinates.X, dist)));
}
else if (Position.Coordinates.Y > 2000 - dist)
{
SetAsservOffset(new Position(angle, new RealPoint(Position.Coordinates.X, 2000 - dist)));
}
}
}
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 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);
}
static public Frame TrajectoirePolaire(SensAR sensAr, List <RealPoint> points, Robot robot)
{
byte[] tab = new byte[5 + points.Count * 2 * 2];
tab[0] = (byte)robot.AsservBoard;
tab[1] = (byte)UdpFrameFunction.TrajectoirePolaire;
tab[2] = (byte)sensAr;
tab[3] = (byte)ByteDivide(points.Count, true);
tab[4] = (byte)ByteDivide(points.Count, false);
for (int i = 0; i < points.Count; i++)
{
tab[5 + i * 4 + 0] = ByteDivide((int)(points[i].X * 10), true);
tab[5 + i * 4 + 1] = ByteDivide((int)(points[i].X * 10), false);
tab[5 + i * 4 + 2] = ByteDivide((int)(points[i].Y * 10), true);
tab[5 + i * 4 + 3] = ByteDivide((int)(points[i].Y * 10), false);
}
Frame retour = new Frame(tab);
return(retour);
}
public override List <int>[] DiagnosticLine(int distance, SensAR sens)
{
List <int>[] output = new List <int> [2];
output[0] = new List <int>();
output[1] = new List <int>();
RealPoint startPos = new RealPoint(Position.Coordinates);
MoveForward(distance, false);
while ((Position.Coordinates.X != _destination.Coordinates.X ||
Position.Coordinates.Y != _destination.Coordinates.Y) && !Execution.Shutdown)
{
double dist = startPos.Distance(Position.Coordinates);
output[0].Add((int)(dist * 1000));
output[1].Add((int)(dist * 1000));
Thread.Sleep(1);
}
return(output);
}
public override List <int>[] DiagnosticPID(int steps, SensAR sens, int pointsCount)
{
_lastPidTest = new List <int> [2];
_lastPidTest[0] = new List <int>();
_lastPidTest[1] = new List <int>();
Frame frame = UdpFrameFactory.EnvoiConsigneBrute(steps, sens, this);
_asserConnection.SendMessage(frame);
frame = UdpFrameFactory.DemandePositionsCodeurs(this);
while (_lastPidTest[0].Count < pointsCount)
{
_asserConnection.SendMessage(frame);
Thread.Sleep(30);
}
List <int>[] output = new List <int> [2];
output[0] = _lastPidTest[0].GetRange(0, pointsCount);
output[1] = _lastPidTest[1].GetRange(0, pointsCount);
return(output);
}
protected void DoTurn(SensAR sensAr, SensGD sensGd)
{
int distance;
int angle;
bool ok = true;
if (!Int32.TryParse(txtDistance.Text, out distance) || distance == 0)
{
txtDistance.ErrorMode = true;
ok = false;
}
if (!Int32.TryParse(txtAngle.Text, out angle) || angle == 0)
{
txtAngle.ErrorMode = true;
ok = false;
}
if (ok)
{
Robot.Turn(sensAr, sensGd, distance, angle, false);
}
}
public abstract List <int>[] DiagnosticLine(int distance, SensAR sens);
public abstract List <int>[] DiagnosticPID(int steps, SensAR sens, int valuesCount);
protected void Virage_Click(SensAR sensAr, SensGD sensGd)
{
int distance = 0;
int angle = 0;
bool ok = true;
if (!Int32.TryParse(txtDistance.Text, out distance) || distance == 0)
{
txtDistance.ErrorMode = true;
ok = false;
}
if (!Int32.TryParse(txtAngle.Text, out angle) || angle == 0)
{
txtAngle.ErrorMode = true;
ok = false;
}
if (ok)
Robot.Virage(sensAr, sensGd, distance, angle, false);
}
public abstract List<int>[] MesureTestPid(int consigne, SensAR sens, int nbValeurs);
public ActionRecallage(Robot r, SensAR s)
{
robot = r;
sens = s;
}
public static int Factor(this SensAR sens)
{
return(sens == SensAR.Avant ? 1 : -1);
}
public override void Virage(SensAR sensAr, SensGD sensGd, int rayon, int angle, bool attendre = true)
{
if (attendre)
SemaphoresMove[FonctionMove.FinDeplacement] = new Semaphore(0, int.MaxValue);
Historique.AjouterAction(new ActionVirage(this, rayon, angle, sensAr, sensGd));
Trame trame = TrameFactory.Virage(sensAr, sensGd, rayon, angle, this);
Connexion.SendMessage(trame);
if (attendre)
SemaphoresMove[FonctionMove.FinDeplacement].WaitOne();
}
public override void TrajectoirePolaire(SensAR sens, List<PointReel> points, bool attendre = true)
{
if (attendre)
SemaphoresMove[FonctionMove.FinDeplacement] = new Semaphore(0, int.MaxValue);
//Historique.AjouterAction(new ActionVirage(this, rayon, angle, sensAr, sensGd)); TODO
Trame trame = TrameFactory.TrajectoirePolaire(sens, points, this);
Connexion.SendMessage(trame);
if (attendre)
SemaphoresMove[FonctionMove.FinDeplacement].WaitOne();
}
public static Trame TrajectoirePolaire(SensAR sensAr, List<PointReel> points, Robot robot)
{
byte[] tab = new byte[5 + points.Count * 2 * 2];
tab[0] = (byte)robot.Carte;
tab[1] = (byte)FonctionMove.TrajectoirePolaire;
tab[2] = (byte)sensAr;
tab[3] = (byte)ByteDivide(points.Count, true);
tab[4] = (byte)ByteDivide(points.Count, false);
for (int i = 0; i < points.Count; i++)
{
tab[5 + i * 4 + 0] = ByteDivide((int)(points[i].X * 10), true);
tab[5 + i * 4 + 1] = ByteDivide((int)(points[i].X * 10), false);
tab[5 + i * 4 + 2] = ByteDivide((int)(points[i].Y * 10), true);
tab[5 + i * 4 + 3] = ByteDivide((int)(points[i].Y * 10), false);
}
Trame retour = new Trame(tab);
return retour;
}
public override List<int>[] MesureTestPid(int consigne, SensAR sens, int nbValeurs)
{
retourTestPid = new List<int>[2];
retourTestPid[0] = new List<int>();
retourTestPid[1] = new List<int>();
Trame trame = TrameFactory.EnvoiConsigneBrute(consigne, sens, this);
Connexion.SendMessage(trame);
trame = TrameFactory.DemandePositionsCodeurs(this);
while (retourTestPid[0].Count < nbValeurs)
{
Connexion.SendMessage(trame);
Thread.Sleep(30);
}
while (retourTestPid[0].Count > nbValeurs)
retourTestPid[0].RemoveAt(retourTestPid[0].Count - 1);
while (retourTestPid[1].Count > nbValeurs)
retourTestPid[1].RemoveAt(retourTestPid[1].Count - 1);
return retourTestPid;
}
public override void Turn(SensAR sensAr, SensGD sensGd, int rayon, AngleDelta angle, bool wait = true)
{
// TODO2018
}
public abstract void TrajectoirePolaire(SensAR sens, List<PointReel> points, bool attendre = true);
public abstract void PolarTrajectory(SensAR sens, List <RealPoint> points, bool waitEnd = true);
public ActionRecallage(Robot r, SensAR s)
{
robot = r;
sens = s;
}
public abstract void Turn(SensAR sensAr, SensGD sensGd, int radius, AngleDelta angle, bool waitEnd = true);
public override List<int>[] MesureTestPid(int consigne, SensAR sens, int nbValeurs)
{
List<int>[] retour = new List<int>[2];
retour[0] = new List<int>();
retour[1] = new List<int>();
for (double i = 0; i < nbValeurs; i++)
{
retour[0].Add((int)(Math.Sin((i + DateTime.Now.Millisecond) / 100.0 * Math.PI) * consigne * 10000000 / (i * i * i)));
retour[1].Add((int)(Math.Sin((i + DateTime.Now.Millisecond) / 100.0 * Math.PI) * consigne * 10000000 / (i * i * i) + 10));
}
return retour;
}
public static Trame Virage(SensAR sensAr, SensGD sensGd, int rayon, double angle, Robot robot)
{
byte[] tab = new byte[8];
tab[0] = (byte)robot.Carte;
tab[1] = (byte)FonctionMove.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);
Trame retour = new Trame(tab);
return retour;
}
public override void Recallage(SensAR sens, bool attendre = true)
{
RecallageEnCours = true;
Historique.AjouterAction(new ActionRecallage(this, sens));
int accelTmp = AccelerationDebutDeplacement;
AccelerationDebutDeplacement = 4000;
while (Position.Coordonnees.X - Longueur / 2 > 0 &&
Position.Coordonnees.X + Longueur / 2 < Plateau.LongueurPlateau &&
Position.Coordonnees.Y - Longueur / 2 > 0 &&
Position.Coordonnees.Y + Longueur / 2 < Plateau.LargeurPlateau)
{
if (sens == SensAR.Arriere)
Reculer(5);
else
Avancer(5);
}
if (Position.Coordonnees.X < 0)
Position.Coordonnees.X = Longueur / 2;
if (Position.Coordonnees.X > Plateau.LongueurPlateau)
Position.Coordonnees.X = Plateau.LongueurPlateau - Longueur / 2;
if (Position.Coordonnees.Y < 0)
Position.Coordonnees.Y = Longueur / 2;
if (Position.Coordonnees.Y > Plateau.LargeurPlateau)
Position.Coordonnees.Y = Plateau.LargeurPlateau - Longueur / 2;
AccelerationDebutDeplacement = accelTmp;
RecallageEnCours = false;
}
public static Trame EnvoiConsigneBrute(int consigne, SensAR sens, Robot robot)
{
byte[] tab = new byte[5];
tab[0] = (byte)robot.Carte;
tab[1] = (byte)FonctionMove.EnvoiConsigneBrute;
tab[2] = (byte)sens;
tab[3] = (byte)ByteDivide(consigne, true);
tab[4] = (byte)ByteDivide(consigne, false);
Trame retour = new Trame(tab);
return retour;
}
public static Trame Recallage(SensAR sens, Robot robot)
{
byte[] tab = new byte[3];
tab[0] = (byte)robot.Carte;
tab[1] = (byte)FonctionMove.Recallage;
tab[2] = (byte)sens;
Trame retour = new Trame(tab);
return retour;
}
public override void TrajectoirePolaire(SensAR sens, List<PointReel> points, bool attendre = true)
{
trajectoirePolaire = points;
pointCourantTrajPolaire = 0;
while (pointCourantTrajPolaire != -1)
Thread.Sleep(10);
}
public override void Recallage(SensAR sens, bool attendre = true)
{
if (attendre)
SemaphoresMove[FonctionMove.FinDeplacement] = new Semaphore(0, int.MaxValue);
Historique.AjouterAction(new ActionRecallage(this, sens));
Trame trame = TrameFactory.Recallage(sens, this);
Connexion.SendMessage(trame);
if (attendre)
SemaphoresMove[FonctionMove.FinDeplacement].WaitOne();
}
public abstract void Recallage(SensAR sens, bool attendre = true);
public override void Virage(SensAR sensAr, SensGD sensGd, int rayon, int angle, bool attendre = true)
{
// TODO
}
public abstract void Virage(SensAR sensAr, SensGD sensGd, int rayon, int angle, bool attendre = true);
public static Trame Deplacer(SensAR sens, int distance, Robot robot)
{
byte[] tab = new byte[5];
tab[0] = (byte)robot.Carte;
tab[1] = (byte)FonctionMove.Deplace;
tab[2] = (byte)sens;
tab[3] = ByteDivide(distance, true);
tab[4] = ByteDivide(distance, false);
Trame retour = new Trame(tab);
return retour;
}