private bool checkButton(OmniPosition pos, string btn, int value)
{
if (!btn.Equals(""))
{
if (btn.Contains("Left"))
{
if (pos.ButtonsLeft.Equals(value))
{
return(true);
}
}
else if (btn.Contains("Right"))
{
if (pos.ButtonsRight.Equals(value))
{
return(true);
}
}
else
{
for (int i = 0; i < pos.ExtraButtons.Count(); i++)
{
if (pos.ExtraButtons[i] && i.Equals(value))
{
return(true);
}
}
}
}
return(false);
}
public void CheckIfFollowing(OmniPosition Position)
{
bool btnPressed = checkButton(Position, FollowingBoundBtn, FollowingBoundValue);
if (!btnPressed && FollowBtnPressed)
{
FollowBtnPressed = false;
IsFollowing = !IsFollowing;
}
FollowBtnPressed = btnPressed;
}
public bool CheckForFreeze(OmniPosition Position)
{
bool btnPressed = checkButton(Position, FreezeBoundBtn, FreezeBoundValue);
if (!btnPressed && FreezeBtnPressed)
{
FreezeBtnPressed = false;
return(true);
}
FreezeBtnPressed = btnPressed;
return(false);
}
public bool CheckForEmergencySwitch(OmniPosition Position)
{
bool btnPressed = checkButton(Position, EmergencySwitchBoundBtn, EmergencySwitchBoundValue);
if (!btnPressed && EmergBtnPressed)
{
EmergBtnPressed = false;
return(true);
}
EmergBtnPressed = btnPressed;
return(false);
}
public void SetOmniForce(OmniPosition Force)
{
if (HasOmnis)
{
LeftOmni.SetpointX = Force.LeftX;
LeftOmni.SetpointY = Force.LeftY;
LeftOmni.SetpointZ = Force.LeftZ;
RightOmni.SetpointX = Force.RightX;
RightOmni.SetpointY = Force.RightY;
RightOmni.SetpointZ = Force.RightZ;
}
}
public void OmniFollow(OmniPosition InControlPosition)
{
if (HasOmnis)
{
OmniPosition currentPosition = GetOmniPositions();
double forceLX = followingForceConstant * (InControlPosition.LeftX - currentPosition.LeftX);
if (forceLX > forceMax)
{
forceLX = forceMax;
}
double forceLY = followingForceConstant * (InControlPosition.LeftY - currentPosition.LeftY);
if (forceLY > forceMax)
{
forceLY = forceMax;
}
double forceLZ = followingForceConstant * (InControlPosition.LeftZ - currentPosition.LeftZ);
if (forceLZ > forceMax)
{
forceLZ = forceMax;
}
double forceRX = followingForceConstant * (InControlPosition.RightX - currentPosition.RightX);
if (forceRX > forceMax)
{
forceRX = forceMax;
}
double forceRY = followingForceConstant * (InControlPosition.RightY - currentPosition.RightY);
if (forceRY > forceMax)
{
forceRY = forceMax;
}
double forceRZ = followingForceConstant * (InControlPosition.RightZ - currentPosition.RightZ);
if (forceRZ > forceMax)
{
forceRZ = forceMax;
}
SetOmniForce(new OmniPosition(forceLX, forceLY, forceLZ, forceRX, forceRY, forceRZ));
}
}
public OmniPosition GetOmniPositions()
{
LeftOmni.Update();
RightOmni.Update();
OmniPosition currentPosition = new OmniPosition(LeftOmni, RightOmni);
//add any external buttons
if (extButtonsConnected)
{
if (externalButtons.Length == 0)
{
externalButtons = new bool[NumExternalButtons];
}
if (extButtonsPort.BytesToRead >= NumExternalButtons)
{
//externalButtons = new bool[NumExternalButtons];
int intReturnASCII = 0;
string returnMessage = "";
for (int i = NumExternalButtons; i > 0; i--)
{
intReturnASCII = extButtonsPort.ReadByte();
returnMessage = returnMessage + Convert.ToChar(intReturnASCII);
}
if (returnMessage != "")
{
//if (externalButtons == new bool[0])
for (int i = 0; i < NumExternalButtons; i++)
{
externalButtons[i] = Convert.ToBoolean(Char.GetNumericValue(returnMessage[i]));
}
}
extButtonsPort.DiscardInBuffer();
}
currentPosition.ExtraButtons = externalButtons;
}
return(currentPosition);
/*
* //Update External Buttons Label
* if (externalButtons != null)
* {
* int j = 1;
* foreach (bool b in externalButtons)
* {
* if (b)
* lbl_ExBtns.Text = "Ex. Buttons : " + j;
* j++;
* }
* }
*
*
*
*
* if (isInControl)
* inControlPosition = currentPosition.Add(positionOffset);
*
* if (currentPosition.ButtonsRight == 2 && !buttonPressed)
* {
* if (!isInControl)
* {
* buttonPressed = true;
* feedbackEnabled = !feedbackEnabled;
* }
* }
* //Check for emergency switch press
* if (isMaster && !isInControl)
* {
* if (EmergencySwitchBoundBtn.Contains("Left"))
* {
* if (currentPosition.ButtonsLeft == EmergencySwitchBoundValue)
* emergencySwitchControl();
* }
* else if (EmergencySwitchBoundBtn.Contains("Right"))
* {
* if (currentPosition.ButtonsRight == EmergencySwitchBoundValue)
* emergencySwitchControl();
* }
* else if (EmergencySwitchBoundBtn.Contains("Ex"))
* {
* if (currentPosition.ExtraButtons[EmergencySwitchBoundValue])
* emergencySwitchControl();
* }
* }
*
* if (cb_NoRobot.Checked)
* {
* //Check for R2 press (used for TelSurge Freeze)
* if (currentPosition.ButtonsRight == 2 && !buttonPressed)
* {
* buttonPressed = true;
* if (isInControl)
* {
* freezeCommandReceived();
* }
* if (!isInControl)
* {
* buttonPressed = true;
* feedbackEnabled = !feedbackEnabled;
* }
* }
* if (currentPosition.ButtonsRight == 1 && !buttonPressed)
* {
* //Check for R1 press (used for TelSurge Emergency Switch)
* emergencySwitchControl();
* buttonPressed = true;
* }
* }
* //reset buttonPressed (to ensure code runs once per press)
* if (currentPosition.ButtonsRight == 0 && buttonPressed)
* buttonPressed = false;
*
* if (isMaster || hasMasterData)
* {
* if (isFrozen)
* {
* OutputPosition = frozenPosition;
* //Don't want to freeze buttons
* OutputPosition.ButtonsLeft = currentPosition.ButtonsLeft;
* OutputPosition.ButtonsRight = currentPosition.ButtonsRight;
* OutputPosition.ExtraButtons = currentPosition.ExtraButtons;
* }
* else
* {
* OutputPosition = inControlPosition;
* }
* }
*
* tb_SendingLeft.Text = "X = " + OutputPosition.LeftX + Environment.NewLine + "Y = " + OutputPosition.LeftY + Environment.NewLine + "Z = " + OutputPosition.LeftZ;
* tb_SendingRight.Text = "X = " + OutputPosition.RightX + Environment.NewLine + "Y = " + OutputPosition.RightY + Environment.NewLine + "Z = " + OutputPosition.RightZ;
*/
}
