Added support for second gripper

MissouriMRDT · May 31, 2019 · bc1ac9f · bc1ac9f
1 parent 6e39ecd
commit bc1ac9f
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 5 deletions.
diff --git 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<ArmPositionViewModel> Positions = new ObservableCollection<ArmPositionViewModel>();
         internal ArmPositionViewModel SelectedPosition;
         internal float CoordinateX;

diff --git 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<string, float> 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<string, float> 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<string, float> 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
@@ -33,6 +33,7 @@
                         <RowDefinition/>
                         <RowDefinition/>
                         <RowDefinition/>
+                        <RowDefinition/>
                     </Grid.RowDefinitions>
                     <Label Grid.Column="0" Grid.Row="1">Base Control Multiplier:</Label>
                     <Label Grid.Column="1" Grid.Row="1" x:Name="BaseRangeFactor"></Label>
@@ -46,6 +47,9 @@
                     <Label Grid.Column="0" Grid.Row="4">Gripper Control Multiplier:</Label>
                     <Label Grid.Column="1" Grid.Row="4" x:Name="GripperRangeFactor"></Label>
                     <Slider Grid.Column="2" Grid.Row="4" Width="300" Maximum="1000" Minimum="0" IsMoveToPointEnabled="True" Value="{Binding GripperRangeFactor}"></Slider>
+                    <Label Grid.Column="0" Grid.Row="5">Gripper Two Control Multiplier:</Label>
+                    <Label Grid.Column="1" Grid.Row="5" x:Name="Gripper2RangeFactor"></Label>
+                    <Slider Grid.Column="2" Grid.Row="5" Width="300" Maximum="1000" Minimum="0" IsMoveToPointEnabled="True" Value="{Binding Gripper2RangeFactor}"></Slider>
                 </Grid>
             </Expander>
 		</StackPanel>