protected override int Step(RobotMode Mode, int StepIndex)
{
int NextStep = StepIndex;
Thread.Sleep(10);
if (StepIndex == 0)
{
InitCycle();
m_Robot1.RunOnce();
m_Robot2.RunOnce();
NextStep++;
}
;
if (StepIndex == 1) //Todo: synchronize Robots by waiting for Ready
{
if (m_Robot1.ReadyForCycle() && m_Robot2.ReadyForCycle())
{
NextStep = 0;
}
else if (m_Robot1.Aborted() || m_Robot2.Aborted())
{
Stop();
}
}
;
return(NextStep);
}
private void RunRobotsOnce(RobotMode Mode)
{
m_ModuleState.SetActiveMode(Mode);
StartRobotDispatcher();
m_RobotDispatcher.RunOnce();
//m_RobotDispatcher.DoAutomatic();
}
public SendData()
{
Joysticks = new JoystickData[6];
for (int i = 0; i < 6; i++)
{
Joysticks[i] = new JoystickData();
}
DigitalHdrs = new long[10];
for (int i = 0; i < 10; i++)
{
DigitalHdrs[i] = 0;
}
DigitalMxp = new OutputDigitalMxp[16];
for (int i = 0; i < 16; i++)
{
DigitalMxp[i] = new OutputDigitalMxp();
}
Encoders = new EncoderData[8];
for (int i = 0; i < 8; i++)
{
Encoders[i] = new EncoderData();
}
MatchInfo = new MatchInfo();
RobotMode = new RobotMode();
}
private Direction FindNewPath()
{
var surrounding = GetNeighbors(_currentLocation);
if (surrounding.Count(_ => _.Value is Wall) == 3) // dead end
{
return(surrounding.Where(_ => !(_.Value is Wall)).First().Key);
}
else if (surrounding.Count(_ => _.Value is Wall) < 2)
{
_mode = RobotMode.Automatic;
}
switch (_mode)
{
case RobotMode.Automatic:
var next = _previousDirection;
while (!(surrounding[next] is null))
{
next = TurnRight(next);
}
return(next);
case RobotMode.BackTracking:
return(GetOpposite(_pathUntilNow.Pop()));
default:
return(Direction.South);
}
}
/// <summary>
/// Set robot shooting mode
/// </summary>
/// <param name="mode"></param>
public void SetMode(RobotMode mode)
{
if (mode == RobotMode.Pistol)
{
bulletSpawner.Interval = 1f;
}
else if (mode == RobotMode.Auto)
{
bulletSpawner.Interval = 0.1f;
}
}
public void SetActiveMode(RobotMode Mode)
{
//??error if switching directly from automatic to manual
RobotMode OldMode = m_ActiveMode;
m_ActiveMode = Mode;
if (OldMode != m_ActiveMode)
{
OnUpdate();
}
;
}
/// <summary>
/// Sets the robot mode
/// </summary>
/// <param name="mode">The robot mode to set</param>
public static void SetRobotMode(RobotMode mode)
{
RobotMode prevState = RobotMode.Teleop;
if (SimData.DriverStation.ControlData.DsAttached)
{
prevState = RobotMode.Autonomous;
}
else if (SimData.DriverStation.ControlData.Test)
{
prevState = RobotMode.Test;
}
switch (mode)
{
case RobotMode.Autonomous:
if (prevState != RobotMode.Autonomous)
{
SetEnabledState(EnabledState.Disabled);
}
SimData.DriverStation.ControlData.Autonomous = true;
SimData.DriverStation.ControlData.Test = false;
break;
case RobotMode.Teleop:
if (prevState != RobotMode.Teleop)
{
SetEnabledState(EnabledState.Disabled);
}
SimData.DriverStation.ControlData.Autonomous = false;
SimData.DriverStation.ControlData.Test = false;
break;
case RobotMode.Test:
if (prevState != RobotMode.Test)
{
SetEnabledState(EnabledState.Disabled);
}
SimData.DriverStation.ControlData.Autonomous = false;
SimData.DriverStation.ControlData.Test = true;
break;
default:
SetEnabledState(EnabledState.Disabled);
SimData.DriverStation.ControlData.Autonomous = false;
SimData.DriverStation.ControlData.Test = false;
break;
}
}
public async Task <string> SetModeAsync(RobotMode mode)
{
HttpResponseMessage response;
if (mode == RobotMode.Freeze)
{
response = await _connection.SetFreezeMode();
Mode = RobotMode.Freeze;
}
else
{
response = await _connection.SetRelaxMode();
Mode = RobotMode.Relax;
}
return(response.StatusCode == HttpStatusCode.OK ? "OK" : "Robot does not respond");
}
/// <summary>
/// Hier drin muss der Arbeits-Ablauf implementiert werden
/// </summary>
/// <param name="StepIndex"></param>
/// <returns></returns>
protected virtual int Step(RobotMode Mode, int StepIndex)
{
int NextStep = 0;
//UpdateStatDisplay(string.Format("Tick {0:d}", StepIndex));
//UpdateModulDisplay();
if (StepIndex == 0)
{
InitCycle();
}
;
if (StepIndex == 6)
{
NextStep = 0;
}
else
{
NextStep = StepIndex + 1;
}
//?? auf Stepoverflow überwachen
return(NextStep);
}
public RobotState(int robotID, RobotMode mode, RobotSubMode subMode, RobotPose pose, RobotPose rpose,
bool poseWatchdogFail, bool plannerFail, bool inObstacle, bool wpInObstacle, bool isLockedOn,
RobotNeutralizeStatus neutralizeStatus, List<Vector2> waypoints, double angleWhenDone,
IPath path, IPath sparsePath, RobotTwoWheelCommand cmd, DateTime cmdTime, int waypointsCompleted)
{
RobotID = robotID;
Mode = mode;
SubMode = subMode;
Pose = pose.DeepCopy();
rPose = rpose.DeepCopy();
PoseWatchdogFail = poseWatchdogFail;
PlannerFail = plannerFail;
InObstacle = inObstacle;
WpInObstacle = wpInObstacle;
IsLockedOn = isLockedOn;
NeutralizeStatus = neutralizeStatus;
Waypoints = new List<Vector2>(waypoints);
AngleWhenDone = angleWhenDone;
TrackedPath = path.Clone();
SparsePath = sparsePath.Clone();
Cmd = new RobotTwoWheelCommand(cmd.velocity, cmd.turn);
CmdTime = new DateTime(cmdTime.Ticks);
WaypointsCompleted = waypointsCompleted;
}
public RobotState(int robotID, RobotMode mode, RobotSubMode subMode, RobotPose pose, RobotPose rpose,
bool poseWatchdogFail, bool plannerFail, bool inObstacle, bool wpInObstacle, bool isLockedOn,
RobotNeutralizeStatus neutralizeStatus, List <Vector2> waypoints, double angleWhenDone,
IPath path, IPath sparsePath, RobotTwoWheelCommand cmd, DateTime cmdTime, int waypointsCompleted)
{
RobotID = robotID;
Mode = mode;
SubMode = subMode;
Pose = pose.DeepCopy();
rPose = rpose.DeepCopy();
PoseWatchdogFail = poseWatchdogFail;
PlannerFail = plannerFail;
InObstacle = inObstacle;
WpInObstacle = wpInObstacle;
IsLockedOn = isLockedOn;
NeutralizeStatus = neutralizeStatus;
Waypoints = new List <Vector2>(waypoints);
AngleWhenDone = angleWhenDone;
TrackedPath = path.Clone();
SparsePath = sparsePath.Clone();
Cmd = new RobotTwoWheelCommand(cmd.velocity, cmd.turn);
CmdTime = new DateTime(cmdTime.Ticks);
WaypointsCompleted = waypointsCompleted;
}
/// <summary>
/// Sets the robot mode
/// </summary>
/// <param name="mode">The robot mode to set</param>
public static void SetRobotMode(RobotMode mode)
{
RobotMode prevState = RobotMode.Teleop;
if (HalData["control"]["autonomous"])
{
prevState = RobotMode.Autonomous;
}
else if (HalData["control"]["test"])
{
prevState = RobotMode.Test;
}
switch (mode)
{
case RobotMode.Autonomous:
if (prevState != RobotMode.Autonomous)
SetEnabledState(EnabledState.Disabled);
HalDSData["control"]["autonomous"] = true;
HalDSData["control"]["test"] = false;
break;
case RobotMode.Teleop:
if (prevState != RobotMode.Teleop)
SetEnabledState(EnabledState.Disabled);
HalDSData["control"]["autonomous"] = false;
HalDSData["control"]["test"] = false;
break;
case RobotMode.Test:
if (prevState != RobotMode.Test)
SetEnabledState(EnabledState.Disabled);
HalDSData["control"]["autonomous"] = false;
HalDSData["control"]["test"] = true;
break;
default:
SetEnabledState(EnabledState.Disabled);
HalDSData["control"]["autonomous"] = false;
HalDSData["control"]["test"] = false;
break;
}
}
public void SetRobotMode(System.Guid newOwnerId, RobotMode mode)
{
robotMode = mode;
robotModeStartTime = Time.time;
ownerId = newOwnerId;
}
/// <summary>
/// Reports the mode change.
/// </summary>
/// <param name="mode">The mode.</param>
public void ReportModeChange(RobotMode mode)
{
if (SmartData.RobotMode != mode)
SmartData.RobotMode = mode;
}
/// <summary>
/// Sets the robot mode
/// </summary>
/// <param name="mode">The robot mode to set</param>
public static void SetRobotMode(RobotMode mode)
{
RobotMode prevState = RobotMode.Teleop;
if (SimData.DriverStation.ControlData.DsAttached)
{
prevState = RobotMode.Autonomous;
}
else if (SimData.DriverStation.ControlData.Test)
{
prevState = RobotMode.Test;
}
switch (mode)
{
case RobotMode.Autonomous:
if (prevState != RobotMode.Autonomous)
SetEnabledState(EnabledState.Disabled);
SimData.DriverStation.ControlData.Autonomous = true;
SimData.DriverStation.ControlData.Test = false;
break;
case RobotMode.Teleop:
if (prevState != RobotMode.Teleop)
SetEnabledState(EnabledState.Disabled);
SimData.DriverStation.ControlData.Autonomous = false;
SimData.DriverStation.ControlData.Test = false;
break;
case RobotMode.Test:
if (prevState != RobotMode.Test)
SetEnabledState(EnabledState.Disabled);
SimData.DriverStation.ControlData.Autonomous = false;
SimData.DriverStation.ControlData.Test = true;
break;
default:
SetEnabledState(EnabledState.Disabled);
SimData.DriverStation.ControlData.Autonomous = false;
SimData.DriverStation.ControlData.Test = false;
break;
}
}
protected override int Step(RobotMode Mode, int StepIndex)
{
m_StepStart = System.Environment.TickCount;
Thread.Sleep(10);
int NextStep = StepIndex;
string UserInfo = "";
int _i = -1;
Thread.Sleep(5);
((DemoModuleDispl)this.m_Interface.GetModuleDisplay()).DrawState(GetEAState());
((DemoModuleDispl)this.m_Interface.GetModuleDisplay()).SetMessage(GetEAState().ToString() + " ->" + m_NextState.ToString());
///////////////////////////////////////////////////////////////////////////////////////
//Grundstellungsfahrt
if (Mode == RobotMode.Homing)
{
#region Schrittkette Homing
if (StepIndex == ++_i)
{
InitCycle();
m_StepTimer = 0;
m_StepTimeout = 0;
NextStep = StepIndex + 1;
}
else if (StepIndex == ++_i)
{
if (GetEAState() == State.Unknown)
{
UserInfo = "Bereit für Einschalten";
m_NextState = State.PowerOK;
SetEAState(m_NextState);
}
if (IsInState(State.PowerOK))// && m_NextState == State.PowerOK) {
{
UserInfo = "Greifer öffnen";
m_StepTimer = 0;
m_StepTimeout = 1000;
m_NextState = State.GripperOpen;
SetEAState(m_NextState);
}
if (IsInState(State.GripperOpen))//&& m_NextState == State.GripperOpen) {
{
UserInfo = "deindexieren";
m_StepTimer = 0;
m_StepTimeout = 1000;
m_NextState = State.IndexHP;
SetEAState(m_NextState);
}
if (IsInState(State.IndexHP))// && m_NextState == State.IndexHP) {
{
UserInfo = "Schlitten in HP fahren";
m_StepTimer = 0;
m_StepTimeout = 1000;
m_NextState = State.SledgeOut;
SetEAState(m_NextState);
}
if (IsInState(State.SledgeOut))// && m_NextState == State.SledgeOut) {
{
m_NextState = State.HP;
m_StepTimer = 0;
m_StepTimeout = 1000;
SetEAState(m_NextState);
}
if (IsInState(State.HP))// && m_NextState == State.HP) {//out Station
{
NextStep = StepIndex + 1;
}
//Wenn wir in keinen stabilen Zustand sind schlägt irgendwann das Timeout an
if (m_StepTimeout > 0 && m_StepTimer > m_StepTimeout)
{
UserInfo = Translator.Tr("Timeout for settling State", "") + " " + m_NextState.ToString();
throw new SystemException(UserInfo);
}
}
else if (StepIndex == ++_i)
{
UserInfo = "Homing ok";
NextStep = 0;
}
#endregion
}
///////////////////////////////////////////////////////////////////////////////////////
//Automatik
if (Mode == RobotMode.Automatic)
{
#region Schrittkette Automatic
if (StepIndex == ++_i)
{
InitCycle();
NextStep = StepIndex + 1;
m_StepTimeout = 0;
}
else if (StepIndex == ++_i) //check power and HP
{
m_StepTimeout = 0;
if (!IsInState(State.HP))
{
UserInfo = "Grundstellungsfahr erforderlich";
}
else
{
m_NextState = State.HP;
NextStep = StepIndex + 1;
}
}
else if (StepIndex == ++_i) //wait for start from Station1
{
if (WTFileCollection.GetWTFile(0).GetProgram() > 0)
{
NextStep = StepIndex + 1;
}
}
else if (StepIndex == ++_i) //into the station
{
if (IsInState(m_NextState) && m_NextState == State.HP)
{
UserInfo = "in Grundstellung";
m_NextState = State.SledgeIn;
SetEAState(m_NextState);
m_StepTimer = 0;
m_StepTimeout = 1000;
}
else if (IsInState(m_NextState) && m_NextState == State.SledgeIn)
{
UserInfo = "Schlitten eingefahren";
m_NextState = State.IndexWP;
SetEAState(m_NextState);
m_StepTimer = 0;
m_StepTimeout = 1000;
}
else if (IsInState(m_NextState) && m_NextState == State.IndexWP) //in Station
{
UserInfo = "indexiert";
m_NextState = State.GripperClosed;
SetEAState(m_NextState);
m_StepTimer = 0;
m_StepTimeout = 1000;
}
else if (IsInState(m_NextState) && m_NextState == State.GripperClosed)
{
UserInfo = "Messung vorbereiten";
NextStep = StepIndex + 1;
}
}
else if (StepIndex == ++_i)
{
//check voltage
WTFileCollection.GetWTFile(0).GetResultDef().SetResultValue("AD1", ((double)(Randomize.Int(600))) / 100);
NextStep = StepIndex + 1;
}
else if (StepIndex == ++_i)
{
//check button
MeasureTorque();
NextStep = StepIndex + 1;
}
else if (StepIndex == ++_i) //out of station
{
if (IsInState(m_NextState) && m_NextState == State.GripperClosed)
{
UserInfo = "Messung Ende";
m_NextState = State.GripperOpen;
SetEAState(m_NextState);
m_StepTimer = 0;
m_StepTimeout = 1000;
}
else if (IsInState(m_NextState) && m_NextState == State.GripperOpen)
{
UserInfo = "Greifer offen";
m_NextState = State.IndexHP;
SetEAState(m_NextState);
m_StepTimer = 0;
m_StepTimeout = 1000;
}
else if (IsInState(m_NextState) && m_NextState == State.IndexHP)
{
UserInfo = "deindexiert";
m_NextState = State.SledgeOut;
SetEAState(m_NextState);
m_StepTimer = 0;
m_StepTimeout = 1000;
}
else if (IsInState(m_NextState) && m_NextState == State.SledgeOut)
{
UserInfo = "Schlitten ausgefahren";
m_NextState = State.HP;
SetEAState(m_NextState);
m_StepTimer = 0;
m_StepTimeout = 1000;
}
else if (IsInState(m_NextState) && m_NextState == State.HP) //out Station
{
NextStep = StepIndex + 1;
}
}
else if (StepIndex == ++_i)
{
m_ReportGenerator.AddTotalResult(WTFileCollection.GetWTFile(0));
switch (WTFileCollection.GetWTFile(0).GetResultDef().GetTotalResult())
{
case ResultDef.TestResultEnum.Pass:
UserInfo = Translator.Tr("final result OK", "");
m_Counter.Count(true);
break;
default:
UserInfo = Translator.Tr("final result NOK", "");
m_Counter.Count(false);
break;
}
UpdateResultDisplay(WTFileCollection.GetWTFile(0));
NextStep = 0;
}
//Wenn wir in keinen stabilen Zustand sind schlägt irgendwann das Timeout an
if (m_StepTimeout > 0 && m_StepTimer > m_StepTimeout)
{
UserInfo = Translator.Tr("Timeout for settling State", "") + " " + m_NextState.ToString();
throw new SystemException(UserInfo);
}
#endregion
}
if ((StepIndex != NextStep) || UserInfo.Length > 0)
{
UpdateStatDisplay(string.Format("Step {0:d} " + UserInfo, StepIndex));
}
m_StepTimer += System.Environment.TickCount - m_StepStart;
OnStepExit(); //aktuell wird nur vom Simulator ausgewertet
return(NextStep);
}
protected override int Step(RobotMode Mode, int StepIndex)
{
Thread.Sleep(10);
int NextStep = StepIndex;
string UserInfo = "";
int _i = -1;
bool _ShowResult = false;
if (StepIndex == ++_i)
{
InitCycle();
NextStep++;
}
else if (StepIndex == ++_i)
{
UserInfo = "Teil einlegen und START drücken";
if (m_BK.Ch().DI_PartInserted&& m_BK.Ch().DI_Start)
{
WTFileCollection.GetWTFile(0).SetProgram(1); //wird als Startflag für Robot2 verwendet
WTFileCollection.GetWTFile(0).SetSN(Randomize.Int(10000).ToString());
NextStep++;
}
else
{
WTFileCollection.GetWTFile(0).SetProgram(0);
}
}
else if (StepIndex == ++_i)
{
UserInfo = "Teil einschleusen " + GetParams().m_CustTypeNumber;
if (m_BK.Ch().DI_SledgeInWP)
{
NextStep++;
}
}
else if (StepIndex == ++_i)
{
UserInfo = "Teil prüfen";
if (m_BK.Ch().DI_SledgeInHP)
{
NextStep++;
}
}
else if (StepIndex == ++_i)
{
_ShowResult = true;
UserInfo = "Teil entnehmen";
if (!m_BK.Ch().DI_PartInserted)
{
WTFileCollection.GetWTFile(0).SetProgram(0);
NextStep = 0;
}
}
if (!_ShowResult && ((StepIndex != NextStep) || UserInfo.Length > 0))
{
UpdateStatDisplay(string.Format("Step {0:d} " + UserInfo, StepIndex));
}
if (_ShowResult && ((StepIndex != NextStep) || UserInfo.Length > 0))
{
UpdateStatDisplay(WTFileCollection.GetWTFile(0).GetResultDef().GetTotalResult(), UserInfo);
}
return(NextStep);
}
