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); }