/// <summary>
/// Creates a new <see cref="ADXL362"/> with the specified <see cref="SPI.Port">Port</see>
/// and <see cref="AccelerometerRange">Range</see>.
/// </summary>
/// <param name="port">The SPI Port the accelerometer is connected to.</param>
/// <param name="range">The range that the accelerometer will measure.</param>
public ADXL362(SPI.Port port, AccelerometerRange range)
{
m_spi = new SPI(port);
m_spi.SetClockRate(3000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnRising();
m_spi.SetClockActiveHigh();
m_spi.SetChipSelectActiveLow();
byte[] commands = new byte[]
{
RegRead,
PartIdRegister,
0,
};
m_spi.Transaction(commands, commands, 3);
if (commands[2] != 0xF2)
{
DriverStation.ReportError("Could not find ADXL362", false);
m_gsPerLSB = 0.0;
return;
}
AccelerometerRange = range;
commands[0] = RegWrite;
commands[1] = PowerCtlRegister;
commands[2] = PowerCtlRegister | PowerCtl_UltraLowNoise;
m_spi.Write(commands, 3);
Report(ResourceType.kResourceType_ADXL362, (byte)port);
LiveWindow.LiveWindow.AddSensor("ADXL362", port.ToString(), this);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="port">The <see cref="SPI.Port"/> that the gyro is connected to.</param>
public ADXRS450_Gyro(SPI.Port port)
{
m_spi = new SPI(port);
if (RobotBase.IsSimulation)
{
m_spi.InitAccumulator(SamplePeriod, 0x20000000, 4, 0x0c000000, 0x04000000,
10, 16, true, true);
m_spi.ResetAccumulator();
return;
}
m_spi.SetClockRate(3000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnRising();
m_spi.SetClockActiveHigh();
m_spi.SetChipSelectActiveLow();
// Validate the part ID
if ((ReadRegister(PIDRegister) & 0xff00) != 0x5200)
{
m_spi.Dispose();
m_spi = null;
DriverStation.ReportError("could not find ADXRS450 gyro on SPI port " + port.ToString(), false);
return;
}
m_spi.InitAccumulator(SamplePeriod, 0x20000000, 4, 0x0c000000, 0x04000000,
10, 16, true, true);
Calibrate();
Report(ResourceType.kResourceType_ADXRS450, (byte)port);
LiveWindow.LiveWindow.AddSensor("ADXRS450_Gyro", port.ToString(), this);
}
protected Joystick(int port, int numAxisTypes, int numButtonTypes)
{
m_ds = DriverStation.Instance;
m_axes = new byte[numAxisTypes];
m_buttons = new byte[numButtonTypes];
m_port = port;
}
/// <summary>
/// Creates a new RobotBase. When this is called, it initializes NetworkTables and the
/// <see cref="DriverStation"/>
/// </summary>
protected RobotBase()
{
NetworkTable.SetServerMode();
m_ds = DriverStation.Instance;
NetworkTable.GetTable("");
NetworkTable.GetTable("LiveWindow").GetSubTable("~STATUS~").PutBoolean("LW Enabled", false);
}
/// <summary>
/// Creates a new RobotBase. When this is called, it initializes NetworkTables and the
/// <see cref="DriverStation"/>
/// </summary>
protected RobotBase()
{
NetworkTable.SetNetworkIdentity("Robot");
NetworkTable.SetPersistentFilename("/home/lvuser/networktables.ini");
NetworkTable.SetServerMode();
m_ds = DriverStation.Instance;
NetworkTable.GetTable("");
NetworkTable.GetTable("LiveWindow").GetSubTable("~STATUS~").PutBoolean("LW Enabled", false);
}
/// <summary>
/// Creates a new RobotBase. When this is called, it initializes NetworkTables and the
/// <see cref="DriverStation"/>
/// </summary>
protected RobotBase()
{
NetworkTable.SetNetworkIdentity("Robot");
NetworkTable.SetPersistentFilename("/home/lvuser/networktables.ini");
NetworkTable.SetServerMode();
m_ds = DriverStation.Instance;
NetworkTable.GetTable("");
NetworkTable.GetTable("LiveWindow").GetSubTable("~STATUS~").PutBoolean("LW Enabled", false);
}
internal static bool CheckCTRStatus(CTR_Code status, [CallerMemberName] string memberName = "",
[CallerFilePath] string filePath = "", [CallerLineNumber] int lineNumber = 0)
{
if (status != CTR_Code.CTR_OKAY)
{
//Pass the caller memebers along
DriverStation.ReportError(HAL_GetErrorMessage((int)status), true, (int)status, memberName, filePath, lineNumber);
}
return(status == CTR_Code.CTR_OKAY);
}
/// <summary>
/// Check if this motor has exceeded its timeout. </summary>
/// <remarks>This method is called periodically to determine if this motor has exceeded its timeout value.
/// If it has, the stop method is called, and the motor is shut down until its value is
/// updated again.
/// </remarks>
public void Check()
{
if (!SafetyEnabled || RobotState.Disabled || RobotState.Test)
{
return;
}
if (m_stopTime < GetFPGATimestamp())
{
DriverStation.ReportError($"{m_safeObject.Description}... Output not updated often enough", false);
m_safeObject.StopMotor();
}
}
public void Check()
{
bool enabled;
TimeSpan stopTime;
lock (m_thisMutex)
{
enabled = m_enabled;
stopTime = m_stopTime;
}
if (!enabled || RobotState.IsDisabled || RobotState.IsTest)
{
return;
}
if (stopTime < Timer.FPGATimestamp)
{
DriverStation.ReportError(Description + "... Output not updated often enough", false);
StopMotor();
}
}
public static void CheckStatus(int status, [CallerMemberName] string memberName = "",
[CallerFilePath] string filePath = "", [CallerLineNumber] int lineNumber = 0)
{
if (status == 0)
{
return;
}
if (status == HALErrors.NO_AVAILABLE_RESOURCES ||
status == HALErrors.RESOURCE_IS_ALLOCATED ||
status == HALErrors.RESOURCE_OUT_OF_RANGE)
{
throw new AllocationException($" Code : {status}. {HAL_GetErrorMessage(status)}");
}
else if (status < 0)
{
throw new UncleanStatusException(status, $" Code : {status}. {HAL_GetErrorMessage(status)} \n at {memberName} path:{filePath} line:{lineNumber}");
}
else if (status > 0)
{
//Pass the caller members along.
DriverStation.ReportError(HAL_GetErrorMessage(status), true, status, memberName, filePath, lineNumber);
}
}
/// <summary>
/// Starting point for robot applications. You can provide either an assembly, or a type.
/// If passed a type, it takes priority.
/// </summary>
/// <param name="robotAssembly">The assembly the main robot class is located in.</param>
/// <param name="robotType">The main robot class type</param>
public static void Main(Assembly robotAssembly, Type robotType = null)
{
try
{
HAL.Base.HAL.Initialize();
}
catch (Exception e)
{
Console.WriteLine("Static Robot Initialization Failed.");
Console.WriteLine(e);
Environment.Exit(1);
}
Report(ResourceType.kResourceType_Language, Instances.kLanguage_DotNet);
//Find the robot code class
try
{
//If we were passed an assembly but not a type
if (robotType == null && robotAssembly != null)
{
//Load the first non abstract class inheriting from RobotBase we can find.
var robotClasses =
(from t in robotAssembly.GetTypes()
where typeof(RobotBase).IsAssignableFrom(t) && !t.GetTypeInfo().IsAbstract&& !t.GetTypeInfo().IsInterface
select t)
.ToList();
if (robotClasses.Count == 0)
{
throw new Exception(
"Could not find base robot class. Are you sure the assembly got passed correctly to the main function?");
}
robotType = robotClasses[0];
s_robot = (RobotBase)Activator.CreateInstance(robotType);
}
else
{
//If not type or assembly throw an exception
if (robotType == null)
{
throw new Exception("Both robotAssembly and robotType cannot be null.");
}
//Otherwise just initialize the type we were passed.
s_robot = (RobotBase)Activator.CreateInstance(robotType);
}
}
catch (Exception ex)
{
string robotName = "";
if (robotType != null)
{
robotName = robotType.Name;
}
DriverStation.ReportError("ERROR Unhandled exception instantiating robot " + robotName + " " + ex + " at " + ex.StackTrace, false);
Console.Error.WriteLine("WARNING: Robots don't quit!");
Console.Error.WriteLine("Error: Could not instantiate robot " + robotName + "!");
Environment.Exit(1);
return;
}
//Write to the version file.
// TODO: FIX ME (THAD) Need to know if sim or not
if (true)
{
string file = "/tmp/frc_versions/FRC_Lib_Version.ini";
try
{
if (File.Exists(file))
{
File.Delete(file);
}
var version = typeof(RobotBase).GetTypeInfo().Assembly.GetName().Version;
File.WriteAllText(file, "RobotDotNet V" + version);
}
catch (IOException ex)
{
Console.WriteLine(ex.StackTrace);
}
}
bool errorOnExit = false;
try
{
Console.WriteLine("********** Robot program starting **********");
s_robot.StartCompetition();
}
catch (Exception ex)
{
DriverStation.ReportError("ERROR Unhandled exception" + ex, false);
errorOnExit = true;
}
finally
{
Console.Error.WriteLine("WARNING: Robots don't quit!");
if (errorOnExit)
{
Console.Error.WriteLine("---> The StartCompetition() method (or methods called by it) should have handled the exception above.");
}
else
{
Console.Error.WriteLine("---> Unexpected return fom the StartCompetition() method.");
}
DriverStation.ReportError("ERROR StartCompetition() returned", false);
Environment.Exit(1);
}
}
/// <summary>
/// Protected version of the constructor to be called by sub-classes.
/// </summary>
/// <remarks>
/// This constructor allows the subclass to configure the number of constants
/// for axes and buttons.
/// </remarks>
/// <param name="port">The port on the Driver Station the joystick is connected to [0..5]</param>
/// <param name="numAxisTypes">The number of axis types in the enum</param>
/// <param name="numButtonTypes">The number of button types in the enum</param>
public Joystick(int port, int numAxisTypes, int numButtonTypes) : base(port)
{
m_ds = DriverStation.Instance;
m_axes = new int[numAxisTypes];
m_buttons = new int[numButtonTypes];
}
private void PrintLoopOverrunMessage()
{
DriverStation.ReportWarning($"Loop time of {Period}s overrun\n", false);
}
private static void RunRobot <Robot>() where Robot : RobotBase, new()
{
Console.WriteLine("\n********** Robot program starting **********");
Robot robot;
try
{
robot = new Robot();
}
#pragma warning disable CA1031 // Do not catch general exception types
catch (Exception ex)
#pragma warning restore CA1031 // Do not catch general exception types
{
string robotName = typeof(Robot).Name;
DriverStation.ReportError(("Unhandled exception instanciating robot "
+ robotName + " " + ex.Message), ex.StackTrace !);
DriverStation.ReportWarning("Robots should not quit, but yours did!", false);
DriverStation.ReportError("Could not instanciate robot " + robotName + "!", false);
return;
}
lock (m_runMutex)
{
m_robotCopy = robot;
}
if (IsReal)
{
try
{
File.WriteAllText("/tmp/frc_versions/FRC_Lib_Version.ini", $"C# {WPILibVersion}");
}
catch (IOException ex)
{
DriverStation.ReportError("Could not write FRC_Lib_Version.ini: " + ex.Message, ex.StackTrace !);
}
}
bool errorOnExit = false;
try
{
robot.StartCompetition();
}
#pragma warning disable CA1031 // Do not catch general exception types
catch (Exception ex)
#pragma warning restore CA1031 // Do not catch general exception types
{
Console.WriteLine(ex);
DriverStation.ReportError("Unhandled Exception: " + ex.Message, ex.StackTrace !);
errorOnExit = true;
}
finally
{
bool suppressExitWarningLocal;
lock (m_runMutex)
{
suppressExitWarningLocal = suppressExitWarning;
}
if (!suppressExitWarningLocal)
{
DriverStation.ReportWarning("Robots should not quit, but yours did!", false);
if (errorOnExit)
{
DriverStation.ReportError("The StartCompetition() method (or methods called by it) should have handled the exception above.", false);
}
else
{
DriverStation.ReportError("Unexpected return from StartCompetition() method.", false);
}
}
}
}
