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) { /// We add an event handler to detect the closing of the application _handler += new EventHandler(Handler); SetConsoleCtrlHandler(_handler, true); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); msgGenerator = new MsgGenerator(); msgProcessor = new MsgProcessor(); serialPort = new SerialAutoConnect(); /// Creation of links between modules, except from and to the graphical interface Console.WriteLine("[MAIN] Begin Booting Sequence"); /// Creation of link between projects msgDecoder.OnMessageDecodedEvent += msgProcessor.ProcessRobotDecodedMessage; /// Management of messages sent by the robot msgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot; msgEncoder.OnMessageEncodedEvent += serialPort.SendMessageToRobot; /// Management of messages received by the robot serialPort.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived; msgDecoder.OnMessageDecodedEvent += msgProcessor.ProcessRobotDecodedMessage; /// management of messages to be displayed in the console serialPort.OnSerialConnectedEvent += ConsolePrint.OnPrintEvent; RegisterConsolePrintEvents(); /// Launching GUI if (usingRobotInterface) { StartRobotInterface(); } Console.Write("[MAIN] End Booting Sequence"); while (!exitSystem) { Thread.Sleep(500); } }
[STAThread] //à ajouter au projet initial static void Main(string[] args) { SciChartSurface.SetRuntimeLicenseKey(@"<LicenseContract> <Customer>University of Toulon</Customer> <OrderId>EDUCATIONAL-USE-0109</OrderId> <LicenseCount>1</LicenseCount> <IsTrialLicense>false</IsTrialLicense> <SupportExpires>11/04/2019 00:00:00</SupportExpires> <ProductCode>SC-WPF-SDK-PRO-SITE</ProductCode> <KeyCode>lwABAQEAAABZVzOfQ0zVAQEAewBDdXN0b21lcj1Vbml2ZXJzaXR5IG9mICBUb3Vsb247T3JkZXJJZD1FRFVDQVRJT05BTC1VU0UtMDEwOTtTdWJzY3JpcHRpb25WYWxpZFRvPTA0LU5vdi0yMDE5O1Byb2R1Y3RDb2RlPVNDLVdQRi1TREstUFJPLVNJVEWDf0QgB8GnCQXI6yAqNM2njjnGbUt2KsujTDzeE+k69K1XYVF1s1x1Hb/i/E3GHaU=</KeyCode> </LicenseContract>"); serialPort1 = new ReliableSerialPort("COM19", 115200, Parity.None, 8, StopBits.One); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); msgGenerator = new MsgGenerator(); msgProcessor = new MsgProcessor(); //Gestion des messages envoyé par le respirateur msgGenerator.OnMessageToRespirateurGeneratedEvent += msgEncoder.EncodeMessageToRespirateur; msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage; //Gestion des messages reçu par le respirateur serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived; msgDecoder.OnMessageDecodedEvent += msgProcessor.ProcessDecodedMessage; StartInterface(); lock (ExitLock) { // Do whatever setup code you need here // once we are done wait System.Threading.Monitor.Wait(ExitLock); } }
private static bool ProcessMessage(byte[] body) { try { MsgDecoder msgDecoder = new MsgDecoder(); JT808_PackageData packageData = new JT808_PackageData(); string content = ""; content = BitConverter.ToString(body).Replace("-", " "); Console.WriteLine("Received: {0}", content); logger.Info("Received: " + content); //终端设备号 消息体前6个byte String deviceCode = body[0].ToString("X2") + body[1].ToString("X2") + body[2].ToString("X2") + body[3].ToString("X2") + body[4].ToString("X2") + body[5].ToString("X2"); packageData.locationInfo = (msgDecoder.ToLocationInfoMsg(body)); string alc = packageData.locationInfo.alc; //报警标志 string bst = packageData.locationInfo.bst; //状态 double lon = packageData.locationInfo.lon; //经度 double lat = packageData.locationInfo.lat; //纬度 double hgt = packageData.locationInfo.hgt; //高度 double spd = packageData.locationInfo.spd; //速度 double agl = packageData.locationInfo.agl; //方向 DateTime gtm = packageData.locationInfo.gtm; //时间 double mlg = packageData.locationInfo.mlg; //里程 double oil = packageData.locationInfo.oil; //油量 double spd2 = packageData.locationInfo.spd2; //记录仪速度 int est = 0; //信号状态 if (packageData.locationInfo.est != "") { est = int.Parse(packageData.locationInfo.est); } int io = 0;//IO状态位 if (packageData.locationInfo.io != "") { io = int.Parse(packageData.locationInfo.io); } int ad1 = 0;//模拟量 if (packageData.locationInfo.ad1 != "") { ad1 = int.Parse(packageData.locationInfo.ad1); } int yte = packageData.locationInfo.yte; //信号强度 int gnss = packageData.locationInfo.gnss; //定位卫星数 string connectionStr = //从配置文件读取连接字符串 WebConfigurationManager.ConnectionStrings["connStr1"].ConnectionString; //string connectionStr = "data source='" + datasource + "';user id='" + uname + "';password='******';charset=utf8"; DBConnectionSingletion.ConnectionString = connectionStr; MySqlConnection conn = pool.BorrowDBConnection();//从连接池借一个连接对象 String sql = "SELECT * FROM gps_main.t_gps_main where device_code = '" + deviceCode + "'"; DataSet result = null; try { result = DBGuid.Select(conn, sql, "t_gps_main"); } catch (Exception e) { logger.Error(e.Message); } //将连接对象还给连接池 pool.ReturnDBConnection(conn); TBOperation operation = new TBOperation(); int latterSeq = operation.SelectLatterSeqNo(conn, "gps"); //已存在的最大轨迹序列号 int latestSeq = TbUtil.CreateLatestSeqNo(latterSeq, "gps"); //创建新的轨迹序列号 //int latterAlarmSeq = operation.SelectLatterSeqNo(conn, "alarm");//已存在的最大报警序列号 //int latestAlarmSeq = TbUtil.CreateLatestSeqNo(latterAlarmSeq, "alarm");//创建新的报警序列号 if (result != null && result.Tables.Count > 0 && result.Tables[0].Rows.Count > 0) {//主表已保存该设备信息 //更新轨迹快照表sql DataRow deviceRow = result.Tables[0].Rows[0]; string vendor = deviceRow["vendor_code"].ToString(); string updateSnapSql = string.Format("UPDATE gps_main.t_gps_snapshot " + "set alarm_status={1},vehicle_status={2},lat={3},lon={4},height={5},speed={6}," + "direction={7},time={8},mile={9},oil={10},speed2={11},signal_status={12},bst={13},io_status={14},analog={15},wifi={16},satellite_num={17},create_time={18},vendor_code='{19}'" + " where device_code='{0}';", deviceCode, alc, bst, lat, lon, hgt, spd, agl, TbUtil.GetGMTInMS(gtm), mlg, oil, spd2, est, bst, io, ad1, yte, gnss, "FROM_UNIXTIME(NOW())", vendor); Console.WriteLine("<<Info>>update gps_main.t_gps_snapshot {0}", deviceCode); logger.Info("update gps_main.t_gps_snapshot " + deviceCode); if (conn != null && !conn.State.Equals("open")) { conn = pool.BorrowDBConnection();//从连接池借一个连接对象 } DBGuid.Update(conn, updateSnapSql); pool.ReturnDBConnection(conn);//将连接对象还给连接池 //轨迹表新增一条轨迹信息 string dbName = TbUtil.GetDbName("gps", latestSeq, "gps"); string tbName = TbUtil.GetTableName(packageData.locationInfo.gtm, "t_gps", latestSeq, "gps"); string tbPath = dbName + "." + tbName; string insertGpsSql = string.Format("INSERT INTO " + tbPath + " (device_code,alarm_status,vehicle_status,lat,lon,height,speed," + "direction,time,mile,oil,speed2,signal_status,bst,io_status,analog,wifi,satellite_num,create_time,vendor_code)" + "VALUES ('{0}',{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},'{15}',{16},{17},{18},'{19}');", deviceCode, alc, bst, lat, lon, hgt, spd, agl, TbUtil.GetGMTInMS(gtm), mlg, oil, spd2, est, bst, io, ad1, yte, gnss, "FROM_UNIXTIME(NOW())", vendor); Console.WriteLine("<<Info>>insert {0} {1}", tbPath, deviceCode); logger.Info("insert " + tbPath + " " + deviceCode); if (conn != null && !conn.State.Equals("open")) { conn = pool.BorrowDBConnection();//从连接池借一个连接对象 } DBGuid.Insert(conn, insertGpsSql); pool.ReturnDBConnection(conn);//将连接对象还给连接池 //报警表新增一条报警信息 //先判断是否有报警 //string alarmStatus = packageData.locationInfo.alc; //if (alarmStatus.Equals("0000"))//有报警则新增 //{ // string alarmDb = TbUtil.GetDbName("gps_alarm", latestAlarmSeq, "alarm"); // string alarmTb = TbUtil.GetTableName(gtm, "t_gps_alarm", latestAlarmSeq, "alarm"); // string alarmTbPath = alarmDb + "." + alarmTb; // string insertalarmsql = string.Format("insert into " + alarmTbPath + // " (device_code,alarm_status,vehicle_status,lat,lon,height,speed," + // "direction,time,mile,oil,speed2,signal_status,bst,io_status,analog,wifi,satellite_num,create_time,vendor_code,alarm_handle)" + // "values ('{0}',{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},'{15}',{16},{17},{18},'{19}',{20});", // deviceCode, alc, bst, lat, lon, hgt, spd, agl, TbUtil.GetGMTInMS(gtm), mlg, oil, spd2, est, bst, io, ad1, yte, gnss, "from_unixtime(now())", vendor, 0); //} } else {//主表不存在该设备信息 //插入main表sql //for test //随机生成一个车牌号 Random R = new Random(); string[] strArr = { "A", "B", "C", "D", "E", "F", "G", "H", "I", "J", "K", "L", "M", "N", "O", "P", "Q", "R", "S", "T", "U", "V","W", "X", "Y", "Z" }; int ron = R.Next(1000, 9999); int ran = R.Next(0, 25); String plateNo = "浙" + strArr[ran] + strArr[ran] + ron; //随机生成一个运营商代码 String vendor = ""; for (int i = 0; i < 6; i++) { vendor += strArr[ran]; } vendor = vendor + ron; String remark = "test" + DateTime.Now.ToString(); //主表新增一条设备信息 String insertMainSql = string.Format("INSERT INTO gps_main.t_gps_main (device_code,plate_no,vendor_code,trail_seq_no,alarm_seq_no,status,remark)" + "VALUES ('{0}', '{1}', '{2}', '{3}', '{4}', '{5}', '{6}');", deviceCode, plateNo, vendor, latestSeq, 0, 10, remark); Console.WriteLine("<<Info>>insert gps_main.t_gps_main {0}", deviceCode); logger.Info("insert gps_main.t_gps_main " + deviceCode); if (conn != null && !conn.State.Equals("open")) { conn = pool.BorrowDBConnection();//从连接池借一个连接对象 } DBGuid.Insert(conn, insertMainSql); pool.ReturnDBConnection(conn);//将连接对象还给连接池 //插入轨迹快照表sql string insertSnapSql = string.Format("INSERT INTO " + "gps_main.t_gps_snapshot" + " (device_code,alarm_status,vehicle_status,lat,lon,height,speed," + "direction,time,mile,oil,speed2,signal_status,bst,io_status,analog,wifi,satellite_num,create_time,vendor_code)" + "VALUES ('{0}',{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},'{15}',{16},{17},{18},'{19}');", deviceCode, alc, bst, lat, lon, hgt, spd, agl, TbUtil.GetGMTInMS(gtm), mlg, oil, spd2, est, bst, io, ad1, yte, gnss, "FROM_UNIXTIME(NOW())", vendor); if (conn != null && !conn.State.Equals("open")) { conn = pool.BorrowDBConnection();//从连接池借一个连接对象 } Console.WriteLine("<<Info>>insert gps_main.t_gps_snapshot {0}", deviceCode); logger.Info("insert gps_main.t_gps_snapshot " + deviceCode); DBGuid.Insert(conn, insertSnapSql); pool.ReturnDBConnection(conn);//将连接对象还给连接池 //轨迹表新增一条轨迹信息 string dbName = TbUtil.GetDbName("gps", latestSeq, "gps"); string tbName = TbUtil.GetTableName(packageData.locationInfo.gtm, "t_gps", latestSeq, "gps"); string tbPath = dbName + "." + tbName; string insertGpsSql = string.Format("INSERT INTO " + tbPath + " (device_code,alarm_status,vehicle_status,lat,lon,height,speed," + "direction,time,mile,oil,speed2,signal_status,bst,io_status,analog,wifi,satellite_num,create_time,vendor_code)" + "VALUES ('{0}',{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},'{15}',{16},{17},{18},'{19}');", deviceCode, alc, bst, lat, lon, hgt, spd, agl, TbUtil.GetGMTInMS(gtm), mlg, oil, spd2, est, bst, io, ad1, yte, gnss, "FROM_UNIXTIME(NOW())", vendor); if (conn != null && !conn.State.Equals("open")) { conn = pool.BorrowDBConnection();//从连接池借一个连接对象 } Console.WriteLine("<<Info>>insert {0} {1}", tbPath, deviceCode); logger.Info("insert " + tbPath + " " + deviceCode); DBGuid.Insert(conn, insertGpsSql); pool.ReturnDBConnection(conn);//将连接对象还给连接池 //报警表新增一条报警信息 //string alarmDb = TbUtil.GetDbName("gps_alarm", latestAlarmSeq, "alarm"); //string alarmTb = TbUtil.GetTableName(gtm, "t_gps_alarm", latestAlarmSeq, "alarm"); //string alarmTbPath = alarmDb + "." + alarmTb; //string insertalarmsql = string.Format("insert into " + alarmTbPath + // " (device_code,alarm_status,vehicle_status,lat,lon,height,speed," + // "direction,time,mile,oil,speed2,signal_status,bst,io_status,analog,wifi,satellite_num,create_time,vendor_code,alarm_handle)" + // "values ('{0}',{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},'{15}',{16},{17},{18},'{19}',{20});", // devicecode, alc, bst, lat, lon, hgt, spd, agl, operation.getgmtinms(gtm), mlg, oil, spd2, est, bst, io, ad1, yte, gnss, "from_unixtime(now())", vendor, 0); } } catch (Exception e) { logger.Error(e.Message); return(false); } return(true); }
public Protobuf(MessageObject encodeProtos, MessageObject decodeProtos) { this.encoder = new MsgEncoder(encodeProtos); this.decoder = new MsgDecoder(decodeProtos); }
public HLBeginEndMarkReceiveFilter(IAppSession appSession) : base(BeginMark, EndMark) { this.appSession = appSession; decoder = new MsgDecoder(); //encoder = new MsgEncoder(appSession.Logger); }
static void Main(string[] args) { // Set this code once in App.xaml.cs or application startup SciChartSurface.SetRuntimeLicenseKey("RJWA77RbaJDdCRJpg4Iunl5Or6/FPX1xT+Gzu495Eaa0ZahxWi3jkNFDjUb/w70cHXyv7viRTjiNRrYqnqGA+Dc/yzIIzTJlf1s4DJvmQc8TCSrH7MBeQ2ON5lMs/vO0p6rBlkaG+wwnJk7cp4PbOKCfEQ4NsMb8cT9nckfdcWmaKdOQNhHsrw+y1oMR7rIH+rGes0jGGttRDhTOBxwUJK2rBA9Z9PDz2pGOkPjy9fwQ4YY2V4WPeeqM+6eYxnDZ068mnSCPbEnBxpwAldwXTyeWdXv8sn3Dikkwt3yqphQxvs0h6a8Dd6K/9UYni3o8pRkTed6SWodQwICcewfHTyGKQowz3afARj07et2h+becxowq3cRHL+76RyukbIXMfAqLYoT2UzDJNsZqcPPq/kxeXujuhT4SrNF3444MU1GaZZ205KYEMFlz7x/aEnjM6p3BuM6ZuO3Fjf0A0Ki/NBfS6n20E07CTGRtI6AsM2m59orPpI8+24GFlJ9xGTjoRA=="); //On ajoute un gestionnaire d'évènement pour détecter la fermeture de l'application _handler += new EventHandler(Handler); SetConsoleCtrlHandler(_handler, true); //serialPort1 = new ReliableSerialPort(cfgSerialPort.CommName, cfgSerialPort.ComBaudrate, cfgSerialPort.Parity, cfgSerialPort.DataByte, cfgSerialPort.StopByte); //serialPort1 = new ReliableSerialPort("COM1", 115200, Parity.None, 8, StopBits.One); int teamId = (int)TeamId.Team1; int robotId = (int)RobotId.Robot1 + teamId; usbDriver = new USBVendorNS.USBVendor(); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); robotMsgGenerator = new MsgGenerator(); robotMsgProcessor = new MsgProcessor(robotId, competition); imuProcessor = new ImuProcessor.ImuProcessor(robotId); lidar_OMD60M_TCP = new LidaRxR2000(50, R2000SamplingRate._72kHz); perceptionManager = new PerceptionManager(robotId, competition); kalmanPositioning = new KalmanPositioning.KalmanPositioning(robotId, 50, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, 0.02); trajectoryPlanner = new TrajectoryGeneratorHolonome(robotId, competition); localWorldMapManager = new LocalWorldMapManager(robotId, teamId, bypassMulticast: false); globalWorldMapManager = new GlobalWorldMapManager(robotId, teamId); switch (competition) { case GameMode.RoboCup: strategyManager = new StrategyRoboCup(robotId, teamId, "224.16.32.79"); break; case GameMode.Eurobot: strategyManager = new StrategyEurobot2021(robotId, teamId, "224.16.32.79"); break; case GameMode.Demo: break; } //strategyManager = new StrategyManagerNS.StrategyManager(robotId, teamId, "224.16.32.79", competition); robotUdpMulticastSender = new UDPMulticastSender(robotId, "224.16.32.79"); robotUdpMulticastReceiver = new UDPMulticastReceiver(robotId, "224.16.32.79"); robotUdpMulticastInterpreter = new UDPMulticastInterpreter(robotId); herkulexManager = new HerkulexManager(); herkulexManager.AddServo(ServoId.BrasCentral, HerkulexDescription.JOG_MODE.positionControlJOG); herkulexManager.AddServo(ServoId.BrasDroit, HerkulexDescription.JOG_MODE.positionControlJOG); herkulexManager.AddServo(ServoId.BrasGauche, HerkulexDescription.JOG_MODE.positionControlJOG); herkulexManager.AddServo(ServoId.PorteDrapeau, HerkulexDescription.JOG_MODE.positionControlJOG); xBoxManette = new XBoxControllerNS.XBoxController(robotId); //Démarrage des interface de visualisation if (usingRobotInterface) { StartRobotInterface(); } //if (usingLogReplay) // StartReplayNavigatorInterface(); //Initialisation du logger 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 logReplay = new LogReplay.LogReplay(); //Liens entre modules //strategyManager.strategy.OnRefereeBoxCommandEvent += globalWorldMapManager.OnRefereeBoxCommandReceived; strategyManager.OnGameStateChangedEvent += trajectoryPlanner.OnGameStateChangeReceived; strategyManager.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived; //Kalman perceptionManager.OnAbsolutePositionEvent += kalmanPositioning.OnAbsolutePositionCalculatedEvent; imuProcessor.OnGyroSpeedEvent += kalmanPositioning.OnGyroRobotSpeedReceived; robotMsgProcessor.OnPolarOdometrySpeedFromRobotEvent += kalmanPositioning.OnOdometryRobotSpeedReceived; kalmanPositioning.OnKalmanLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived; kalmanPositioning.OnKalmanLocationEvent += perceptionManager.OnPhysicalRobotPositionReceived; kalmanPositioning.OnKalmanLocationEvent += strategyManager.OnPositionRobotReceived; //Update des données de la localWorldMap perceptionManager.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived; strategyManager.OnDestinationEvent += localWorldMapManager.OnDestinationReceived; strategyManager.OnRoleEvent += localWorldMapManager.OnRoleReceived; //Utile pour l'affichage strategyManager.OnMessageDisplayEvent += localWorldMapManager.OnMessageDisplayReceived; //Utile pour l'affichage strategyManager.OnHeatMapStrategyEvent += localWorldMapManager.OnHeatMapStrategyReceived; strategyManager.OnWaypointEvent += localWorldMapManager.OnWaypointReceived; strategyManager.OnHeatMapWayPointEvent += localWorldMapManager.OnHeatMapWaypointReceived; trajectoryPlanner.OnGhostLocationEvent += localWorldMapManager.OnGhostLocationReceived; //Gestion des events liés à une détection de collision soft trajectoryPlanner.OnCollisionEvent += kalmanPositioning.OnCollisionReceived; //trajectoryPlanner.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; strategyManager.OnMessageEvent += lidar_OMD60M_TCP.OnMessageReceivedEvent; strategyManager.On4WheelsPolarSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage4WheelsPolarSpeedPIDSetup; strategyManager.On4WheelsIndependantSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage4WheelsIndependantSpeedPIDSetup; strategyManager.On2WheelsPolarSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage2WheelsPolarSpeedPIDSetup; strategyManager.On2WheelsIndependantSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage2WheelsIndependantSpeedPIDSetup; strategyManager.OnSetAsservissementModeEvent += robotMsgGenerator.GenerateMessageSetAsservissementMode; strategyManager.OnSetSpeedConsigneToMotor += robotMsgGenerator.GenerateMessageSetSpeedConsigneToMotor; strategyManager.OnEnableDisableMotorCurrentDataEvent += robotMsgGenerator.GenerateMessageEnableMotorCurrentData; strategyManager.OnOdometryPointToMeterEvent += robotMsgGenerator.GenerateMessageOdometryPointToMeter; strategyManager.On4WheelsAngleSetEvent += robotMsgGenerator.GenerateMessage4WheelsAngleSet; strategyManager.On4WheelsToPolarSetEvent += robotMsgGenerator.GenerateMessage4WheelsToPolarMatrixSet; strategyManager.On2WheelsAngleSetEvent += robotMsgGenerator.GenerateMessage2WheelsAngleSet; strategyManager.On2WheelsToPolarSetEvent += robotMsgGenerator.GenerateMessage2WheelsToPolarMatrixSet; herkulexManager.OnHerkulexSendToSerialEvent += robotMsgGenerator.GenerateMessageForwardHerkulex; lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += perceptionManager.OnRawLidarDataReceived; perceptionManager.OnLidarRawDataEvent += localWorldMapManager.OnRawLidarDataReceived; perceptionManager.OnLidarProcessedDataEvent += localWorldMapManager.OnLidarDataReceived; perceptionManager.OnLidarProcessedLandmarksEvent += localWorldMapManager.OnLidarDataReceived; perceptionManager.OnLidarProcessedSegmentsEvent += localWorldMapManager.OnLidarProcessedSegmentsReceived; //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(usingXBoxController); //Gestion des messages envoyé par le robot robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot; //msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage; msgEncoder.OnMessageEncodedEvent += usbDriver.SendUSBMessage; //Gestion des messages reçu par le robot //serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived; usbDriver.OnUSBDataReceivedEvent += msgDecoder.DecodeMsgReceived; msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage; robotMsgProcessor.OnIMURawDataFromRobotGeneratedEvent += imuProcessor.OnIMURawDataReceived; robotMsgProcessor.OnIOValuesFromRobotGeneratedEvent += strategyManager.OnIOValuesFromRobot; robotMsgProcessor.OnIOValuesFromRobotGeneratedEvent += perceptionManager.OnIOValuesFromRobotEvent; landmarksExtractor = new LandmarksExtractor(); landmarksExtractor.OnLinesLandmarksExtractedEvent += perceptionManager.OnLandmarksReceived; perceptionManager.OnLidarRawDataEvent += landmarksExtractor.OnRobotLidarReceived; perceptionManager.OnAbsolutePositionEvent += landmarksExtractor.OnRobotPositionReceived; // robotMsgProcessor.OnMotorsCurrentsFromRobotGeneratedEvent += strategyManager.OnMotorCurrentReceive; //Le local Manager n'est là que pour assurer le stockage de ma local world map avant affichage et transmission des infos, il ne doit pas calculer quoique ce soit, //c'est le perception manager qui le fait. trajectoryPlanner.OnPidSpeedResetEvent += robotMsgGenerator.GenerateMessageResetSpeedPid; ////Event d'interprétation d'une globalWorldMap à sa réception dans le robot robotUdpMulticastInterpreter.OnRefBoxMessageEvent += strategyManager.OnRefBoxMsgReceived; /// Event de Transmission des Local World Map du robot vers le multicast /// Disparaitra quand on voudra jouer sans liaison multicast localWorldMapManager.OnMulticastSendLocalWorldMapEvent += robotUdpMulticastSender.OnMulticastMessageToSendReceived; /// Events de réception des localWorldMap /// Soit en direct si on se transmet à nous même, soit via le Multicast pour transmettre aux autres robots localWorldMapManager.OnLocalWorldMapToGlobalWorldMapGeneratorEvent += globalWorldMapManager.OnLocalWorldMapReceived; robotUdpMulticastInterpreter.OnLocalWorldMapEvent += globalWorldMapManager.OnLocalWorldMapReceived; /// Event généré lorsque la Global World Map a été calculée. /// Elle n'a pas vocation à être renvoyée à tous les robots puisqu'on la génère dans chaque robot en parallèle globalWorldMapManager.OnGlobalWorldMapEvent += strategyManager.OnGlobalWorldMapReceived; /// Event de Réception de data Multicast sur le robot robotUdpMulticastReceiver.OnDataReceivedEvent += robotUdpMulticastInterpreter.OnMulticastDataReceived; /// LOGGER related events perceptionManager.OnLidarRawDataEvent += logRecorder.OnRawLidarDataReceived; robotMsgProcessor.OnIMURawDataFromRobotGeneratedEvent += logRecorder.OnIMURawDataReceived; robotMsgProcessor.OnPolarOdometrySpeedFromRobotEvent += logRecorder.OnPolarSpeedDataReceived; //omniCamera.OpenCvMatImageEvent += logRecorder.OnOpenCVMatImageReceived; //strategyManagerDictionary.Add(robotId, strategyManager); trajectoryPlanner.InitRobotPosition(0, 0, 0); strategyManager.InitStrategy(robotId, teamId); while (!exitSystem) { Thread.Sleep(500); } }
[STAThread] //à ajouter au projet initial static void Main(string[] args) { SciChartSurface.SetRuntimeLicenseKey(@"<LicenseContract> <Customer>University of Toulon</Customer> <OrderId>EDUCATIONAL-USE-0109</OrderId> <LicenseCount>1</LicenseCount> <IsTrialLicense>false</IsTrialLicense> <SupportExpires>11/04/2019 00:00:00</SupportExpires> <ProductCode>SC-WPF-SDK-PRO-SITE</ProductCode> <KeyCode>lwABAQEAAABZVzOfQ0zVAQEAewBDdXN0b21lcj1Vbml2ZXJzaXR5IG9mICBUb3Vsb247T3JkZXJJZD1FRFVDQVRJT05BTC1VU0UtMDEwOTtTdWJzY3JpcHRpb25WYWxpZFRvPTA0LU5vdi0yMDE5O1Byb2R1Y3RDb2RlPVNDLVdQRi1TREstUFJPLVNJVEWDf0QgB8GnCQXI6yAqNM2njjnGbUt2KsujTDzeE+k69K1XYVF1s1x1Hb/i/E3GHaU=</KeyCode> </LicenseContract>"); switch (robotMode) { case RobotMode.Standard: usingLidar = true; usingLogging = false; usingLogReplay = false; break; case RobotMode.LidarAcquisition: usingLidar = true; usingLogging = true; usingLogReplay = false; break; case RobotMode.LidarReplay: usingLidar = false; usingLogging = false; usingLogReplay = true; break; case RobotMode.Nolidar: usingLidar = false; usingLogging = false; usingLogReplay = false; break; } ethernetTeamNetworkAdapter = new EthernetTeamNetworkAdapter(); serialPort1 = new ReliableSerialPort("COM1", 115200, Parity.None, 8, StopBits.One); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); robotMsgGenerator = new RobotMsgGenerator(); robotMsgProcessor = new RobotMsgProcessor(); physicalSimulator = new PhysicalSimulator.PhysicalSimulator(); int robotId = (int)TeamId.Team1 + (int)RobotId.Robot1; int teamId = (int)TeamId.Team1; physicalSimulator.RegisterRobot(robotId, 0, 0); robotPilot = new RobotPilot.RobotPilot(robotId); refBoxAdapter = new RefereeBoxAdapter.RefereeBoxAdapter(); trajectoryPlanner = new TrajectoryPlanner(robotId); waypointGenerator = new WaypointGenerator(robotId); strategyManager = new StrategyManager.StrategyManager(robotId); localWorldMapManager = new LocalWorldMapManager(robotId, teamId); lidarSimulator = new LidarSimulator.LidarSimulator(robotId); perceptionSimulator = new PerceptionSimulator(robotId); if (usingLidar) { lidar_OMD60M = new Lidar_OMD60M(robotId); lidarProcessor = new LidarProcessor.LidarProcessor(robotId); } xBoxManette = new XBoxController.XBoxController(robotId); if (!usingSimulatedCamera) { omniCamera = new BaslerCameraAdapter(); } else { omniCameraSimulator = new SimulatedCamera.SimulatedCamera(); } imageProcessingPositionFromOmniCamera = new ImageProcessingPositionFromOmniCamera(); //Démarrage des interface de visualisation StartInterfaces(); //Démarrage du logging if (usingLogging) { logRecorder = new LogRecorder.LogRecorder(); } if (usingLogReplay) { logReplay = new LogReplay.LogReplay(); lidarProcessor = new LidarProcessor.LidarProcessor(robotId); } //Liens entre modules strategyManager.OnDestinationEvent += waypointGenerator.OnDestinationReceived; strategyManager.OnHeatMapEvent += waypointGenerator.OnStrategyHeatMapReceived; waypointGenerator.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived; if (!usingXBoxController) { trajectoryPlanner.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed; robotPilot.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; } else { //Sur evenement xx -->> Action a effectuer xBoxManette.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed; xBoxManette.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; xBoxManette.OnPriseBalleEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToMotor; xBoxManette.OnMoveTirUpEvent += robotMsgGenerator.GenerateMessageMoveTirUp; xBoxManette.OnMoveTirDownEvent += robotMsgGenerator.GenerateMessageMoveTirDown; xBoxManette.OnTirEvent += robotMsgGenerator.GenerateMessageTir; } robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot; msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage; serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived; msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage; physicalSimulator.OnPhysicalRobotPositionEvent += trajectoryPlanner.OnPhysicalPositionReceived; physicalSimulator.OnPhysicicalObjectListLocationEvent += perceptionSimulator.OnPhysicalObjectListLocationReceived; physicalSimulator.OnPhysicalRobotPositionEvent += perceptionSimulator.OnPhysicalRobotPositionReceived; physicalSimulator.OnPhysicalBallPositionEvent += perceptionSimulator.OnPhysicalBallPositionReceived; perceptionSimulator.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived; //lidarSimulator.OnSimulatedLidarEvent += localWorldMapManager.OnRawLidarDataReceived; strategyManager.OnDestinationEvent += localWorldMapManager.OnDestinationReceived; waypointGenerator.OnWaypointEvent += localWorldMapManager.OnWaypointReceived; strategyManager.OnHeatMapEvent += localWorldMapManager.OnHeatMapReceived; //waypointGenerator.OnHeatMapEvent += localWorldMapManager.OnHeatMapReceived; if (usingLidar) { lidar_OMD60M.OnLidarEvent += lidarProcessor.OnRawLidarDataReceived; lidarProcessor.OnLidarProcessedEvent += localWorldMapManager.OnRawLidarDataReceived; } //Event de recording if (usingLogging) { lidar_OMD60M.OnLidarEvent += logRecorder.OnRawLidarDataReceived; } //Event de replay if (usingLogReplay) { logReplay.OnLidarEvent += lidarProcessor.OnRawLidarDataReceived; lidarProcessor.OnLidarProcessedEvent += localWorldMapManager.OnRawLidarDataReceived; lidarProcessor.OnLidarObjectProcessedEvent += localWorldMapManager.OnLidarObjectsReceived; } //Timer de stratégie timerStrategie = new HighFreqTimer(0.5); timerStrategie.Tick += TimerStrategie_Tick; timerStrategie.Start(); lock (ExitLock) { // Do whatever setup code you need here // once we are done wait Monitor.Wait(ExitLock); } }
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); } }
[STAThread] //à ajouter au projet initial static void Main(string[] args) { SciChartSurface.SetRuntimeLicenseKey(@"<LicenseContract> <Customer>Universite De Toulon</Customer> <OrderId>EDUCATIONAL-USE-0128</OrderId> <LicenseCount>1</LicenseCount> <IsTrialLicense>false</IsTrialLicense> <SupportExpires>02/17/2020 00:00:00</SupportExpires> <ProductCode>SC-WPF-2D-PRO-SITE</ProductCode> <KeyCode>lwAAAQEAAACS9FAFUqnVAXkAQ3VzdG9tZXI9VW5pdmVyc2l0ZSBEZSBUb3Vsb247T3JkZXJJZD1FRFVDQVRJT05BTC1VU0UtMDEyODtTdWJzY3JpcHRpb25WYWxpZFRvPTE3LUZlYi0yMDIwO1Byb2R1Y3RDb2RlPVNDLVdQRi0yRC1QUk8tU0lURYcbnXYui4rna7TqbkEmUz1V7oD1EwrO3FhU179M9GNhkL/nkD/SUjwJ/46hJZ31CQ==</KeyCode> </LicenseContract>"); //TODO : Créer un projet World... ethernetTeamNetworkAdapter = new EthernetTeamNetworkAdapter(); serialPort1 = new ReliableSerialPort("FTDI", 230400 /*115200*/, Parity.None, 8, StopBits.One); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); robotMsgGenerator = new RobotMsgGenerator(); robotMsgProcessor = new RobotMsgProcessor(); physicalSimulator = new PhysicalSimulator.PhysicalSimulator(); int robotId = (int)TeamId.Team1 + (int)RobotId.Robot1; int teamId = (int)TeamId.Team1; physicalSimulator.RegisterRobot(robotId, 0, 0); robotPilot = new RobotPilot.RobotPilot(robotId); refBoxAdapter = new RefereeBoxAdapter.RefereeBoxAdapter(); trajectoryPlanner = new TrajectoryPlanner(robotId); waypointGenerator = new WaypointGenerator(robotId); strategyManager = new StrategyManager.StrategyManager(robotId); localWorldMapManager = new LocalWorldMapManager(robotId, teamId); lidarSimulator = new LidarSimulator.LidarSimulator(robotId); perceptionSimulator = new PerceptionSimulator(robotId); if (usingLidar) { lidar_OMD60M = new Lidar_OMD60M(robotId); } xBoxManette = new XBoxController.XBoxController(robotId); if (!usingSimulatedCamera) { omniCamera = new BaslerCameraAdapter(); } else { omniCameraSimulator = new SimulatedCamera.SimulatedCamera(); } imageProcessingPositionFromOmniCamera = new ImageProcessingPositionFromOmniCamera(); StartInterfaces(); //Liens entre modules strategyManager.OnDestinationEvent += waypointGenerator.OnDestinationReceived; strategyManager.OnHeatMapEvent += waypointGenerator.OnStrategyHeatMapReceived; waypointGenerator.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived; if (!usingXBoxController) { trajectoryPlanner.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed; robotPilot.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; } else { //Sur evenement xx -->> Action a effectuer xBoxManette.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed; xBoxManette.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; xBoxManette.OnPriseBalleEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToMotor; xBoxManette.OnMoveTirUpEvent += robotMsgGenerator.GenerateMessageMoveTirUp; xBoxManette.OnMoveTirDownEvent += robotMsgGenerator.GenerateMessageMoveTirDown; xBoxManette.OnTirEvent += robotMsgGenerator.GenerateMessageTir; } robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot; msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage; serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived; msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage; physicalSimulator.OnPhysicalRobotPositionEvent += trajectoryPlanner.OnPhysicalPositionReceived; physicalSimulator.OnPhysicicalObjectListLocationEvent += perceptionSimulator.OnPhysicalObjectListLocationReceived; physicalSimulator.OnPhysicalRobotPositionEvent += perceptionSimulator.OnPhysicalRobotPositionReceived; physicalSimulator.OnPhysicalBallPositionEvent += perceptionSimulator.OnPhysicalBallPositionReceived; perceptionSimulator.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived; //lidarSimulator.OnSimulatedLidarEvent += localWorldMapManager.OnRawLidarDataReceived; strategyManager.OnDestinationEvent += localWorldMapManager.OnDestinationReceived; waypointGenerator.OnWaypointEvent += localWorldMapManager.OnWaypointReceived; strategyManager.OnHeatMapEvent += localWorldMapManager.OnHeatMapReceived; //waypointGenerator.OnHeatMapEvent += localWorldMapManager.OnHeatMapReceived; if (usingLidar) { lidar_OMD60M.OnLidarEvent += localWorldMapManager.OnRawLidarDataReceived; } //Timer de stratégie timerStrategie = new HighFreqTimer(0.5); timerStrategie.Tick += TimerStrategie_Tick; timerStrategie.Start(); lock (ExitLock) { // Do whatever setup code you need here // once we are done wait Monitor.Wait(ExitLock); } }
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); } }