static void Main(string[] args)
{
/// Enregistrement de la license SciChart en début de code
SciChartSurface.SetRuntimeLicenseKey("RJWA77RbaJDdCRJpg4Iunl5Or6/FPX1xT+Gzu495Eaa0ZahxWi3jkNFDjUb/w70cHXyv7viRTjiNRrYqnqGA+Dc/yzIIzTJlf1s4DJvmQc8TCSrH7MBeQ2ON5lMs/vO0p6rBlkaG+wwnJk7cp4PbOKCfEQ4NsMb8cT9nckfdcWmaKdOQNhHsrw+y1oMR7rIH+rGes0jGGttRDhTOBxwUJK2rBA9Z9PDz2pGOkPjy9fwQ4YY2V4WPeeqM+6eYxnDZ068mnSCPbEnBxpwAldwXTyeWdXv8sn3Dikkwt3yqphQxvs0h6a8Dd6K/9UYni3o8pRkTed6SWodQwICcewfHTyGKQowz3afARj07et2h+becxowq3cRHL+76RyukbIXMfAqLYoT2UzDJNsZqcPPq/kxeXujuhT4SrNF3444MU1GaZZ205KYEMFlz7x/aEnjM6p3BuM6ZuO3Fjf0A0Ki/NBfS6n20E07CTGRtI6AsM2m59orPpI8+24GFlJ9xGTjoRA==");
/// Initialisation des modules utilisés dans le robot
int robotId = 10; /// Ne pas changer cette variable !
int teamId = 0; /// Ne pas changer cette variable !
usbDriver = new USBVendor();
msgDecoder = new MsgDecoder();
msgEncoder = new MsgEncoder();
robotMsgGenerator = new MsgGenerator();
robotMsgProcessor = new MsgProcessor(robotId, competition);
xBoxManette = new XBoxControllerNS.XBoxController(robotId);
strategyManager = new StrategyEurobot(robotId, teamId, "224.16.32.79");
localWorldMapManager = new LocalWorldMapManager(robotId, teamId, bypassMulticast: false);
positioning2Wheels = new Positioning2Wheels(robotId);
trajectoryGenerator = new TrajectoryGeneratorNonHolonome(robotId);
/// Création des liens entre module, sauf depuis et vers l'interface graphique
usbDriver.OnUSBDataReceivedEvent += msgDecoder.DecodeMsgReceived; // Transmission des messages reçus par l'USB au Message Decoder
msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage; // Transmission les messages décodés par le Message Decoder au Message Processor
//Events d'activation et configuration de l'asservissement en vitesse depuis le Strategy Manager
strategyManager.On2WheelsToPolarMatrixSetupEvent += robotMsgGenerator.GenerateMessage2WheelsToPolarMatrixSet; //Transmission des messages de set-up de la matrice de transformation moteurindepeandt -> polaire en embarqué
strategyManager.On2WheelsAngleSetupEvent += robotMsgGenerator.GenerateMessage2WheelsAngleSet; //Transmission des messages de set-up de la config angulaire des roues en embarqué
strategyManager.OnOdometryPointToMeterSetupEvent += robotMsgGenerator.GenerateMessageOdometryPointToMeter; //Transmission des messages de set-up du coeff pointToMeter en embarqué
strategyManager.On2WheelsIndependantSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage2WheelsIndependantSpeedPIDSetup; //Setup du PID independant
strategyManager.OnSetAsservissementModeEvent += robotMsgGenerator.GenerateMessageSetAsservissementMode;
robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot; // Envoi des messages du générateur de message à l'encoder
msgEncoder.OnMessageEncodedEvent += usbDriver.SendUSBMessage; // Envoi des messages en USB depuis le message encoder
robotMsgProcessor.OnPolarOdometrySpeedFromRobotEvent += positioning2Wheels.OnOdometryRobotSpeedReceived; //Envoi des vitesses reçues de l'embarqué au module de calcul de positionnement
positioning2Wheels.OnCalculatedLocationEvent += trajectoryGenerator.OnPhysicalPositionReceived; //Envoi du positionnement calculé au module de génération de trajectoire
trajectoryGenerator.OnGhostLocationEvent += localWorldMapManager.OnGhostLocationReceived;
trajectoryGenerator.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; //Transmission des commande de vitesse aux moteurs de déplacement
strategyManager.OnSetSpeedConsigneToMotorEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToMotor; //Transmission des commande de vitesse (aux moteurs annexes)
positioning2Wheels.OnCalculatedLocationEvent += localWorldMapManager.OnPhysicalPositionReceived;
strategyManager.InitStrategy(); //à faire après avoir abonné les events !
StartRobotInterface();
while (!exitSystem)
{
Thread.Sleep(500);
}
}
static void Main(string[] args)
{
ConsoleFormat.InitMainConsole();
/// Enregistrement de la license SciChart en début de code
SciChartSurface.SetRuntimeLicenseKey(ConstVar.SCICHART_RUNTIME_KEY);
ConsoleFormat.SetupScichartLicenceKey();
/// Initialisation des modules utilisés dans le robot
int robotId = (int)RobotId.Robot1;
int teamId = (int)TeamId.Team1;
usbDriver = new USBVendor();
msgDecoder = new MsgDecoder();
msgEncoder = new MsgEncoder();
msgGenerator = new MsgGenerator();
msgProcessor = new MsgProcessor(robotId, competition);
ConsoleFormat.SetupAllCommunication();
xBoxManette = new XBoxController(robotId);
ConsoleFormat.SetupXboxController();
logRecorder = new LogRecorder();
logReplay = new LogReplay();
strategyManager = new StrategyEurobot(robotId, teamId, "224.16.32.79");
#region Communication to Low Lvl
/// Création des liens entre module, sauf depuis et vers l'interface graphique
usbDriver.OnUSBuffReceivedEvent += msgDecoder.BuffReceived; // Transmission des messages reçus par l'USB au Message Decoder
msgDecoder.OnCorrectMessageReceivedEvent += msgProcessor.ProcessRobotDecodedMessage; // Transmission les messages décodés par le Message Decoder au Message Processor
msgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeAndSendMessage; // Envoi des messages du générateur de message à l'encoder
msgEncoder.OnSendMessageEvent += usbDriver.SendUSBMessage; // Envoi des messages en USB depuis le message encoder
#endregion
#region Console
// Control:
bool hex_viewer = false;
bool hex_sender = false;
bool hex_viewer_error = true;
bool hex_sender_error = true;
bool hex_processor = false;
bool hex_generator = false;
#region USB Vendor
usbDriver.OnDeviceAddedEvent += ConsoleFormat.PrintNewDeviceAdded;
usbDriver.OnDeviceRemovedEvent += ConsoleFormat.PrintDeviceRemoved;
usbDriver.OnUsbVendorExeptionEvent += ConsoleFormat.PrintUsbErrorExeption;
#endregion
#region Hex Viewer
if (hex_viewer)
{
msgDecoder.OnUnknowByteEvent += ConsoleFormat.PrintUnknowByte;
msgDecoder.OnSOFByteReceivedEvent += ConsoleFormat.PrintSOF;
msgDecoder.OnFunctionMSBByteReceivedEvent += ConsoleFormat.PrintFunctionMSB;
msgDecoder.OnFunctionLSBByteReceivedEvent += ConsoleFormat.PrintFunctionLSB;
msgDecoder.OnPayloadLenghtMSBByteReceivedEvent += ConsoleFormat.PrintLenghtMSB;
msgDecoder.OnPayloadLenghtLSBByteReceivedEvent += ConsoleFormat.PrintLenghtLSB;
msgDecoder.OnPayloadByteReceivedEvent += ConsoleFormat.PrintPayloadByte;
msgDecoder.OnCorrectMessageReceivedEvent += ConsoleFormat.PrintCorrectChecksum;
msgDecoder.OnErrorMessageReceivedEvent += ConsoleFormat.PrintWrongChecksum;
}
#endregion
#region Hex Viewer Error
if (hex_viewer_error)
{
msgDecoder.OnOverLenghtMessageEvent += ConsoleFormat.PrintOverLenghtWarning;
msgDecoder.OnUnknowFunctionEvent += ConsoleFormat.PrintUnknowFunctionReceived;
msgDecoder.OnWrongLenghtFunctionEvent += ConsoleFormat.PrintWrongFonctionLenghtReceived;
}
#endregion
#region Hex Sender
if (hex_sender)
{
msgEncoder.OnSendMessageByteEvent += ConsoleFormat.PrintSendMsg;
}
#endregion
#region Hex Sender Error
if (hex_sender_error)
{
msgEncoder.OnSerialDisconnectedEvent += ConsoleFormat.PrintOnSerialDisconnectedError;
msgEncoder.OnUnknownFunctionSentEvent += ConsoleFormat.PrintUnknowFunctionSent;
msgEncoder.OnWrongPayloadSentEvent += ConsoleFormat.PrintWrongFunctionLenghtSent;
}
#endregion
#endregion
#region Lidar
lidar = new SickLidar(17422959); // 18110177
lidarProcess = new LidarProcess(robotId, teamId);
lidar.OnLidarDeviceConnectedEvent += lidarProcess.OnNewLidarConnected;
lidar.OnLidarDeviceConnectedEvent += ConsoleFormat.NewLidarDeviceConnected;
lidar.PointsAvailable += lidarProcess.OnRawPointAvailable;
lidar.Start();
#endregion
lidarProcess.OnRawLidarDataEvent += logRecorder.OnRawLidarDataReceived;
msgProcessor.OnSpeedPolarOdometryFromRobotEvent += logRecorder.OnPolarSpeedDataReceived;
#region Local World Map
localWorldMap = new LocalWorldMap(robotId, teamId);
localWorldMap.OnUpdateRobotLocationEvent += lidarProcess.OnRobotLocation;
lidarProcess.OnRawLidarPointPolarEvent += localWorldMap.OnLidarRawPointReceived;
lidarProcess.OnProcessLidarPolarDataEvent += localWorldMap.OnLidarProcessedPointReceived;
lidarProcess.OnProcessLidarLineDataEvent += localWorldMap.OnLidarProcessedLineReceived;
lidarProcess.OnProcessLidarCupDataEvent += localWorldMap.OnLidarProcessedCupReceived;
lidarProcess.OnLidarSetupRobotLocationEvent += localWorldMap.OnRobotLocation;
//lidarProcess.OnProcessLidarObjectsDataEvent += localWorldMap.OnLidarProcesObjectsReceived;
localWorldMap.OnLocalWorldMapEvent += strategyManager.OnLocalWorldMapReceived;
localWorldMap.Init();
#endregion
#region Position2Wheels
positioning2Wheels = new Positioning2Wheels(robotId);
msgProcessor.OnSpeedPolarOdometryFromRobotEvent += positioning2Wheels.OnOdometryRobotSpeedReceived;
positioning2Wheels.OnCalculatedLocationEvent += localWorldMap.OnRobotLocationArgs;
#endregion
#region TrajectoryPlanner
trajectoryPlanner = new TrajectoryPlanner(robotId);
trajectoryPlanner.OnNewGhostLocationEvent += localWorldMap.OnGhostLocation;
trajectoryPlanner.OnNewRobotLocationEvent += localWorldMap.OnRobotLocation;
trajectoryPlanner.OnDestinationReachedEvent += strategyManager.OnGhostLocationReached;
#endregion
#region Strategy /!\Need to be Last /! \
strategyManager.On2WheelsToPolarMatrixSetupEvent += msgGenerator.GenerateMessage2WheelsToPolarMatrixSet; //Transmission des messages de set-up de la matrice de transformation moteurindepeandt -> polaire en embarqué
strategyManager.On2WheelsAngleSetupEvent += msgGenerator.GenerateMessage2WheelsAngleSet; //Transmission des messages de set-up de la config angulaire des roues en embarqué
strategyManager.OnOdometryPointToMeterSetupEvent += msgGenerator.GenerateMessageOdometryPointToMeter; //Transmission des messages de set-up du coeff pointToMeter en embarqué
strategyManager.OnSetAsservissementModeEvent += msgGenerator.GenerateMessageSetAsservissementMode;
strategyManager.OnDestinationReachedEvent += localWorldMap.OnDestinationReached;
strategyManager.OnWaypointsReachedEvent += localWorldMap.OnWaypointReached;
strategyManager.OnSetActualLocationEvent += trajectoryPlanner.OnUpdateActualLocation;
strategyManager.OnSetWantedLocationEvent += trajectoryPlanner.OnUpdateWantedDestination;
strategyManager.OnGhostCalculationBeginEvent += trajectoryPlanner.OnLaunchCalculation;
strategyManager.OnSetNewWaypointEvent += localWorldMap.AddNewWaypointsEvent;
strategyManager.OnSetNewDestinationEvent += localWorldMap.SetDestinationLocationEvent;
strategyManager.OnUpdateGhostCalculationOrderEvent += trajectoryPlanner.OnCalculateGhostMovement;
ConsoleFormat.PrintStrategyBoot();
strategyManager.InitStrategy(); //à faire après avoir abonné les events !
#endregion
if (usingMatchDisplay)
{
StartMatchInterface();
}
else
{
StartRobotInterface();
}
ConsoleFormat.EndMainBootSequence();
while (!exitSystem)
{
Thread.Sleep(500);
}
}
static void Main(string[] args)
{
SetConsoleCtrlHandler(new HandlerRoutine(ConsoleCtrlCheck), true);
// Set this code once in App.xaml.cs or application startup
SciChartSurface.SetRuntimeLicenseKey("wsCOsvBlAs2dax4o8qBefxMi4Qe5BVWax7TGOMLcwzWFYRNCa/f1rA5VA1ITvLHSULvhDMKVTc+niao6URAUXmGZ9W8jv/4jtziBzFZ6Z15ek6SLU49eIqJxGoQEFWvjANJqzp0asw+zvLV0HMirjannvDRj4i/WoELfYDubEGO1O+oAToiJlgD/e2lVqg3F8JREvC0iqBbNrmfeUCQdhHt6SKS2QpdmOoGbvtCossAezGNxv92oUbog6YIhtpSyGikCEwwKSDrlKlAab6302LLyFsITqogZychLYrVXJTFvFVnDfnkQ9cDi7017vT5flesZwIzeH497lzGp3B8fKWFQyZemD2RzlQkvj5GUWBwxiKAHrYMnQjJ/PsfojF1idPEEconVsh1LoYofNk2v/Up8AzXEAvxWUEcgzANeQggaUNy+OFet8b/yACa/bgYG7QYzFQZzgdng8IK4vCPdtg4/x7g5EdovN2PI9vB76coMuKnNVPnZN60kSjtd/24N8A==");
switch (robotMode)
{
case RobotMode.Standard:
usingLidar = true;
usingCamera = true;
usingLogging = false;
usingLogReplay = false;
break;
case RobotMode.Acquisition:
usingLidar = true;
usingCamera = true;
usingLogging = true;
usingLogReplay = false;
break;
case RobotMode.Replay:
usingLidar = false;
usingCamera = false;
usingLogging = false;
usingLogReplay = true;
break;
case RobotMode.Nolidar:
usingLidar = false;
usingCamera = true;
usingLogging = false;
usingLogReplay = false;
break;
case RobotMode.NoCamera:
usingLidar = true;
usingCamera = false;
usingLogging = false;
usingLogReplay = false;
break;
}
int robotId = (int)TeamId.Team1 + (int)RobotId.Robot1;
int teamId = (int)TeamId.Team1;
serialPort1 = new ReliableSerialPort("COM1", 115200, Parity.None, 8, StopBits.One);
msgDecoder = new MsgDecoder();
msgEncoder = new MsgEncoder();
robotMsgGenerator = new MsgGenerator();
robotMsgProcessor = new MsgProcessor(robotId, GameMode.RoboCup);
robotPilot = new RobotPilot.RobotPilot(robotId);
strategyManager = new StrategyRoboCup(robotId, teamId, "224.16.32.79");
waypointGenerator = new WaypointGenerator(robotId, GameMode.RoboCup);
trajectoryPlanner = new TrajectoryGeneratorHolonome(robotId, GameMode.RoboCup);
kalmanPositioning = new KalmanPositioning.KalmanPositioning(robotId, 50, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, 0.02);
localWorldMapManager = new LocalWorldMapManager(robotId, teamId, bypassMulticast: false);
perceptionManager = new PerceptionManager(robotId, GameMode.RoboCup);
imuProcessor = new ImuProcessor.ImuProcessor(robotId);
if (usingYolo)
{
yoloDetector = new YoloObjectDetector.YoloObjectDetector(false); //Instancie un detecteur avec un Wrappeur Yolo utilisant le GPU
}
if (usingLidar)
{
lidar_OMD60M_TCP = new LidaRxR2000();
}
if (usingLidar || usingLogReplay)
{
lidarProcessor = new LidarProcessor.LidarProcessor(robotId, GameMode.RoboCup);
}
xBoxManette = new XBoxControllerNS.XBoxController(robotId);
if (usingCamera || usingLogReplay)
{
imageProcessingPositionFromOmniCamera = new ImageProcessingPositionFromOmniCamera();
absolutePositionEstimator = new AbsolutePositionEstimator(robotId);
}
if (usingCamera)
{
omniCamera = new BaslerCameraAdapter();
omniCamera.CameraInit();
//omniCamera.BitmapPanoramaImageEvent += absolutePositionEstimator.AbsolutePositionEvaluation;
}
if (usingImageExtractor && usingCamera)
{
imgSaver = new ImageSaver.ImageSaver();
omniCamera.BitmapPanoramaImageEvent += imgSaver.OnSaveBitmapImage;
}
//Démarrage des interface de visualisation
if (usingRobotInterface)
{
StartRobotInterface();
}
if (usingCameraInterface)
{
StartCameraInterface();
}
//Démarrage du logger si besoin
if (usingLogging)
{
logRecorder = new LogRecorder.LogRecorder();
}
//Démarrage du log replay si l'interface est utilisée et existe ou si elle n'est pas utilisée, sinon on bloque
if (usingLogReplay)
{
logReplay = new LogReplay.LogReplay();
}
//Liens entre modules
strategyManager.OnDestinationEvent += waypointGenerator.OnDestinationReceived;
strategyManager.OnHeatMapStrategyEvent += waypointGenerator.OnStrategyHeatMapReceived;
waypointGenerator.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived;
//Filtre de Kalman perceptionManager.OnAbsolutePositionEvent += kalmanPositioning.OnAbsolutePositionCalculatedEvent;
robotMsgProcessor.OnPolarOdometrySpeedFromRobotEvent += kalmanPositioning.OnOdometryRobotSpeedReceived;
imuProcessor.OnGyroSpeedEvent += kalmanPositioning.OnGyroRobotSpeedReceived;
kalmanPositioning.OnKalmanLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived;
kalmanPositioning.OnKalmanLocationEvent += perceptionManager.OnPhysicalRobotPositionReceived;
//kalmanPositioning.OnKalmanLocationEvent += strategyManager.OnPositionRobotReceived;
//L'envoi des commandes dépend du fait qu'on soit en mode manette ou pas.
//Il faut donc enregistrer les évènement ou pas en fonction de l'activation
//C'est fait plus bas dans le code avec la fonction que l'on appelle
ConfigControlEvents(useXBoxController: true);
//Gestion des messages envoyé par le robot
robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot;
msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage;
//Gestion des messages reçu par le robot
serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived;
msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage;
robotMsgProcessor.OnIMURawDataFromRobotGeneratedEvent += imuProcessor.OnIMURawDataReceived;
//physicalSimulator.OnPhysicalRobotLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived;
//physicalSimulator.OnPhysicicalObjectListLocationEvent += perceptionSimulator.OnPhysicalObjectListLocationReceived;
//physicalSimulator.OnPhysicalRobotLocationEvent += perceptionSimulator.OnPhysicalRobotPositionReceived;
//physicalSimulator.OnPhysicalBallPositionEvent += perceptionSimulator.OnPhysicalBallPositionReceived;
perceptionManager.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived;
strategyManager.OnDestinationEvent += localWorldMapManager.OnDestinationReceived;
waypointGenerator.OnWaypointEvent += localWorldMapManager.OnWaypointReceived;
strategyManager.OnHeatMapStrategyEvent += localWorldMapManager.OnHeatMapStrategyReceived;
//if (usingLidar)
//{
// lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += lidarProcessor.OnRawLidarDataReceived;
// //lidar_OMD60M.OnLidarDecodedFrameEvent += absolutePositionEstimator.OnRawLidarDataReceived;
// lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += localWorldMapManager.OnRawLidarDataReceived;
// lidarProcessor.OnLidarObjectProcessedEvent += localWorldMapManager.OnLidarObjectsReceived;
//}
if (usingLidar)
{
lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += perceptionManager.OnRawLidarDataReceived;
//lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += localWorldMapManager.OnLidarDataReceived;
lidarProcessor.OnLidarProcessedEvent += localWorldMapManager.OnLidarDataReceived;
}
//Events de recording
if (usingLogging)
{
//lidar_OMD60M_UDP.OnLidarDecodedFrameEvent += logRecorder.OnRawLidarDataReceived;
lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += logRecorder.OnRawLidarDataReceived;
omniCamera.BitmapFishEyeImageEvent += logRecorder.OnBitmapImageReceived;
imuProcessor.OnIMUProcessedDataGeneratedEvent += logRecorder.OnIMURawDataReceived;
robotMsgProcessor.OnPolarOdometrySpeedFromRobotEvent += logRecorder.OnPolarSpeedDataReceived;
//omniCamera.OpenCvMatImageEvent += logRecorder.OnOpenCVMatImageReceived;
}
if (usingLogReplay)
{
logReplay.OnLidarEvent += perceptionManager.OnRawLidarDataReceived;
//logReplay.OnCameraImageEvent += imageProcessingPositionFromOmniCamera.ProcessOpenCvMatImage;
//logReplay.OnCameraImageEvent += absolutePositionEstimator.AbsolutePositionEvaluation;
//lidarProcessor.OnLidarObjectProcessedEvent += localWorldMapManager.OnLidarObjectsReceived;
}
lock (ExitLock)
{
// Do whatever setup code you need here
// once we are done wait
Monitor.Wait(ExitLock);
}
}
