diff --git a/RoverAttachmentManager/Models/Arm/ArmModel.cs b/RoverAttachmentManager/Models/Arm/ArmModel.cs index 150e67e9..00bed495 100644 --- a/RoverAttachmentManager/Models/Arm/ArmModel.cs +++ b/RoverAttachmentManager/Models/Arm/ArmModel.cs @@ -17,6 +17,7 @@ internal class ArmModel internal int ElbowRangeFactor = 500; internal int WristRangeFactor = 1000; internal int GripperRangeFactor = 1000; + internal int Gripper2RangeFactor = 1000; internal ObservableCollection Positions = new ObservableCollection(); internal ArmPositionViewModel SelectedPosition; internal float CoordinateX; diff --git a/RoverAttachmentManager/ViewModels/Arm/ArmViewModel.cs b/RoverAttachmentManager/ViewModels/Arm/ArmViewModel.cs index 3ed0365d..ab4f418f 100644 --- a/RoverAttachmentManager/ViewModels/Arm/ArmViewModel.cs +++ b/RoverAttachmentManager/ViewModels/Arm/ArmViewModel.cs @@ -134,6 +134,18 @@ public int GripperRangeFactor NotifyOfPropertyChange(() => GripperRangeFactor); } } + public int Gripper2RangeFactor + { + get + { + return _model.Gripper2RangeFactor; + } + set + { + _model.Gripper2RangeFactor = value; + NotifyOfPropertyChange(() => Gripper2RangeFactor); + } + } public float AngleJ1 { get @@ -474,6 +486,7 @@ private void SetOpenLoopValues(Dictionary values) Int16 ArmBaseTwist = 0; Int16 ArmBaseBend = 0; Int16 Gripper = 0; + Int16 Gripper2 = 0; Int16 Nipper = 0; switch (ControllerBase.JoystickDirection(values["WristBend"], values["WristTwist"])) @@ -512,14 +525,23 @@ private void SetOpenLoopValues(Dictionary values) ArmBaseTwist = (Int16)(-ControllerBase.TwoButtonToggleDirection(values["BaseTwistDirection"] != 0, (values["BaseTwistMagnitude"])) * BaseRangeFactor); ArmBaseBend = (Int16)(-ControllerBase.TwoButtonToggleDirection(values["BaseBendDirection"] != 0, (values["BaseBendMagnitude"])) * BaseRangeFactor); - Gripper = (Int16)(ControllerBase.TwoButtonTransform(values["GripperClose"] > 0, values["GripperOpen"] > 0, values["GripperClose"], -values["GripperOpen"], 0) * GripperRangeFactor); + + float gripperAmmount = ControllerBase.TwoButtonTransform(values["GripperClose"] > 0, values["GripperOpen"] > 0, values["GripperClose"], -values["GripperOpen"], 0); + if (SelectedTool == 0) + { + Gripper = (Int16)(gripperAmmount * GripperRangeFactor); + }else if (SelectedTool == 1) + { + Gripper2 = (Int16)(gripperAmmount * Gripper2RangeFactor); + } + Nipper = (Int16)values["Nipper"]; - Int16[] sendValues = { Nipper, Gripper, ArmWristTwist, ArmWristBend, ArmElbowTwist, ArmElbowBend, ArmBaseBend, ArmBaseTwist }; //order before we reverse + Int16[] sendValues = { Gripper2, Nipper, Gripper, ArmWristTwist, ArmWristBend, ArmElbowTwist, ArmElbowBend, ArmBaseBend, ArmBaseTwist }; //order before we reverse byte[] data = new byte[sendValues.Length * sizeof(Int16)]; Buffer.BlockCopy(sendValues, 0, data, 0, data.Length); Array.Reverse(data); - _rovecomm.SendCommand(new Packet("ArmValues", data, 8, (byte)DataTypes.INT16_T)); + _rovecomm.SendCommand(new Packet("ArmValues", data, 9, (byte)DataTypes.INT16_T)); if (values["GripperSwap"] == 1) { @@ -528,11 +550,11 @@ private void SetOpenLoopValues(Dictionary values) if (values["SwitchTool"] == 1 && previousTool == SelectedTool) { - if(++SelectedTool > 2) + if(++SelectedTool > 1) { SelectedTool = 0; } - _rovecomm.SendCommand(new Packet("ToolSelection", SelectedTool)); + //_rovecomm.SendCommand(new Packet("ToolSelection", SelectedTool)); } else if (values["SwitchTool"] == 0){ previousTool = SelectedTool; diff --git a/RoverAttachmentManager/Views/Arm/ArmView.xaml b/RoverAttachmentManager/Views/Arm/ArmView.xaml index aa11d065..34d955c7 100644 --- a/RoverAttachmentManager/Views/Arm/ArmView.xaml +++ b/RoverAttachmentManager/Views/Arm/ArmView.xaml @@ -33,6 +33,7 @@ + @@ -46,6 +47,9 @@ + + +