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; */ }