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());
            }
        }