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