Skip to content

Commit

Permalink
added initial control for atlas arm
Browse files Browse the repository at this point in the history
  • Loading branch information
@cantthinkofaname1029
cantthinkofaname1029 committed Mar 24, 2018
1 parent 5051144 commit ce32272
Show file tree
Hide file tree
Showing 5 changed files with 71 additions and 47 deletions.
38 changes: 24 additions & 14 deletions RED/Configurations/Input/InputManagerConfig.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,22 +18,32 @@ internal static class InputManagerConfig
new InputChannelContext("Y", "VectorY"),
new InputChannelContext("Slider0", "Throttle"),
new InputChannelContext("Button7Debounced", "ModeCycle") }),
new InputMappingContext("Arm (Traditional)", "Xbox", "Arm", 200, new[] {
new InputMappingContext("Arm (Traditional)", "Xbox", "Arm", 100, new[] {
new InputChannelContext("JoyStick1Y", "IKYIncrement"){ Parabolic = true },
new InputChannelContext("JoyStick1Y", "ElbowBend"){ Parabolic = true },
new InputChannelContext("JoyStick1X", "IKXIncrement"){ Parabolic = true },
new InputChannelContext("JoyStick1X", "ElbowTwist"){ Parabolic = true },
new InputChannelContext("JoyStick2Y", "WristTwist"){ Parabolic = true, LinearScaling = -1 },
new InputChannelContext("JoyStick2X", "WristBend"){ Parabolic = true },
new InputChannelContext("DPadU", "ShoulderBendForward"),
new InputChannelContext("DPadD", "ShoulderBendBackward"),
new InputChannelContext("DPadR", "ShoulderTwistForward"),
new InputChannelContext("DPadL", "ShoulderTwistBackward"),
new InputChannelContext("ButtonY", "DebouncedArmReset"),
new InputChannelContext("LeftTrigger", "GripperOpen"),
new InputChannelContext("RightTrigger", "GripperClose"),
new InputChannelContext("ButtonRb", "ServoClockwise"),
new InputChannelContext("ButtonLb", "ServoCounterClockwise"),
new InputChannelContext("ButtonB", "TowRopeOut"),
new InputChannelContext("ButtonX", "TowRopeIn"),
new InputChannelContext("JoyStick2Y", "IKPitchIncrement"){ Parabolic = true },
new InputChannelContext("JoyStick2Y", "WristBend"){ Parabolic = true},
new InputChannelContext("JoyStick2X", "IKYawIncrement"){ Parabolic = true },
new InputChannelContext("JoyStick2X", "WristTwist"){ Parabolic = true },
new InputChannelContext("DPadU", "UseOpenLoop"),
new InputChannelContext("DPadD", "UseAngular"),
new InputChannelContext("DPadR", "UseRoverPerspectiveIK"),
new InputChannelContext("DPadL", "UseWristPerspectiveIK"),
new InputChannelContext("ButtonY", "NipperClose"),
new InputChannelContext("ButtonA", "NipperOpen"),
new InputChannelContext("LeftTrigger", "IKZMagnitude"),
new InputChannelContext("LeftTrigger", "BaseBendMagnitude"),
new InputChannelContext("RightTrigger", "IKRollMagnitude"),
new InputChannelContext("RightTrigger", "BaseTwistMagnitude"),
new InputChannelContext("ButtonRb", "IKRollDirection"),
new InputChannelContext("ButtonRb", "BaseTwistDirection"),
new InputChannelContext("ButtonLb", "IKZDirection"),
new InputChannelContext("ButtonLb", "BaseBendDirection"),
new InputChannelContext("ButtonB", "GripperOpen"),
new InputChannelContext("ButtonX", "GripperClose"),
new InputChannelContext("ButtonBackDebounced", "DebouncedArmReset"),
new InputChannelContext("ButtonStartDebounced", "ModeCycle") }),
new InputMappingContext("Xbox Science Arm", "Xbox", "ScienceArm", 30, new[] {
new InputChannelContext("JoyStick1Y", "Arm"){ Parabolic = true },
Expand Down
12 changes: 8 additions & 4 deletions RED/Configurations/MetadataManagerConfig.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ internal static class MetadataManagerConfig
new MetadataRecordContext(804, "ArmJ4", "Positive is up. Negative is down."),
new MetadataRecordContext(805, "ArmJ5", "Positive is forward. Negative is backward."),
new MetadataRecordContext(806, "ArmJ6", "Positive is clockwise. Negative is counter-clockwise."),
new MetadataRecordContext(807, "ArmValues", "All values for the arm together. Armj1-j6, then primary then secondary gripper."),
new MetadataRecordContext(800, "ArmStop", "Stops all arm movement"),
new MetadataRecordContext(816, "ArmEnableAll", "Enables/Disables Power to all Joints"),
new MetadataRecordContext(817, "ArmEnableMain", "Enables/Disables Power to 12V Bus"),
Expand All @@ -29,14 +30,17 @@ internal static class MetadataManagerConfig
new MetadataRecordContext(821, "ArmEnableJ4", "Enables/Disables Power to J4"),
new MetadataRecordContext(822, "ArmEnableJ5", "Enables/Disables Power to J5"),
new MetadataRecordContext(823, "ArmEnableJ6", "Enables/Disables Power to J6"),
new MetadataRecordContext(824, "ArmEnableEndeff", "Enables/Disables Power to Endeffector"),
new MetadataRecordContext(825, "ArmEnableServo", "Enables/Disables Power to Endeffector Servo"),
new MetadataRecordContext(824, "ArmEnableEndeff1", "Enables/Disables Power to Endeffector"),
new MetadataRecordContext(825, "ArmEnableEndeff2", "Enables/Disables Power to Endeffector Servo"),
new MetadataRecordContext(784, "ArmAbsoluteAngle", "Sets position based on six absolute angles"),
new MetadataRecordContext(785, "ArmAbsoluteXYZ", "Sets position based on absolute XYZ, yaw pitch and roll"),
new MetadataRecordContext(786, "IKRoverIncrement", "Increments IK positions based on rover perspective, XYZ yaw pitch roll. Each value is normalized -1000 to 1000"),
new MetadataRecordContext(787, "IKWristIncrement", "Increments IK positions based on wrist perspective, XYZ yaw pitch roll. Each value is normalized -1000 to 1000"),
new MetadataRecordContext(793, "ArmGetPosition", "Requests Current Position"),
new MetadataRecordContext(896, "LimitSwitchUnOverride", "Index of limit switch to remove override"),
new MetadataRecordContext(897, "LimitSwitchOverride", "Index of limit switch to override"),
new MetadataRecordContext(864, "Gripper", "Positive is open. Negative is close."),
new MetadataRecordContext(868, "EndeffectorServo", "Positive is clockwise. Negative is counterclockwise.")
new MetadataRecordContext(864, "Endeffector1", "Positive is open. Negative is close."),
new MetadataRecordContext(868, "Endeffector2", "Positive is clockwise. Negative is counterclockwise.")
},
Telemetry = new[] {
new MetadataRecordContext(792, "ArmCurrentPosition", "Contains the current angles from each joint"),
Expand Down
12 changes: 12 additions & 0 deletions RED/ViewModels/Input/ControllerBase.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,18 @@ protected float Debounce(string key, bool newState)
}
}

static public float TwoButtonToggleDirection(bool directionButtonInput, float analogButtonInput)
{
if (directionButtonInput)
{
return -1 * analogButtonInput;
}
else
{
return analogButtonInput;
}
}

static public float DeadzoneTransform(int x, int deadzone)
{
return (x < deadzone && x > -deadzone) ? 0f : (x + (x < 0 ? deadzone : -deadzone)) / (float)(32768 - deadzone);
Expand Down
4 changes: 2 additions & 2 deletions RED/ViewModels/Input/Controllers/XboxControllerInputViewModel.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,8 +95,8 @@ public Dictionary<string, float> GetValues()
["JoyStick2X"] = DeadzoneTransform(currentGamepad.RightThumbX, deadzone),
["JoyStick2Y"] = DeadzoneTransform(currentGamepad.RightThumbY, deadzone),

["LeftTrigger"] = (float)currentGamepad.LeftTrigger / 255,
["RightTrigger"] = (float)currentGamepad.RightTrigger / 255,
["LeftTrigger"] = (float)currentGamepad.LeftTrigger / 255.0f,
["RightTrigger"] = (float)currentGamepad.RightTrigger / 255.0f,
["ButtonA"] = ((currentGamepad.Buttons & GamepadButtonFlags.A) != 0) ? 1 : 0,
["ButtonB"] = ((currentGamepad.Buttons & GamepadButtonFlags.B) != 0) ? 1 : 0,
["ButtonX"] = ((currentGamepad.Buttons & GamepadButtonFlags.X) != 0) ? 1 : 0,
Expand Down
52 changes: 25 additions & 27 deletions RED/ViewModels/Modules/ArmViewModel.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -355,8 +355,8 @@ private void SetOpenLoopValues(Dictionary<string, float> values)
}


ArmBaseTwist = (Int16)(TwoButtonToggleDirection(values["BaseTwistDirection"] != 0, (values["BaseTwistMagnitude"])) * MotorRangeFactor);
ArmBaseBend = (Int16)(TwoButtonToggleDirection(values["BaseBendDirection"] != 0, (values["BaseBendMagnitude"])) * MotorRangeFactor);
ArmBaseTwist = (Int16)(ControllerBase.TwoButtonToggleDirection(values["BaseTwistDirection"] != 0, (values["BaseTwistMagnitude"])) * MotorRangeFactor);
ArmBaseBend = (Int16)(ControllerBase.TwoButtonToggleDirection(values["BaseBendDirection"] != 0, (values["BaseBendMagnitude"])) * MotorRangeFactor);
Gripper = (Int16)(ControllerBase.TwoButtonTransform(values["GripperClose"] > 0, values["GripperOpen"] > 0, values["GripperClose"], -values["GripperOpen"], 0) * EndeffectorSpeedLimit);
Nipper = (Int16)(ControllerBase.TwoButtonTransform(values["NipperClose"] > 0, values["NipperOpen"] > 0, values["NipperClose"], -values["NipperOpen"], 0) * EndeffectorSpeedLimit);

Expand Down Expand Up @@ -417,8 +417,8 @@ private void SetIKValues(Dictionary<string, float> values, ArmControlState state
}


Z = (Int16)(TwoButtonToggleDirection(values["IKZDirection"] != 0, (values["IKZMagnitude"])) * MotorRangeFactor);
Roll = (Int16)(TwoButtonToggleDirection(values["IKRollDirection"] != 0, (values["IKRollMagnitude"])) * MotorRangeFactor);
Z = (Int16)(ControllerBase.TwoButtonToggleDirection(values["IKZDirection"] != 0, (values["IKZMagnitude"])) * MotorRangeFactor);
Roll = (Int16)(ControllerBase.TwoButtonToggleDirection(values["IKRollDirection"] != 0, (values["IKRollMagnitude"])) * MotorRangeFactor);
Gripper = (Int16)(ControllerBase.TwoButtonTransform(values["GripperClose"] > 0, values["GripperOpen"] > 0, values["GripperClose"], -values["GripperOpen"], 0) * MotorRangeFactor);
Nipper = (Int16)(ControllerBase.TwoButtonTransform(values["NipperClose"] > 0, values["NipperOpen"] > 0, values["NipperClose"], -values["NipperOpen"], 0) * MotorRangeFactor);

Expand All @@ -445,22 +445,22 @@ private void UpdateControlState(Dictionary<string, float> values)
if (values["UseAngular"] != 0)
{
myState = ArmControlState.Angular;
stateLog = "Switching robotic arm control state to angular control";
stateLog = "Switching to angular control";
}
else if (values["UseOpenLoop"] != 0)
{
myState = ArmControlState.OpenLoop;
stateLog = "Switching robotic arm control state to open loop";
stateLog = "Switching to open loop";
}
else if (values["UseRoverPerspectiveIK"] != 0)
{
myState = ArmControlState.IKRoverPOV;
stateLog = "Switching robotic arm control state to rover perspective IK";
stateLog = "Switching to rover perspective IK";
}
else if (values["UseWristPerspectiveIK"] != 0)
{
myState = ArmControlState.IKWristPOV;
stateLog = "Switching robotic arm control state to wrist perspective IK";
stateLog = "Switching to wrist perspective IK";
}

if (oldState != myState)
Expand Down Expand Up @@ -492,9 +492,8 @@ public void SetValues(Dictionary<string, float> values)
public void StopMode()
{
_router.Send(_idResolver.GetId("ArmStop"), (Int16)(0), true);
_router.Send(_idResolver.GetId("Gripper"), (Int16)(0), true);
_router.Send(_idResolver.GetId("EndeffectorServo"), (Int16)(0), true);
_router.Send(_idResolver.GetId("CarabinerSpeed"), (Int16)(0), true);
_router.Send(_idResolver.GetId("Endeffector1"), (Int16)(0), true);
_router.Send(_idResolver.GetId("Endeffector2"), (Int16)(0), true);
}

public void EnableCommand(string bus, bool enableState)
Expand All @@ -504,11 +503,14 @@ public void EnableCommand(string bus, bool enableState)
{
case "All": id = _idResolver.GetId("ArmEnableAll"); break;
case "Main": id = _idResolver.GetId("ArmEnableMain"); break;
case "J12": id = _idResolver.GetId("ArmEnableJ1"); break;
case "J1": id = _idResolver.GetId("ArmEnableJ1"); break;
case "J2": id = _idResolver.GetId("ArmEnableJ2"); break;
case "J3": id = _idResolver.GetId("ArmEnableJ3"); break;
case "J45": id = _idResolver.GetId("ArmEnableJ4"); break;
case "Endeff": id = _idResolver.GetId("ArmEnableEndeff"); break;
case "Servo": id = _idResolver.GetId("ArmEnableServo"); break;
case "J4": id = _idResolver.GetId("ArmEnableJ4"); break;
case "J5": id = _idResolver.GetId("ArmEnableJ5"); break;
case "J6": id = _idResolver.GetId("ArmEnableJ6"); break;
case "Endeff1": id = _idResolver.GetId("ArmEnableEndeff1"); break;
case "Endeff2": id = _idResolver.GetId("ArmEnableEndeff2"); break;
default: return;
}

Expand All @@ -527,6 +529,14 @@ public void SetPosition()
_router.Send(_idResolver.GetId("ArmAbsoluteAngle"), data, true);
}

public void SetXYZPosition()
{
float[] coordinates = { CoordinateX, CoordinateY, CoordinateZ, Yaw, Pitch, Roll };
byte[] data = new byte[coordinates.Length * sizeof(float)];
Buffer.BlockCopy(coordinates, 0, data, 0, data.Length);
_router.Send(_idResolver.GetId("ArmAbsoluteXYZ"), data, true);
}

public void LimitSwitchOverride(byte index)
{
_router.Send(_idResolver.GetId("LimitSwitchOverride"), index, true);
Expand Down Expand Up @@ -589,18 +599,6 @@ private JoystickDirections JoystickDirection(float y, float x)
return JoystickDirections.None;
}

private float TwoButtonToggleDirection(bool directionButtonInput, float analogButtonInput)
{
if (directionButtonInput)
{
return -1 * analogButtonInput;
}
else
{
return analogButtonInput;
}
}

private enum JoystickDirections
{
None = 0,
Expand Down

0 comments on commit ce32272

Please sign in to comment.