// ---------- ROBOT MAIN LOOP ---------- //
public void Run()
{
//Read incoming serial packets
NUC_SerialConnection.ReadFromNUC();
//Check for dashboard enable signal in order for the robot to be enabled
bool ROBOT_ENABLED = NUC_SerialConnection.IsRobotEnabled();
//Reset the robot control mode to disabled
controlModeHandler.SetMode(ControlModeHandler.ControlMode.DISABLED);
if (ROBOT_ENABLED)
{
//Heartbeat pulse to the motors to enable them
//Motor controllers will disable automatically if this is not regularly received
CTRE.Phoenix.Watchdog.Feed();
//Update the robot control mode and send over the logitech controller object to be updated
if (ROBOT_ENABLED)
{
controlModeHandler.SetMode(NUC_SerialConnection.GetControlMode());
controlModeHandler.updateControllerValues(ref logitechController, ref NUC_SerialConnection);
}
//MoveMotors
if (CAN_NETWORK_IS_ENABLED)
{
// Debug.Print("Fix BackRight shaft key DB");
driveBase.Update(ref logitechController, controlModeHandler.IsRobotActive());
excavation.Update(ref logitechController, controlModeHandler.IsRobotActive());
}
}
//Serial write to Dashboard with talon motor data
if (CAN_NETWORK_IS_ENABLED)
{
PackageTalonInfo();
}
}