static void dsConnectThread()
{
TcpListener dsListener = new TcpListener(FMSIp, 1750);
try
{
dsListener.Start();
} catch
{
Console.WriteLine();
Console.WriteLine("Something went wrong while attempting to start connecting driver stations. Please check the network configuration and try again.");
Console.WriteLine("Press any key to exit.");
Arena.estop = true;
Console.ReadKey(true);
System.Environment.Exit(0);
}
Console.WriteLine("Listening for driver stations on {0} on port {1}", FMSIp.ToString(), 1750);
while (Arena.currentGamePhase == GamePhase.PREMATCH)
{
TcpClient tcpClient = dsListener.AcceptTcpClient();
byte[] buffer = new byte[5];
tcpClient.GetStream().Read(buffer, 0, buffer.Length);
if (!(buffer[0] == 0 && buffer[1] == 3 && buffer[2] == 24))
{
tcpClient.Close();
Console.WriteLine("Bad connection");
continue;
}
int teamId_1 = (int)buffer[3] << 8;
int teamId_2 = buffer[4];
int teamId = teamId_1 | teamId_2;
int allianceStation = -1;
string ip = tcpClient.Client.RemoteEndPoint.ToString().Split(':')[0];
IPAddress dsIp = IPAddress.Parse(ip);
for (int i = 0; i < redAlliance.Length; i++)
{
if (redAlliance[i].TeamNumber == teamId)
{
allianceStation = (int)DriverStation.IntToStation((i + 1), true);
redAlliance[i].setDsConnection(dsIp, tcpClient);
break;
}
}
for (int i = 0; i < blueAlliance.Length && allianceStation == -1; i++)
{
if (blueAlliance[i].TeamNumber == teamId)
{
allianceStation = (int)DriverStation.IntToStation((i + 1), false);
blueAlliance[i].setDsConnection(dsIp, tcpClient);
break;
}
}
if (currentGamePhase != GamePhase.PREMATCH)
{
break;
}
if (allianceStation == -1)
{
Console.WriteLine("Driver Station from team {0} attempted to connect, but they are not in this match!", teamId);
tcpClient.Close();
continue;
}
Console.WriteLine("Team {0} has connected their driver station!", teamId);
//byte knowledge from Team 254's Cheesy Arena.
byte[] assignmentPacket = new byte[5];
assignmentPacket[0] = 0; //Size
assignmentPacket[1] = 3; //Size
assignmentPacket[2] = 25; //Type
assignmentPacket[3] = (byte)allianceStation; //allianceStation
assignmentPacket[4] = 0; //Station Status, I currently do not have checks for correct station, since there is no vlan.
tcpClient.GetStream().Write(assignmentPacket, 0, assignmentPacket.Length);
}
dsListener.Stop();
}
static void Main(string[] args)
{
//Welcome Message
Console.Clear();
Console.WriteLine("Welcome to the unoffical practice FMS Version {0}", version);
Console.WriteLine("Written by MoSadie for FRC Team 4450, the Olympia Robotics Federation.");
Console.WriteLine("Robots are dangerous. Please be safe.");
//Config Setup
if (!File.Exists(configPath))
{
using (StreamWriter stream = File.CreateText(configPath))
{
foreach (string defaultOption in defaultOptions)
{
stream.WriteLine(defaultOption);
}
}
}
//Config Message
Console.WriteLine();
Console.WriteLine("Configuration can be changed using the config.txt file in the same directory as this executable.");
Console.WriteLine("Current Configuration:");
using (StreamReader stream = File.OpenText(configPath))
{
string line;
while ((line = stream.ReadLine()) != null)
{
string key = line.Split(':')[0];
int number = int.Parse(line.Split(':')[1]);
options.Add(key, number);
Console.WriteLine("{0}: {1}", key, number);
}
}
// Team Selection
redAlliance = new DriverStation[options["RedAllianceCount"]];
blueAlliance = new DriverStation[options["BlueAllianceCount"]];
Console.WriteLine();
Console.WriteLine();
for (int i = 0; i < redAlliance.Length; i++)
{
Console.Write("Enter a team number for the Red " + (i + 1) + " driver station: ");
redAlliance[i] = new DriverStation(Console.ReadLine(), true, i + 1);
Console.WriteLine();
}
for (int i = 0; i < blueAlliance.Length; i++)
{
Console.Write("Enter a team number for the Blue " + (i + 1) + " driver station: ");
blueAlliance[i] = new DriverStation(Console.ReadLine(), false, i + 1);
Console.WriteLine();
}
//2018 Specific Game String Generation
Console.WriteLine();
generateGameString();
Console.WriteLine("Game String for Red: " + redGameString);
Console.WriteLine("Game String for Blue: " + blueGameString);
Console.WriteLine();
Console.WriteLine("Press Enter to start connecting Driver Stations.");
Console.ReadLine();
Console.WriteLine("Now waiting for driver stations to connect.");
ThreadStart dsConnectThreadRef = new ThreadStart(dsConnectThread);
Thread dsConnectThreadObj = new Thread(dsConnectThreadRef);
dsConnectThreadObj.Start();
Thread.Sleep(200);
if (estop)
{
return;
}
while (!readyForMatchStart())
{
currentGamePhase = GamePhase.PREMATCH;
Console.Clear();
Console.WriteLine("Field Status: PreMatch (Waiting on driver stations and robots to connect)");
Console.WriteLine();
Console.WriteLine("Current Team Statuses:");
foreach (DriverStation ds in redAlliance)
{
if (ds.TeamNumber != 0)
{
Console.WriteLine(ds.ToString());
}
}
foreach (DriverStation ds in blueAlliance)
{
if (ds.TeamNumber != 0)
{
Console.WriteLine(ds.ToString());
}
}
Thread.Sleep(500);
}
string teamsParticipatingString = "Teams Participating: ";
List <DriverStation> nonZeroTeams = new List <DriverStation>();
foreach (DriverStation ds in redAlliance)
{
if (ds.TeamNumber != 0)
{
nonZeroTeams.Add(ds);
}
}
foreach (DriverStation ds in blueAlliance)
{
if (ds.TeamNumber != 0)
{
nonZeroTeams.Add(ds);
}
}
for (int i = 0; i < nonZeroTeams.Count - 1; i++)
{
teamsParticipatingString += blueAlliance[i].TeamNumber + ", ";
}
teamsParticipatingString += " " + nonZeroTeams[nonZeroTeams.Count - 1].TeamNumber;
Console.Clear();
Console.WriteLine("Field Status: PreMatch (Ready)");
Console.WriteLine(teamsParticipatingString);
Console.WriteLine();
Console.WriteLine("Press Enter to start the match.");
Console.ReadLine();
foreach (DriverStation ds in redAlliance)
{
ds.sendGameStringPacket();
}
foreach (DriverStation ds in blueAlliance)
{
ds.sendGameStringPacket();
}
Console.Clear();
Console.WriteLine("Field Status: Countdown");
Console.WriteLine(teamsParticipatingString);
TimeLeftInPhase = options["CountdownTime"];
Console.WriteLine("Match Begins in " + TimeLeftInPhase + " seconds.");
Console.WriteLine();
while (TimeLeftInPhase > 0 && currentGamePhase == GamePhase.PREMATCH)
{
Console.WriteLine(TimeLeftInPhase);
Thread.Sleep(1000);
TimeLeftInPhase--;
}
ThreadStart estopThreadRef = new ThreadStart(eStopThread);
Thread estopThread = new Thread(estopThreadRef);
estopThread.Start();
currentGamePhase = GamePhase.AUTO;
TimeLeftInPhase = options["AutonomousTime"];
while (TimeLeftInPhase > 0 && currentGamePhase == GamePhase.AUTO && !estop)
{
Console.Clear();
Console.WriteLine("Field Status: Autonomous");
Console.WriteLine(teamsParticipatingString);
Console.WriteLine("{0} seconds remain in Autonomous.", TimeLeftInPhase);
Console.WriteLine();
Console.WriteLine("Press Enter to EStop the match.");
foreach (DriverStation ds in redAlliance)
{
if (ds.TeamNumber == 0)
{
continue;
}
if (!ds.estop)
{
Console.WriteLine("Press {0} to EStop team {1}", ds.estopKey, ds.TeamNumber);
}
else
{
Console.WriteLine("Team {0} has been estopped.", ds.TeamNumber);
}
}
foreach (DriverStation ds in blueAlliance)
{
if (ds.TeamNumber == 0)
{
continue;
}
if (!ds.estop)
{
Console.WriteLine("Press {0} to EStop team {1}", ds.estopKey, ds.TeamNumber);
}
else
{
Console.WriteLine("Team {0} has been estopped.", ds.TeamNumber);
}
}
Thread.Sleep(1000);
TimeLeftInPhase--;
}
currentGamePhase = GamePhase.PAUSE;
TimeLeftInPhase = options["PauseTime"];
while (TimeLeftInPhase > 0 && currentGamePhase == GamePhase.PAUSE && !estop)
{
Console.Clear();
Console.WriteLine("Field Status: Pause between Autonomous and Teloperated");
Console.WriteLine(teamsParticipatingString);
Console.WriteLine("{0} seconds remain in Pause.", TimeLeftInPhase);
Console.WriteLine();
Console.WriteLine("Press Enter to EStop the match.");
foreach (DriverStation ds in redAlliance)
{
if (ds.TeamNumber == 0)
{
continue;
}
if (!ds.estop)
{
Console.WriteLine("Press {0} to EStop team {1}", ds.estopKey, ds.TeamNumber);
}
else
{
Console.WriteLine("Team {0} has been estopped.", ds.TeamNumber);
}
}
foreach (DriverStation ds in blueAlliance)
{
if (ds.TeamNumber == 0)
{
continue;
}
if (!ds.estop)
{
Console.WriteLine("Press {0} to EStop team {1}", ds.estopKey, ds.TeamNumber);
}
else
{
Console.WriteLine("Team {0} has been estopped.", ds.TeamNumber);
}
}
Thread.Sleep(1000);
TimeLeftInPhase--;
}
currentGamePhase = GamePhase.TELEOP;
TimeLeftInPhase = options["TeleoperatedTime"];
while (TimeLeftInPhase > 0 && currentGamePhase == GamePhase.TELEOP && !estop)
{
Console.Clear();
Console.WriteLine("Field Status: Teleoperated");
Console.WriteLine(teamsParticipatingString);
Console.WriteLine("{0} seconds remain in Teleoperated.", TimeLeftInPhase);
Console.WriteLine("Game Strings: Red: {0} Blue: {1}", redGameString, blueGameString);
Console.WriteLine();
Console.WriteLine("Press Enter to EStop the match.");
foreach (DriverStation ds in redAlliance)
{
if (ds.TeamNumber == 0)
{
continue;
}
if (!ds.estop)
{
Console.WriteLine("Press {0} to EStop team {1}", ds.estopKey, ds.TeamNumber);
}
else
{
Console.WriteLine("Team {0} has been estopped.", ds.TeamNumber);
}
}
foreach (DriverStation ds in blueAlliance)
{
if (ds.TeamNumber == 0)
{
continue;
}
if (!ds.estop)
{
Console.WriteLine("Press {0} to EStop team {1}", ds.estopKey, ds.TeamNumber);
}
else
{
Console.WriteLine("Team {0} has been estopped.", ds.TeamNumber);
}
}
Thread.Sleep(1000);
TimeLeftInPhase--;
}
currentGamePhase = GamePhase.POSTMATCH;
Console.Clear();
Console.WriteLine("Field Status: Match Complete");
Console.WriteLine();
Console.WriteLine("Cleaning up.");
foreach (DriverStation ds in redAlliance)
{
ds.dispose();
}
foreach (DriverStation ds in blueAlliance)
{
ds.dispose();
}
Console.WriteLine("All done! Thank you for using the PFMS by MoSadie.");
Console.WriteLine("To start another match, just restart this program.");
Console.WriteLine();
Console.WriteLine("Press any key to exit...");
Console.ReadKey(true);
System.Environment.Exit(0);
}
