private void GetServoInfos()
{
_linkPolling.RegisterName(" : " + _servo.ID.ToString());
gphMonitoringTorque.AddPoint("Couple", _servo.ReadTorqueCurrent(), Color.Firebrick, true);
gphMonitoringTorque.AddPoint("Alerte", _servo.LastTorqueMax, Color.Red, false);
gphMonitoringPos.AddPoint("Position", _servo.ReadPosition(), Color.RoyalBlue);
}
private void TestAsser(ThreadLink link)
{
link.RegisterName();
Robots.MainRobot.MoveForward(2000);
Robots.MainRobot.PivotRight(270);
Robots.MainRobot.MoveForward(200);
Robots.MainRobot.PivotRight(10);
Robots.MainRobot.MoveForward(100);
Robots.MainRobot.PivotRight(10);
Robots.MainRobot.MoveForward(100);
Robots.MainRobot.PivotRight(10);
Robots.MainRobot.MoveForward(100);
Robots.MainRobot.MoveBackward(1000);
Robots.MainRobot.PivotLeft(90);
Robots.MainRobot.MoveBackward(500);
Robots.MainRobot.PivotLeft(10);
Robots.MainRobot.MoveForward(1000);
Robots.MainRobot.PivotLeft(10);
Robots.MainRobot.MoveForward(100);
Robots.MainRobot.PivotLeft(10);
Robots.MainRobot.MoveForward(100);
Robots.MainRobot.PivotLeft(10);
Robots.MainRobot.MoveForward(100);
Robots.MainRobot.PivotLeft(10);
Robots.MainRobot.MoveForward(100);
Robots.MainRobot.GoToPosition(Recalibration.StartPosition);
Robots.MainRobot.MoveBackward(300);
}
private void Execute()
{
_linkMatch.RegisterName();
SequenceBegin();
SequenceCore();
}
private void ScoreAnimation(ThreadLink link)
{
link.RegisterName();
double progress = 0;
int startYScore = 600;
int endYScore = 241;
int startYSlogan = 507;
int endYSlogan = startYSlogan + (startYScore - endYScore);
int startYSubSlogan = 555;
int endYSubSlogan = startYSubSlogan + (startYScore - endYScore);
int startYLogo = 301;
int endYLogo = startYLogo + (startYScore - endYScore);
int animDuration = 1000;
int fps = 30;
do
{
progress += 1f / fps;
progress = Math.Min(1, progress);
this.InvokeAuto(() =>
{
Font f = new Font(lblScore.Font.Name, (float)(172 - (172 - 124) * Math.Max(0, (progress - 0.5) * 2)));
lblScore.Font = f;
pnlDetails.Location = new Point(pnlDetails.Left, (int)(startYScore - progress * (startYScore - endYScore)));
lblSlogan.Location = new Point(lblSlogan.Left, (int)(startYSlogan - progress * (startYSlogan - endYSlogan)));
lblSubSlogan.Location = new Point(lblSubSlogan.Left, (int)(startYSubSlogan - progress * (startYSubSlogan - endYSubSlogan)));
});
Thread.Sleep(animDuration / fps);
} while (progress < 1);
}
private void ThreadTrajectory(ThreadLink link)
{
link.RegisterName();
this.InvokeAuto(() => btnPathRPCentre.Enabled = false);
Robots.MainRobot.GoToPosition(new Position(positionArrivee.Angle.InDegrees, positionArrivee.Coordinates));
this.InvokeAuto(() => btnPathRPCentre.Enabled = true);
}
private static void ResetOpponentRadiusLoop()
{
_linkResetRadius.RegisterName();
Thread.Sleep(1000);
while (_linkResetRadius != null && !_linkResetRadius.Cancelled && GameBoard.OpponentRadius < GameBoard.OpponentRadiusInitial)
{
GameBoard.OpponentRadius++;
Thread.Sleep(50);
}
}
public void AsyncAsserEnable(ThreadLink link)
{
link.RegisterName();
//TODO2020AllDevices.RecGoBot.Buzz(7000, 200);
Thread.Sleep(500);
TrajectoryFailed = true;
Stop(StopMode.Abrupt);
_lockFrame[UdpFrameFunction.FinDeplacement]?.Release();
//TODO2020AllDevices.RecGoBot.Buzz(0, 200);
}
private void PollingLoop()
{
double posValue;
double ticksCurrent, ticksMin, ticksRange;
int pointsParTour = 4096;
double toursRange = 5;
_linkPolling.RegisterName();
ticksCurrent = 0;//TODO2020 AllDevices.RecGoBot.GetCodeurPosition();
ticksMin = ticksCurrent;
ticksRange = pointsParTour * toursRange;
posValue = _currentPositionnable.Minimum;
while (!_linkPolling.Cancelled)
{
lock (this)
{
_linkPolling.LoopsCount++;
toursRange = trackBarSpeed.Value;
ticksRange = pointsParTour * toursRange;
Thread.Sleep(50);
ticksCurrent = 0;//TODO2020 AllDevices.RecGoBot.GetCodeurPosition();
if (ticksCurrent > ticksMin + ticksRange)
{
ticksMin = ticksCurrent - ticksRange;
}
else if (ticksCurrent < ticksMin)
{
ticksMin = ticksCurrent;
}
posValue = (ticksCurrent - ticksMin) / ticksRange * (_currentPositionnable.Maximum - _currentPositionnable.Minimum) + _currentPositionnable.Minimum;
posValue = Math.Min(posValue, _currentPositionnable.Maximum);
posValue = Math.Max(posValue, _currentPositionnable.Minimum);
}
SetPosition((int)posValue);
}
_linkPolling = null;
}
private void btnOnOff_ValueChanged(object sender, bool value)
{
if (value)
{
ledState.Color = Robots.MainRobot.ReadSensorOnOff(_sensor) ? Color.LimeGreen : Color.Red;
_linkPolling = ThreadManager.CreateThread(link => PollingSensor());
_linkPolling.RegisterName(nameof(PollingSensor) + " " + lblName.Text);
_linkPolling.StartInfiniteLoop(50);
}
else
{
_linkPolling.Cancel();
_linkPolling.WaitEnd();
_linkPolling = null;
ledState.Color = Color.Gray;
}
}
private void UpdateBatteryIcon()
{
_linkBattery?.RegisterName();
picBattery.InvokeAuto(() =>
{
if (!Robots.Simulation && !Connections.ConnectionCanAlim.ConnectionChecker.Connected)
{
picBattery.Image = Properties.Resources.BatteryUnknow96;
lblBattery.Text = ("--,--") + "V";
}
else
{
double voltage = Robots.MainRobot.BatterieVoltage;
if (voltage > Config.CurrentConfig.BatterieRobotVert)
{
picBattery.Image = Properties.Resources.BatteryFull96;
}
else if (voltage > Config.CurrentConfig.BatterieRobotOrange)
{
picBattery.Image = Properties.Resources.BatteryMid96;
}
else if (voltage > Config.CurrentConfig.BatterieRobotRouge)
{
picBattery.Image = Properties.Resources.BatteryLow96;
}
else if (voltage > Config.CurrentConfig.BatterieRobotCritique)
{
picBattery.Image = Properties.Resources.BatteryCritical96;
}
else
{
picBattery.Image = Properties.Resources.BatteryUnknow96;
}
lblBattery.Text = voltage.ToString("00.00") + "V";
}
});
}
private void ThreadGoTo(ThreadLink link)
{
link.RegisterName();
Robot.GoToPosition(new Position((double)numTeta.Value, new RealPoint((double)numX.Value, (double)numY.Value)));
}
void PollingColorRight(ThreadLink link)
{
_linkColorRight = link;
_linkColorRight.RegisterName();
Robots.MainRobot.ReadSensorColor(SensorColorID.BuoyRight, false);
}
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();
}
}
public void GoToDepart(ThreadLink link)
{
link.RegisterName();
Robots.MainRobot.GoToPosition(Recalibration.StartPosition);
}
private void EnvoiTestPid(ThreadLink link)
{
link.RegisterName();
Robot.SendPID((int)numCoeffP.Value, (int)numCoeffI.Value, (int)numCoeffD.Value);
List <int>[] mesures = Robot.DiagnosticPID((int)numPasCodeurs.Value, SensAR.Avant, (int)numNbPoints.Value);
// Afficher les courbes
int derniereValeurGauche = mesures[0][mesures[0].Count - 1];
int derniereValeurDroite = mesures[1][mesures[1].Count - 1];
int premiereValeurGauche = mesures[0][0];
int premiereValeurDroite = mesures[1][0];
for (int i = 0; i < mesures[0].Count; i++)
{
mesures[0][i] = mesures[0][i] - premiereValeurGauche;
}
for (int i = 0; i < mesures[1].Count; i++)
{
mesures[1][i] = mesures[1][i] - premiereValeurDroite;
}
if (derniereValeurGauche == 0)
{
derniereValeurGauche = 1;
}
if (derniereValeurDroite == 0)
{
derniereValeurDroite = 1;
}
double depassementGauchePositif = (mesures[0].Max() - (int)numPasCodeurs.Value) / (double)numPasCodeurs.Value * 100;
double depassementDroitePositif = (mesures[1].Max() - (int)numPasCodeurs.Value) / (double)numPasCodeurs.Value * 100;
int tempsGaucheStable = mesures[0].Count - 1;
while (tempsGaucheStable > 0 && mesures[0][tempsGaucheStable] < mesures[0][mesures[0].Count - 1] * 1.05 && mesures[0][tempsGaucheStable] > mesures[0][mesures[0].Count - 1] * 0.95)
{
tempsGaucheStable--;
}
int tempsDroiteStable = mesures[1].Count - 1;
while (tempsDroiteStable > 0 && mesures[1][tempsDroiteStable] < mesures[1][mesures[1].Count - 1] * 1.05 && mesures[1][tempsDroiteStable] > mesures[1][mesures[1].Count - 1] * 0.95)
{
tempsDroiteStable--;
}
this.InvokeAuto(() =>
{
lblTpsStabilisationGauche.Text = tempsGaucheStable + "ms";
lblTpsStabilisationDroite.Text = tempsDroiteStable + "ms";
lblOvershootGauche.Text = depassementGauchePositif.ToString("0.00") + "%";
lblOvershootDroite.Text = depassementDroitePositif.ToString("0.00") + "%";
lblValeurFinGauche.Text = mesures[0][mesures[0].Count - 1].ToString();
lblValeurFinDroite.Text = mesures[1][mesures[1].Count - 1].ToString();
ctrlGraphique.DeleteCurve("Roue droite");
ctrlGraphique.DeleteCurve("Roue gauche");
for (int i = 0; i < mesures[0].Count; i++)
{
if (boxMoyenne.Checked && i > 1 && i < mesures[0].Count - 2)
{
double valeur = (mesures[0][i - 2] + mesures[0][i - 1] + mesures[0][i] + mesures[0][i + 1] + mesures[0][i + 2]) / 5.0;
ctrlGraphique.AddPoint("Roue gauche", valeur, Color.Blue);
}
else
{
ctrlGraphique.AddPoint("Roue gauche", mesures[0][i], Color.Blue);
}
}
for (int i = 0; i < mesures[1].Count; i++)
{
if (boxMoyenne.Checked && i > 1 && i < mesures[1].Count - 2)
{
double valeur = (mesures[1][i - 2] + mesures[1][i - 1] + mesures[1][i] + mesures[1][i + 1] + mesures[1][i + 2]) / 5.0;
ctrlGraphique.AddPoint("Roue droite", valeur, Color.Green);
}
else
{
ctrlGraphique.AddPoint("Roue droite", mesures[1][i], Color.Green);
}
}
ctrlGraphique.DrawCurves();
});
Robot.DiagnosticPID((int)numPasCodeurs.Value, SensAR.Arriere, (int)numNbPoints.Value);
}