public void Run()
{
#region Wire up all Classes
positionComputer.Initialize(razorVenus, imuLed, gpsLed);
xbee.Initialize(positionComputer, firingSolution, deadLed, razorVenus);
firingSolution.Initialize(positionComputer, xbee, trigger, killLed, statusLed, razorVenus, gpsLed);
#endregion
#region System Ready Blinks
statusLed.On();
Thread.Sleep(300);
imuLed.On();
Thread.Sleep(300);
gpsLed.On();
Thread.Sleep(300);
deadLed.On();
Thread.Sleep(300);
killLed.On();
Thread.Sleep(3000); // LTN March 20, 2012: This is a long LED startup sequence, giving the imu time to fully initialize before the imu data is being used to affect imuLed. Otherwise you get the imuLed blinkyblinking as if it's north because while initializing it outputs a yaw angle of 0mrads.
statusLed.Off();
imuLed.Off();
gpsLed.Off();
deadLed.Off();
killLed.Off();
#endregion
string lineToPrint = "\n\rSystem Wired and Ready for Action\n\r\n\r";
razorVenus.TerminalPrintOut(lineToPrint);
//DLC subscription to test DLC output via serial debug
xbee.ReceivedImDeadDLC += new Radio.DLCReceivedDelegate(xbee_ReceivedDLCMe);
// Call Testing here, after all wiring has been completed.
Testing();
for (; ;)
{
positionComputer.Compute();
Thread.Sleep(100); // Main loop clock cycle Timer
// StatusLed Flashes to confirm Loop is still running.
if (statusLed.CurrentState == Led.State.Off)
{
statusLed.On();
}
else
{
statusLed.Off();
}
}
}
} // Assigning positionEnemyReceived
// Insert trigger event handler, that changes a trigger variable true or false on both edges of trigger.
void positionComputer_AttNavCreated(AttNav shitIn)
{
lock (meLocker)
{
positionMeReceived = shitIn;
} // Now this event handler, which is running on clocked cycle, has an uptodate positionMe
forDebugPrint.TerminalPrintOut("\n\rDOP_e2: " + positionMeReceived.PositionDOP_e2.ToString() + " Time: " + positionMeReceived.GPSTimeInWeek_csec.ToString() + " Y: " + positionMeReceived.Yaw_deg.ToString() + " P: " + positionMeReceived.Pitch_deg.ToString());
#region gpsLed Blink(rate DOP dependent
if (positionMeReceived.PositionDOP_e2 == 0)
{
gpsLed.BlinkyBlink();
}
else
{
if (positionMeReceived.PositionDOP_e2 > 0 && positionMeReceived.PositionDOP_e2 <= 6000) // positionDOP, Ideal = 1000, good = 2000-5000, moderate = 6000-8000is, poor = up by 20,000
{
gpsLed.Off();
}
else
{
if (positionMeReceived.PositionDOP_e2 > 6000 && positionMeReceived.PositionDOP_e2 <= 2000)
{
gpsLed.Blink((30000 + ((20000000 - positionMeReceived.PositionDOP_e2 * 1000000) * 212)) / 1000); // This algorithm creates a scale from 20,000 to 6,000 that maps those end points to 30 to 3000, such that a dop of 20,000 => 30msec blink, and a dop of 6000 => 3000msec blink. Everything is a huge number because it was all mulitplied by 1000 for integer operations. 0.212 is the ratio of 14000 to 2970 (which is the deltas on each end of the scales begin mapped to each other)
}
else
{
gpsLed.On();
}
}
}
#endregion
checkKillShot();
}
// Event Handler Method
void physicalButton_OnInterrupt(uint data1, uint data2, DateTime time)
{
if (controlledLed.CurrentState == Led.State.Blinking)
{
controlledLed.Off();
}
else
{
controlledLed.Blink();
}
}
