public MainWindow()
{
InitializeComponent();
serialPort1 = new ReliableSerialPort("COM9", 115200, System.IO.Ports.Parity.None, 8, System.IO.Ports.StopBits.One);
serialPort1.Open();
serialPort1.DataReceived += SerialPort1_DataReceived;
timerAffichage = new System.Windows.Threading.DispatcherTimer();
timerAffichage.Tick += TimerAffichage_Tick;
timerAffichage.Interval = new TimeSpan(0, 0, 0, 0, 100);
timerAffichage.Start();
m_KeyboardHookManager = new KeyboardHookListener(new GlobalHooker());
m_KeyboardHookManager.Enabled = true;
m_KeyboardHookManager.KeyDown += HookManager_KeyDown;
WorldMap.Init(3.6, 2.4, 3, 2, "");
var robotShape = new WpfSimplifiedWorldMapDisplayNS.PolygonExtended();
robotShape.polygon.Points.Add(new Point(0.2, 0));
robotShape.polygon.Points.Add(new Point(0.1, 0.1));
robotShape.polygon.Points.Add(new Point(-0.1, 0.1));
robotShape.polygon.Points.Add(new Point(-0.1, -0.1));
robotShape.polygon.Points.Add(new Point(0.1, -0.1));
robotShape.polygon.Points.Add(new Point(0.2, 0));
WorldMap.SetRobotShape(robotShape);
WorldMap.UpdateRobotLocation(new WpfSimplifiedWorldMapDisplayNS.Location(robot.xpos, robot.ypos, robot.angle, 0, 0, 0));
//AsservDisplay.UpdatePolarSpeedConsigneValues(consigneX, consigneY, consigneTheta);
}
public void ConnectToSerialAvailable(string COM)
{
serialPort = new ReliableSerialPort(COM, 115200, Parity.None, 8, StopBits.One);
serialPort.OnDataReceivedEvent += SerialPort_OnDataReceived;
serialPort.Open();
OnSerialConnected(COM);
}
public MainWindow()
{
InitializeComponent();
serialPort1 = new ReliableSerialPort("COM4", 115200, System.IO.Ports.Parity.None, 8, System.IO.Ports.StopBits.One);
serialPort1.DataReceived += SerialPort1_DataReceived1;
serialPort1.Open();
timerAffichage = new DispatcherTimer();
timerAffichage.Interval = new TimeSpan(0, 0, 0, 0, 100);
timerAffichage.Tick += TimerAffichage_Tick;
timerAffichage.Start();
}
public MainWindow()
{
InitializeComponent();
serialPort1 = new ReliableSerialPort("COM5", 115200, Parity.None, 8, StopBits.One);
serialPort1.DataReceived += SerialPort1_DataReceived;
serialPort1.Open();
timerAffichage = new DispatcherTimer();
timerAffichage.Interval = new TimeSpan(0, 0, 0, 0, 100);
timerAffichage.Tick += TimerAffichage_Tick;
timerAffichage.Start();
m_KeyboardHookManager = new KeyboardHookListener(new GlobalHooker());
m_KeyboardHookManager.Enabled = true;
m_KeyboardHookManager.KeyDown += HookManager_KeyDown;
}
public MainWindow()
{
InitializeComponent();
/*----------------------------Init du port série + évenement datareceived------------------------------------*/
serialPort1 = new ReliableSerialPort("COM8", 115200, Parity.None, 8, StopBits.One); //entrée USB à droite
serialPort1.DataReceived += SerialPort1_DataReceived;
serialPort1.Open();
/*-----------------------------------------------------------------------------------------------------------*/
/*-----------------------------------Init Timer-------------------------*/
timerAffichage = new DispatcherTimer();
timerAffichage.Interval = new TimeSpan(0, 0, 0, 0, 100);
timerAffichage.Tick += TimerAffichage_Tick;
timerAffichage.Start();
} /*-----------------------------------------------------------------------*/
private void buttonSerial_Click(object sender, RoutedEventArgs e)
{
string select = comboSerial.Text;
if (select != "Null" && comboSerial.SelectedItem != null)
{
if (serialPort != null)
{
// Can't close the serial
//serialPort.Close();
}
else
{
serialPort = new ReliableSerialPort(select, 115200, Parity.None, 8, StopBits.One);
serialPort.DataReceived += SerialPort_DataReceived;
serialPort.Open();
}
}
}
public MainWindow()
{
InitializeComponent();
robot = new Robot();
serialPort = new ReliableSerialPort("COM19", 115200, Parity.None, 8, StopBits.One);
serialPort.DataReceived += SerialPort_DataReceived;
serialPort.Open();
timer = new DispatcherTimer();
timer.Interval = new TimeSpan(0, 0, 0, 0, 100);
timer.Tick += Timer_Tick;
timer.Start();
m_KeyboardHookManager = new KeyboardHookListener(new GlobalHooker());
m_KeyboardHookManager.Enabled = true;
m_KeyboardHookManager.KeyDown += M_KeyboardHookManager_KeyDown;
m_KeyboardHookManager.KeyUp += M_KeyboardHookManager_KeyUp;
}
private void Init()
{
_status = STATUS.Searching;
if (_stream != null)
{
// TODO: Remove write to logfile!
Console.WriteLine("Write to file");
_bw = new BinaryWriter(_stream);
}
if (_settingsSerial == null)
{
_settingsSerial = new SettingsSerial();
}
_serialPort = new ReliableSerialPort(_settingsSerial.PortName, _settingsSerial.BaudRate, _settingsSerial.Parity,
_settingsSerial.DataBits);
_serialPort.Open();
_stream = _serialPort.BaseStream;
}
[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 void comboBox_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
if (isPortSelected == false)
{
if (serialPort1 == null && comboBox.SelectedItem != null && comboBox.Text != null)
{
serialPort1 = new ReliableSerialPort(comboBox.SelectedItem.ToString(), 115200, Parity.None, 8, StopBits.One); //entrée USB à droite
serialPort1.DataReceived += SerialPort1_DataReceived;
serialPort1.Open();
//On change la fonction de la comboBox
isPortSelected = true;
comboBox.Items.Clear();
comboBox.Items.Add("Text");
comboBox.Items.Add("Led");
comboBox.Items.Add("Speed");
comboBox.Items.Add("State");
comboBox.Items.Add("Single byte");
comboBox.SelectedItem = comboBox.Items[0];
AutomaticMode_Click(sender, e);
//UartEncodeAndSendMessage(0x0052, 1, new byte[] { (byte) 0});
}
}
else
{
}
/*----------------------------Init du port série + évenement datareceived------------------------------------*/
//serialPort1 = new ReliableSerialPort("COM7", 115200, Parity.None, 8, StopBits.One); //entrée USB à droite
//serialPort1.DataReceived += SerialPort1_DataReceived;
//serialPort1.Open();
/*-----------------------------------------------------------------------------------------------------------*/
}
public HerkulexController(string portName, int baudRate, Parity parity, int dataBits, StopBits stopBits)
{
serialPort = new ReliableSerialPort(portName, baudRate, parity, dataBits, stopBits);
decoder = new HerkulexDecoder();
DequeueThread = new Thread(new ThreadStart(DequeueFrames));
DequeueThread.Start();
serialPort.DataReceived += decoder.DecodePacket;
decoder.OnRamReadAckEvent += Decoder_OnRamReadAckEvent;
decoder.OnRamWriteAckEvent += Decoder_OnRamWriteAckEvent;
decoder.OnIjogAckEvent += Decoder_OnIjogAckEvent;
decoder.OnSjogAckEvent += Decoder_OnSjogAckEvent;
decoder.OnStatAckEvent += Decoder_OnStatAckEvent;
serialPort.Open();
Task TaskA = Task.Run(() =>
{
while (true)
{
Thread.Sleep(10);
}
});
//starts 500ms after instanciation
PollingTimer = new System.Threading.Timer((c) =>
{
PollingTimerThreadBlock.WaitOne();
foreach (var key in Servos.Keys)
{
RAM_READ(key, HerkulexDescription.RAM_ADDR.Absolute_Position, 2);
RamReadAckReceivedEvent.WaitOne(AckTimeout);
}
}, null, 500, PollingInterval);
}
public MainWindow()
{
InitializeComponent();
SerialStream = new ReliableSerialPort("COM6", 115200, System.IO.Ports.Parity.None, 8, System.IO.Ports.StopBits.One);
FrameDecoder = new msgDecoder();
FrameProcessor = new msgProcessor();
UI_Updater = new DispatcherTimer();
UI_Updater.Interval = new TimeSpan(0, 0, 0, 0, 100); //100ms tick
RobotModel = new robot();
//block logic
UI_Updater.Tick += UpdateUI;
SerialStream.DataReceived += FrameDecoder.DecodeMessage;
FrameDecoder.OnDataDecodedEvent += FrameProcessor.ProcessMessage;
FrameProcessor.OnTextMessageProcessedEvent += FrameProcessor_OnTextMessageProcessedEvent;
FrameProcessor.OnCheckSumErrorOccuredEvent += FrameProcessor_OnCheckSumErrorOccuredEvent;
FrameProcessor.OnIrMessageProcessedEvent += FrameProcessor_OnIrMessageProcessedEvent;
FrameProcessor.OnSpeedMessageProcessedEvent += FrameProcessor_OnSpeedMessageProcessedEvent;
SerialStream.Open();
UI_Updater.Start();
}
private void TimerAffichage_Tick(object sender, EventArgs e)
{
//caca
//using (System.IO.StreamWriter lf = new System.IO.StreamWriter(logPath, true))
//{
// lf.WriteLine(TelemetreEG + " " + TelemetreG + " " + TelemetreC + " " + TelemetreD + " " + TelemetreD + " - " + robotState + " " + stateChanged + " @" + timestamp);
//}
if (autoPortSetting == 1 && isPortSelected == false)
{
string[] menuPortContentAS = ListAllSerialOpen();
serialPort1 = new ReliableSerialPort(menuPortContentAS[menuPortContentAS.Length - 1], 115200, Parity.None, 8, StopBits.One); //entrée USB à droite
serialPort1.DataReceived += SerialPort1_DataReceived;
serialPort1.Open();
isPortSelected = true;
stateMachineActivated = true;
UartEncodeAndSendMessage(0x0020, 2, new byte[] { 1, 1 });
}
if ((stateRobot)(robotState) != (stateRobot)(stateChanged) &&
(stateRobot)(robotState) != (stateRobot)(stateChanged) + 1 &&
stateMachineActivated)
{
UartEncodeAndSendMessage(0x0051, 1, new byte[] { (byte)stateChanged });
}
while (Messages.Count > 0)
{
Message CurMsg = Messages.Dequeue();
byte[] tab = new byte[4];
switch (CurMsg.msgDecodedFunction)
{
case 0x0080: // transmission texte
textBoxReception.Text += "[Text (";
textBoxReception.Text += CurMsg.msgDecodedPayloadLength;
textBoxReception.Text += ")";
if (CurMsg.isValidChecksum == true)
{
textBoxReception.Text += "] ";
}
else
{
textBoxReception.Text += ", invalid checksum !] ";
}
//textBoxReception.AppendText(Encoding.ASCII.GetString(CurMsg.msgDecodedPayload));
for (int i = 0; i < CurMsg.msgDecodedPayloadLength; i++)
{
textBoxReception.Text += (char)CurMsg.msgDecodedPayload[i];
}
textBoxReception.Text += "\n";
break;
case 0x0020: // reglage LED
textBoxReception.Text += "[LED";
if (CurMsg.isValidChecksum == false)
{
textBoxReception.Text += " invalid checksum !] ";
}
else
{
textBoxReception.Text += "] ";
}
textBoxReception.Text += "LED ";
textBoxReception.Text += CurMsg.msgDecodedPayload[0];
if (CurMsg.msgDecodedPayload[1] == 1)
{
textBoxReception.Text += " ON";
}
else
{
textBoxReception.Text += " OFF";
}
textBoxReception.Text += "\n";
break;
case 0x0030: // distance télémètre IR
if (CurMsg.isValidChecksum == true)
{
TelemetreG = CurMsg.msgDecodedPayload[0];
TelemetreC = CurMsg.msgDecodedPayload[1];
TelemetreD = CurMsg.msgDecodedPayload[2];
if (CurMsg.msgDecodedPayloadLength == 5)
{
TelemetreEG = CurMsg.msgDecodedPayload[3];
TelemetreED = CurMsg.msgDecodedPayload[4];
}
}
else
{
textBoxReception.Text += "[IR invalid checksum]\n";
}
break;
case 0x0040: // Consigne vitesse
textBoxReception.Text += "[CONSIGNE";
if (CurMsg.isValidChecksum == true)
{
textBoxReception.Text += "] ";
}
else
{
textBoxReception.Text += ", invalid checksum !] ";
}
textBoxReception.Text += "G=";
textBoxReception.Text += (float)CurMsg.msgDecodedPayload[0] * 100.0 / 255.0;
textBoxReception.Text += "% D=";
textBoxReception.Text += (float)CurMsg.msgDecodedPayload[1] * 100.0 / 255.0;
textBoxReception.Text += "%\n";
break;
case 0x0050: // Etape en cours
receptionTimeState = (((int)CurMsg.msgDecodedPayload[1]) << 24) +
(((int)CurMsg.msgDecodedPayload[2]) << 16) +
(((int)CurMsg.msgDecodedPayload[3]) << 8) +
(((int)CurMsg.msgDecodedPayload[4]) << 0);
receptionTimeState *= 5;
robotState = CurMsg.msgDecodedPayload[0];
break;
case 0x0061: // odométrie
// timestamp
tab = CurMsg.msgDecodedPayload.GetRange(0, 4);
receptionTimeConsigne = tab[0] << 24;
receptionTimeConsigne += tab[0] << 16;
receptionTimeConsigne += tab[0] << 8;
receptionTimeConsigne += tab[0] << 0;
// odo 1
tab = CurMsg.msgDecodedPayload.GetRange(4, 4);
//positionXOdo = BitConverter.ToInt32(tab, 0);
positionXOdo = tab.GetFloat();
// odo 2
tab = CurMsg.msgDecodedPayload.GetRange(8, 4);
positionYOdo = tab.GetFloat();
// angle
tab = CurMsg.msgDecodedPayload.GetRange(12, 4);
angleOdo = tab.GetFloat();
// linear speed
tab = CurMsg.msgDecodedPayload.GetRange(16, 4);
linearSpeedOdo = tab.GetFloat();
// angular speed
tab = CurMsg.msgDecodedPayload.GetRange(20, 4);
angularSpeedOdo = tab.GetFloat();
break;
case 0x0062: // Consigne asserv
tab = new byte[4];
// timestamp
tab = CurMsg.msgDecodedPayload.GetRange(0, 4);
receptionTimeOdo = tab[0] << 24;
receptionTimeOdo += tab[0] << 16;
receptionTimeOdo += tab[0] << 8;
receptionTimeOdo += tab[0] << 0;
// Consigne Linéaire
tab = CurMsg.msgDecodedPayload.GetRange(4, 4);
LineaireConsigne = tab.GetFloat();
// Consigne Angulaire
tab = CurMsg.msgDecodedPayload.GetRange(8, 4);
AngulaireConsigne = tab.GetFloat();
// Consigne Gauche
tab = CurMsg.msgDecodedPayload.GetRange(12, 4);
GaucheConsigne = tab.GetFloat();
// Consigne Droite
tab = CurMsg.msgDecodedPayload.GetRange(16, 4);
DroiteConsigne = tab.GetFloat();
break;
}
}
//textBoxReception.Text += "0x" + byteListReceived.Dequeue().ToString("X2") + " "
//if (receivedtext != receivedtextAnt)
//{
// textBoxReception.Text = receivedtext;
// receivedtextAnt = receivedtext;
//}
}
public MainWindow()
{
serialPort1 = new ReliableSerialPort("COM5", 115200, Parity.None, 8, StopBits.One);
serialPort1.Open();
serialPort1.DataReceived += SerialPort1_DataReceived1;
}
[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);
}
}
[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);
}
}
