public OmniPosition Subtract(OmniPosition pos) { return(new OmniPosition( LeftX - pos.LeftX, LeftY - pos.LeftY, LeftZ - pos.LeftZ, Gimbal1Left - pos.Gimbal1Left, Gimbal2Left - pos.Gimbal2Left, Gimbal3Left - pos.Gimbal3Left, ButtonsLeft, InkwellLeft, RightX - pos.RightX, RightY - pos.RightY, RightZ - pos.RightZ, Gimbal1Right - pos.Gimbal1Right, Gimbal2Right - pos.Gimbal2Right, Gimbal3Right - pos.Gimbal3Right, ButtonsRight, InkwellRight, ExtraButtons )); }
public OmniPosition Add(OmniPosition pos) { return(new OmniPosition( LeftX + pos.LeftX, LeftY + pos.LeftY, LeftZ + pos.LeftZ, Gimbal1Left + pos.Gimbal1Left, Gimbal2Left + pos.Gimbal2Left, Gimbal3Left + pos.Gimbal3Left, ButtonsLeft, InkwellLeft, RightX + pos.RightX, RightY + pos.RightY, RightZ + pos.RightZ, Gimbal1Right + pos.Gimbal1Right, Gimbal2Right + pos.Gimbal2Right, Gimbal3Right + pos.Gimbal3Right, ButtonsRight, InkwellRight, ExtraButtons )); }
private void showOmniPositions(OmniPosition currentPosition) { lbX1value.Text = "X : " + currentPosition.LeftX.ToString(); lbY1value.Text = "Y : " + currentPosition.LeftY.ToString(); lbZ1value.Text = "Z : " + currentPosition.LeftZ.ToString(); lbGimbal11.Text = "Gimbal 1 : " + currentPosition.Gimbal1Left.ToString(); lbGimbal21.Text = "Gimbal 2 : " + currentPosition.Gimbal2Left.ToString(); lbGimbal31.Text = "Gimbal 3 : " + currentPosition.Gimbal3Left.ToString(); lbButtons1.Text = "Buttons : " + currentPosition.ButtonsLeft; lbInk1.Text = "InkWell : " + currentPosition.InkwellLeft.ToString(); bool pedPress = false; for (int i = 0; i < User.NumExternalButtons; i++) { if (currentPosition.ExtraButtons[i]) { lbl_ExBtns.Text = "Ex. Buttons :" + (i + 1).ToString(); pedPress = true; break; } } if (!pedPress) lbl_ExBtns.Text = "Ex. Buttons : 0"; lbX2Value.Text = "X : " + currentPosition.RightX.ToString(); lbY2Value.Text = "Y : " + currentPosition.RightY.ToString(); lbZ2Value.Text = "Z : " + currentPosition.RightZ.ToString(); lbGimbal12.Text = "Gimbal 1 : " + currentPosition.Gimbal1Right.ToString(); lbGimbal22.Text = "Gimbal 2 : " + currentPosition.Gimbal2Right.ToString(); lbGimbal32.Text = "Gimbal 3 : " + currentPosition.Gimbal3Right.ToString(); lbButtons2.Text = "Buttons : " + currentPosition.ButtonsRight.ToString(); lbInk2.Text = "InkWell : " + currentPosition.InkwellRight.ToString(); }
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 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; */ }
private void setForces(OmniPosition position) { try { double forceLX = 0; double forceLY = 0; double forceLZ = 0; double forceRX = 0; double forceRY = 0; double forceRZ = 0; if (feedbackEnabled) { IntPtr ptr = getpos1(); double[] pos1 = new double[8]; Marshal.Copy(ptr, pos1, 0, 8); ReleaseMemory(ptr); IntPtr ptr2 = getpos2(); double[] pos2 = new double[8]; Marshal.Copy(ptr2, pos2, 0, 8); ReleaseMemory(ptr2); forceLX = (position.LeftX - (pos1[0] - forceOffset_LX)) / getForceStrength(); forceLY = (position.LeftY - (pos1[1] - forceOffset_LY)) / getForceStrength(); forceLZ = (position.LeftZ - (pos1[2] - forceOffset_LZ)) / getForceStrength(); forceRX = (position.RightX - (pos2[0] - forceOffset_RX)) / getForceStrength(); forceRY = (position.RightY - (pos2[1] - forceOffset_RY)) / getForceStrength(); forceRZ = (position.RightZ - (pos2[2] - forceOffset_RZ)) / getForceStrength(); } if (cb_noOmnisAttached.Checked) { forceLX = position.LeftX; forceLY = position.LeftY; forceLZ = position.LeftZ; forceRX = position.RightX; forceRY = position.RightY; forceRZ = position.RightZ; tb_forces.Text = @"Left" + Environment.NewLine + "X = " + Math.Round(forceLX, 4) + " Y = " + Math.Round(forceLY, 4) + " Z = " + Math.Round(forceLZ, 4) + Environment.NewLine + "Right" + Environment.NewLine + "X = " + Math.Round(forceRX, 4) + " Y = " + Math.Round(forceRY, 4) + " Z = " + Math.Round(forceRZ, 4); } else { setForce1(forceLX, forceLY, forceLZ); setForce2(forceRX, forceRY, forceRZ); tb_forces.Text = @"Left" + Environment.NewLine + "X = " + Math.Round(forceLX, 4) + " Y = " + Math.Round(forceLY, 4) + " Z = " + Math.Round(forceLZ, 4) + Environment.NewLine + "Right" + Environment.NewLine + "X = " + Math.Round(forceRX, 4) + " Y = " + Math.Round(forceRY, 4) + " Z = " + Math.Round(forceRZ, 4); } } catch (Exception ex) { ShowError(ex.Message, ex.ToString()); } }
public OmniPosition Add(OmniPosition pos) { return new OmniPosition( LeftX + pos.LeftX, LeftY + pos.LeftY, LeftZ + pos.LeftZ, Gimbal1Left + pos.Gimbal1Left, Gimbal2Left + pos.Gimbal2Left, Gimbal3Left + pos.Gimbal3Left, ButtonsLeft, InkwellLeft, RightX + pos.RightX, RightY + pos.RightY, RightZ + pos.RightZ, Gimbal1Right + pos.Gimbal1Right, Gimbal2Right + pos.Gimbal2Right, Gimbal3Right + pos.Gimbal3Right, ButtonsRight, InkwellRight, ExtraButtons ); }
public bool CheckForEmergencySwitch(OmniPosition Position) { bool btnPressed = checkButton(Position, EmergencySwitchBoundBtn, EmergencySwitchBoundValue); if (!btnPressed && EmergBtnPressed) { EmergBtnPressed = false; return true; } EmergBtnPressed = btnPressed; return false; }
private void freezeOmniPosition(OmniPosition pos) { //make sure this method is executed once freezePressed = true; //save current position frozenPosition = pos; //Relieve any forces on omnis feedbackEnabled = false; setForces(new OmniPosition()); }
private void dataReceived(IAsyncResult ar) { try { IPEndPoint ep = new IPEndPoint(IPAddress.Any, dataPort); byte[] arry = dataListener.EndReceive(ar, ref ep); string json = Encoding.ASCII.GetString(arry); SocketMessage dataMsg = JsonConvert.DeserializeObject<SocketMessage>(json); inControlPosition = dataMsg.OmniPosition; inControlIsFrozen = dataMsg.isFrozen; } catch (Exception ex) { //Just log error instead of showing, currently too many non-critical errors logMessage(ex.Message, ex.ToString(), Logging.StatusTypes.Error); } hasMasterData = true; if (isListeningForData) listenForData(); }
private void switchControl(bool takeControl, string controlIP) { if (takeControl) { if (btn_Initialize.Enabled || !inControlIsFrozen) tb_InControl.Text = "You are in control."; else { frozenPosition = inControlPosition; isFrozen = true; tb_InControl.Text = "Press R1 button on omni when ready..."; } tb_InControl.BackColor = Color.Red; } else { tb_InControl.Text = controlIP + " is in control."; tb_InControl.BackColor = Color.Green; isFrozen = false; } inControlIP = controlIP; isInControl = takeControl; groupBox3.Enabled = !takeControl; tb_forces.Enabled = !takeControl; btn_zeroForces.Enabled = !takeControl; lbl_forceStrength.Enabled = !takeControl; trb_forceStrength.Enabled = !takeControl; btn_ReqControl.Enabled = !takeControl; }
private void showOmniPositions() { OmniPosition currentPosition = new OmniPosition(0, 0, 0, 0, 0, 0); double[] pos1 = { 0, 0, 0, 0, 0, 0, 0, 0 }; double[] pos2 = { 0, 0, 0, 0, 0, 0, 0, 0 }; if (!cb_noOmnisAttached.Checked) { try { IntPtr ptr = getpos1(); Marshal.Copy(ptr, pos1, 0, 8); ReleaseMemory(ptr); IntPtr ptr2 = getpos2(); Marshal.Copy(ptr2, pos2, 0, 8); ReleaseMemory(ptr2); currentPosition = new OmniPosition(pos1, pos2); } catch (Exception ex) { ShowError(ex.Message, ex.ToString()); } } lbX1value.Text = "X : " + currentPosition.LeftX.ToString(); lbY1value.Text = "Y : " + currentPosition.LeftY.ToString(); lbZ1value.Text = "Z : " + currentPosition.LeftZ.ToString(); lbGimbal11.Text = "Gimbal 1 : " + currentPosition.Gimbal1Left.ToString(); lbGimbal21.Text = "Gimbal 2 : " + currentPosition.Gimbal2Left.ToString(); lbGimbal31.Text = "Gimbal 3 : " + currentPosition.Gimbal3Left.ToString(); lbButtons1.Text = "Buttons : " + currentPosition.ButtonsLeft.ToString(); lbInk1.Text = "InkWell : " + currentPosition.InkwellLeft.ToString(); lbX2Value.Text = "X : " + currentPosition.RightX.ToString(); lbY2Value.Text = "Y : " + currentPosition.RightY.ToString(); lbZ2Value.Text = "Z : " + currentPosition.RightZ.ToString(); lbGimbal12.Text = "Gimbal 1 : " + currentPosition.Gimbal1Right.ToString(); lbGimbal22.Text = "Gimbal 2 : " + currentPosition.Gimbal2Right.ToString(); lbGimbal32.Text = "Gimbal 3 : " + currentPosition.Gimbal3Right.ToString(); lbButtons2.Text = "Buttons : " + currentPosition.ButtonsRight.ToString(); lbInk2.Text = "InkWell : " + currentPosition.InkwellRight.ToString(); if (isInControl) { inControlPosition = currentPosition; //check for "Freeze" button if (currentPosition.ButtonsRight == 1 && !freezePressed) { if (isFrozen) { //Force omnis to frozen position Thread t = new Thread(new ParameterizedThreadStart(forceOmniPosition)); t.IsBackground = true; t.Start(frozenPosition); } else { freezeOmniPosition(currentPosition); isFrozen = true; } freezePressed = true; } } else { if (currentPosition.ButtonsRight == 1 && !freezePressed) { feedbackEnabled = !feedbackEnabled; freezePressed = true; } } if (currentPosition.ButtonsRight != 1 && freezePressed) freezePressed = false; if (cb_isMaster.Checked || hasMasterData) { if (isFrozen) { OutputPosition = frozenPosition; } 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; }
private void UnderlyingTimerTick(object sender, EventArgs e) { try { //Get current position OmniPosition currentPos = new OmniPosition(); if( User.HasOmnis ) { currentPos = User.GetOmniPositions(); } showOmniPositions(currentPos); if (User.IsInControl) { Surgery.UserInControl = User; if (telSurgeOnly && User.IsFrozen) { Surgery.InControlPosition = User.FrozenPosition; } else Surgery.InControlPosition = currentPos; if (!User.IsMaster) { //Only send data to Master while InControl SocketData.SendUDPDataTo(IPAddress.Parse(Surgery.Master.MyIPAddress), SocketData.CreateMessageToSend()); messageCount++; } //Check for freeze button press if (telSurgeOnly && this.User.CheckForFreeze(currentPos)) Freeze(); //Display Frozen status if (User.IsFrozen) tb_InControl.Text = "You are frozen."; else tb_InControl.Text = "You are in control!"; } else { if (User.IsMaster) { //check for emergency switch if (this.User.CheckForEmergencySwitch(currentPos)) emergencySwitchControl(); } //Check for following this.User.CheckIfFollowing(currentPos); if (User.IsFollowing) User.OmniFollow(Surgery.InControlPosition); else User.SetOmniForce(new OmniPosition()); } if (User.IsMaster) { //Master always sends data if (Surgery.ConnectedClients.Count > 0) SocketData.MasterSendData(); //Check for "stay alive" from clients foreach (User u in Surgery.ConnectedClients) { if (DateTime.Now.Subtract(u.LastHeardFrom).Seconds > 30) { disconnectClient(u); break; } } } else { //Send "stay alive to Master if (User.ConnectedToMaster) { if (DateTime.Now.Subtract(User.LastHeardFrom).Seconds > 20) { TcpClient tCPClient = new TcpClient(); tCPClient.Connect(IPAddress.Parse(Surgery.Master.MyIPAddress), connectionPort); SocketMessage sm = new SocketMessage(Surgery, User); SocketData.SendTCPDataTo(tCPClient, SocketData.SerializeObject<SocketMessage>(sm)); User.LastHeardFrom = DateTime.Now; } } } //Update and show output data (RobotApp) if (Surgery.InControlPosition != null) { OutputPosition = Surgery.InControlPosition; showOutputPosition(); } } catch (Exception ex) { UnderlyingTimer.Enabled = false; ShowError("An error occured during routine program timer. Application must be reset.", ex.Message + " => " + ex.ToString()); } }
public bool CheckForFreeze(OmniPosition Position) { bool btnPressed = checkButton(Position, FreezeBoundBtn, FreezeBoundValue); if (!btnPressed && FreezeBtnPressed) { FreezeBtnPressed = false; return true; } FreezeBtnPressed = btnPressed; return false; }
/* double forceOffset_LX = 0; double forceOffset_LY = 0; double forceOffset_LZ = 0; double forceOffset_RX = 0; double forceOffset_RY = 0; double forceOffset_RZ = 0; bool sendFreezeComd = false; private OmniPosition inControlPosition; private OmniPosition frozenPosition; private OmniPosition positionOffset = new OmniPosition(); public OmniPosition currentPosition; private bool buttonPressed = false; private bool feedbackEnabled = false; //private volatile bool isListeningForData = false; private volatile string inControlIP; private volatile bool applicationRunning = true; string exeFile = (new System.Uri(Assembly.GetEntryAssembly().CodeBase)).AbsolutePath; private bool EmergencySwitch = false; public bool networkDataDelayChanged = false; public string EmergencySwitchBoundBtn; public int EmergencySwitchBoundValue; */ public TelSurgeMain() { InitializeComponent(); try { this.User = new User(this, connectionPort); this.AudioConference = new TelSurge.AudioConference(this, audioPort); this.VideoCapture = new VideoCapture(this, videoPort); this.Surgery = new Surgery(); this.Markup = new Markup(this, markingsPort); this.SocketData = new SocketData(this, dataPort); OutputPosition = new OmniPosition(); TelSurgeMain.CheckForIllegalCrossThreadCalls = false; fillOmniDDL(); fillAudioDeviceDDL(); HapticForces = new OmniPosition(); SendFrozen = false; messageCount = 0; //Set Force Trackbar //want force divider between 20 and 220 //divider = 220 - trackbarValue trb_forceStrength.Minimum = 0; trb_forceStrength.Maximum = 200; // The TickFrequency property establishes how many positions are between each tick-mark. trb_forceStrength.TickFrequency = 20; // The LargeChange property sets how many positions to move if the bar is clicked on either side of the slider. trb_forceStrength.LargeChange = 2; // The SmallChange property sets how many positions to move if the keyboard arrows are used to move the slider. trb_forceStrength.SmallChange = 1; //set initial value of trackbar trb_forceStrength.Value = 170; CaptureImageBox.FunctionalMode = Emgu.CV.UI.ImageBox.FunctionalModeOption.Minimum; } catch (Exception ex) { ShowError(ex.Message, ex.ToString()); } }
public void CheckIfFollowing(OmniPosition Position) { bool btnPressed = checkButton(Position, FollowingBoundBtn, FollowingBoundValue); if (!btnPressed && FollowBtnPressed) { FollowBtnPressed = false; IsFollowing = !IsFollowing; } FollowBtnPressed = btnPressed; }
public OmniPosition Subtract(OmniPosition pos) { return new OmniPosition( LeftX - pos.LeftX, LeftY - pos.LeftY, LeftZ - pos.LeftZ, Gimbal1Left - pos.Gimbal1Left, Gimbal2Left - pos.Gimbal2Left, Gimbal3Left - pos.Gimbal3Left, ButtonsLeft, InkwellLeft, RightX - pos.RightX, RightY - pos.RightY, RightZ - pos.RightZ, Gimbal1Right - pos.Gimbal1Right, Gimbal2Right - pos.Gimbal2Right, Gimbal3Right - pos.Gimbal3Right, ButtonsRight, InkwellRight, ExtraButtons ); }
//Construct Form1 public Form1() { InitializeComponent(); try { OutputPosition = new OmniPosition(); Form1.CheckForIllegalCrossThreadCalls = false; fillOmniDDL(); fillAudioDeviceDDL(); //Set Force Trackbar //want force divider between 20 and 220 //divider = 220 - trackbarValue trb_forceStrength.Minimum = 0; trb_forceStrength.Maximum = 200; // The TickFrequency property establishes how many positions are between each tick-mark. trb_forceStrength.TickFrequency = 20; // The LargeChange property sets how many positions to move if the bar is clicked on either side of the slider. trb_forceStrength.LargeChange = 2; // The SmallChange property sets how many positions to move if the keyboard arrows are used to move the slider. trb_forceStrength.SmallChange = 1; //set initial value of trackbar trb_forceStrength.Value = 170; captureImageBox.FunctionalMode = Emgu.CV.UI.ImageBox.FunctionalModeOption.Minimum; //try to set up capture from default camera _capture = new Capture(); _capture.SetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_FPS, 30); _capture.ImageGrabbed += ProcessFrame; } catch (NullReferenceException nrex) { if (nrex.HResult == -2147467261) ShowError("Cannot connect to default camera!", "No camera could be found on this machine, (" + Environment.MachineName + ")."); else ShowError(nrex.Message, nrex.ToString()); } catch (Exception ex) { ShowError(ex.Message, ex.ToString()); } }