/// <summary>
/// Attempts to attach a value to a node in the virtual bus. Notifies subscribed parties if successful.
/// </summary>
/// <typeparam name="T">The type of the object to attach to the node. Must be a serializable type.</typeparam>
/// <param name="node">The node to attach the value to.</param>
/// <param name="value">The new value of the node.</param>
/// <exception cref="ArgumentException">Thrown if the type of T is not valid or is not compatible with the declared type of the node.</exception>
public void Publish <T>(BusNode node, T value)
{
NodeEntry entry;
T copy;
if (!SerializeCopy(value, out copy))
{
throw new ArgumentException("Cannot publish a value of type " + typeof(T).Name + " to node " + node + ". The type must be serializeable.");
}
entry = _nodes[node];
if (!entry.Node.NodeType.IsAssignableFrom(typeof(T)))
{
throw new ArgumentException("Cannot publish a value of type " + typeof(T).Name + " to a node " + node + " of type " + entry.Node.NodeType.Name + ". The types are not compatible.");
}
lock (entry)
{
entry.Value = copy;
}
QueueNotification(node);
}
/************************************************************************************************************************************/
/* Public Methods */
/************************************************************************************************************************************/
/// <summary>
/// Returns the current value of a node.
/// </summary>
/// <typeparam name="T">The type of the node to retrieve.</typeparam>
/// <param name="node">The node to retrieve.</param>
/// <returns>The value of the node cast to the parameterized type. Must be a serializable type.</returns>
/// <exception cref="InvalidCastException">Thrown if the node can't be cast to the parameterized type.</exception>
/// <exception cref="ArgumentException">Thrown if T is not a valid type.</exception>
public T Get <T>(BusNode node)
{
T value, copy;
try
{
object tmp = _nodes[node].Value;
if (tmp != null)
{
value = (T)(tmp);
}
else
{
value = default(T);
}
}
catch (InvalidCastException e)
{
throw e;
}
if (!SerializeCopy(value, out copy))
{
throw new ArgumentException("Cannot get a value of type " + typeof(T).Name + " from node " + node + ". The type must be serializeable.");
}
return(copy);
}
/// <summary>
/// Queues a node to be notified
/// </summary>
/// <param name="node">The node to call the listeners for.</param>
public void QueueNotification(BusNode node)
{
if (!_updatedNodes.Contains(node))
{
//Debug.WriteLine("Queueing notification...");
_updatedNodes.Enqueue(node);
}
// add the node to the queue and start the notifier if neccessary.
if (_notification == null)
{
_notification = _dispatcher.BeginInvoke(DispatcherPriority.ApplicationIdle, new Action(NotifyNodes));
}
}
/// <summary>
/// Attempts to unsubscribe a value-changed listener from a node.
/// </summary>
/// <param name="node">The node to unsubscribe the listener from.</param>
/// <param name="callback">The listener to unsubscribe.</param>
/// <param name="strict">Whether to throw an exception if something goes wrong.</param>
/// <exception cref="ArgumentException">Thrown if operating in strict mode and the callback wasn't registered to the node.</exception>
public void Unsubscribe(BusNode node, OnValueChangedCallback callback, bool strict = true)
{
NodeEntry entry = null;
entry = _nodes[node];
lock (entry)
{
if (!entry.Subscribers.Remove(callback) && strict)
{
throw new ArgumentException("Unable to unsubscribe from node " + node + " as the delegate wasn't subscribed properly.");
}
}
}
/// <summary>
/// Attempts to subscribe a value-changed listener to a node.
/// </summary>
/// <typeparam name="T">The type of the object to attach to the node. Must be a serializable type.</typeparam>
/// <param name="node">The node to subscribe the listener to.</param>
/// <param name="callback">The delegate to call when a node has been changed.</param>
public void Subscribe(BusNode node, OnValueChangedCallback callback)
{
NodeEntry entry;
entry = _nodes[node];
lock (entry)
{
if (!entry.Subscribers.Contains(callback))
{
entry.Subscribers.Add(callback);
}
}
}
/// <summary>
/// Called when a value is published to the virtual bus.
/// </summary>
/// <param name="node">The node that has been published.</param>
/// <param name="value">The new value of the node.</param>
private void OnValuePublished(BusNode node, object value)
{
Orientation or = (Orientation)value;
if (node == BusNode.ORIENTATION_RIGHT_UPPER_ARM)
{
_upperArmDisplay.Text = OrientationToString(or);
}
else if (node == BusNode.ORIENTATION_RIGHT_LOWER_ARM)
{
_lowerArmDisplay.Text = OrientationToString(or);
}
else if (node == BusNode.ORIENTATION_RIGHT_HAND)
{
//_handDisplay.Text = OrientationToString(or);
_handDisplay.Text = OrientationToString(Bus.Get<Orientation>(BusNode.ORIENTATION_RIGHT_HAND));
}
}
/// <summary>
/// Calls any listeners for a node.
/// </summary>
/// <param name="node">The node to call the listeners for.</param>
private void Notify(BusNode node)
{
NodeEntry entry = null;
entry = _nodes[node];
// make a copy of the value.
object copy;
SerializeCopy(entry.Value, out copy);
lock (entry)
{
foreach (OnValueChangedCallback c in entry.Subscribers)
{
c(node, copy);
}
}
}
/// <summary>
/// This is called when a subscribed value is published on the virtual bus.
/// </summary>
/// <param name="node">The node that has been published.</param>
/// <param name="value">The new value of the node.</param>
private void OnValuePublished(BusNode node, object value)
{
int finalArmPosition = 0;
int finalForearmPosition = 0;
int finalWristPosition = 0;
int finalHandPosition = 0;
int finalShoulderPosition = 0;
// Let's limit the rate at which we send commands
// TODO: Do this right!
//if (System.DateTime.Now.Ticks < _lastUpdateTime + MIN_UPDATE_INTERVAL * System.TimeSpan.TicksPerMillisecond) return;
//_lastUpdateTime = System.DateTime.Now.Millisecond;
// What we want to do here is update our 'goal' position based on the data we can read from the virtual bus.
// Since we've subscribed to the POSITION_TICK, the 'value' parameter has no meaning, we should get the value directly from the bus.
Orientation arm = Bus.Get<Orientation>(BusNode.ORIENTATION_RIGHT_UPPER_ARM);
Orientation forearm = Bus.Get<Orientation>(BusNode.ORIENTATION_RIGHT_LOWER_ARM);
Orientation wrist = Bus.Get<Orientation>(BusNode.ORIENTATION_RIGHT_HAND);
int hand = Bus.Get<int>(BusNode.CLAW_OPEN_PERCENT);
bool armMoving = Bus.Get<bool>(BusNode.ROBOT_ACTIVE);
int wrist_hand = Bus.Get<int>(BusNode.WRIST_PERCENT);
// we should now convert these orientations to the values expected by the servo controller.
if (armMoving)
{
if (node == BusNode.POSITION_TICK)
{
// TODO: All the other joints
if (forearm != null)
{
//Debug.WriteLine("forearm: " + (forearm.Roll * 180) / Math.PI + " / " + (forearm.Pitch * 180) / Math.PI + " / " + (forearm.Yaw * 180) / Math.PI);
if (forearm.Pitch > ELBOW_PITCH_MAX_DEG * RAD_PER_DEG) forearm.Pitch = (float)(ELBOW_PITCH_MAX_DEG * RAD_PER_DEG);
if (forearm.Pitch < ELBOW_PITCH_MIN_DEG * RAD_PER_DEG) forearm.Pitch = (float)(ELBOW_PITCH_MIN_DEG * RAD_PER_DEG);
// scale to the valid range (1500 - 2200)
finalForearmPosition = 1500 + (int)((
(forearm.Pitch - ELBOW_PITCH_MIN_DEG * RAD_PER_DEG)
/ ((ELBOW_PITCH_MAX_DEG - ELBOW_PITCH_MIN_DEG) * RAD_PER_DEG)
) * 700);
// move the stuff!
// TODO: Do This correctly
if (Math.Abs(finalForearmPosition - currentForearmPosition) >= positionDeltaThreshold)
{
currentForearmPosition = finalForearmPosition;
move(ELBOW_JOINT, currentForearmPosition,200);
Debug.WriteLine("Got " + currentForearmPosition);
}
}
if (arm != null)
{
if (arm.Pitch > SHOULDER_PITCH_MAX_DEG * RAD_PER_DEG) arm.Pitch = (float)(SHOULDER_PITCH_MAX_DEG * RAD_PER_DEG);
if (arm.Pitch < SHOULDER_PITCH_MIN_DEG * RAD_PER_DEG) arm.Pitch = (float)(SHOULDER_PITCH_MIN_DEG * RAD_PER_DEG);
// scale to the valid range (800 - 2200)
if (arm.Pitch > 0)
{
finalArmPosition = 1500 - (int)((
(arm.Pitch /
(SHOULDER_PITCH_MAX_DEG * RAD_PER_DEG)
)
) * 700);
}
else
{
finalArmPosition = 1500 + (int)((
(Math.Abs(arm.Pitch) /
(Math.Abs(SHOULDER_PITCH_MIN_DEG) * RAD_PER_DEG)
)
) * 700);
}
// move the stuff!
// TODO: Do This correctly
if (Math.Abs(finalArmPosition - currentForearmPosition) >= positionDeltaThreshold)
{
currentArmPosition = finalArmPosition;
//move(SHOULDER_PITCH, currentArmPosition, 200);
}
}
if (arm != null)
{
if (arm.Yaw > SHOULDER_YAW_MAX_DEG * RAD_PER_DEG) arm.Yaw = (float)(SHOULDER_YAW_MAX_DEG * RAD_PER_DEG);
if (arm.Yaw < SHOULDER_YAW_MIN_DEG * RAD_PER_DEG) arm.Yaw = (float)(SHOULDER_YAW_MIN_DEG * RAD_PER_DEG);
// scale to the valid range (800 - 2200)
if (arm.Yaw > 0)
{
finalShoulderPosition = 1500 - (int)((
(arm.Yaw /
((SHOULDER_YAW_MAX_DEG - SHOULDER_YAW_MIN_DEG) * RAD_PER_DEG)
) //* (180 / ((float)Math.Abs(SHOULDER_PITC_MIN_DEG_PHYSICAL)))
) * 700);
}
else
{
finalShoulderPosition = 1500 + (int)((
(Math.Abs(arm.Yaw) /
((SHOULDER_YAW_MAX_DEG - SHOULDER_YAW_MIN_DEG) * RAD_PER_DEG)
) //* (180 / ((float)Math.Abs(SHOULDER_PITCH_MIN_DEG_PHYSICAL)))
) * 700);
}
// move the stuff!
// TODO: Do This correctly
if (Math.Abs(finalShoulderPosition - currentShoulderPosition) >= positionDeltaThreshold)
{
currentShoulderPosition = finalShoulderPosition;
//move(SHOULDER_YAW, finalShoulderPosition, 200);
}
}
}
if (node == BusNode.CLAW_OPEN_PERCENT)
{
// scale to the valid range (1000 - 2000)
finalHandPosition = 1500 + ((-hand + 50) * 10);
// move the stuff!
// TODO: Do This correctly
if (Math.Abs(finalHandPosition - currentHandPosition) >= positionDeltaThreshold)
{
currentHandPosition = finalHandPosition;
move(FINGERS, currentHandPosition, 400);
}
}
if (node == BusNode.WRIST_PERCENT)
{
finalWristPosition = 1380 + (int)((-wrist_hand + 50) * 12.4f);
if (Math.Abs(finalWristPosition - currentWristPosition) >= positionDeltaThreshold)
{
currentWristPosition = finalWristPosition;
move(WRIST_JOINT, currentWristPosition, 300);
}
}
}
}
/// <summary>
/// Initializes the node entry.
/// </summary>
/// <param name="node">The node the entry corresponds to.</param>
/// <param name="value">The initial value of the node.</param>
/// <param name="subscribers">The initial list of subscribers for the node.</param>
public NodeEntry(BusNode node, Object value, List <VirtualBus.OnValueChangedCallback> subscribers)
{
Node = node;
Value = value;
Subscribers = subscribers;
}
/// <summary>
/// Handles bus
/// </summary>
/// <param name="node"></param>
/// <param name="value"></param>
private void OnBusValueChanged(BusNode node, Object value)
{
if (node == BusNode.STOP_REQUESTED)
{
Debug.WriteLine("Caught a program stop request");
this.Close();
}
}
