private BadgerControlSubsystem() : base(GUI_SUBSYSTEM_ID, "BadgerControlSubsystem")
{
// setup event listeners
#region EVENT_LISTENERS
_eventAggregator = ApplicationService.Instance.EventAggregator;
_eventAggregator.GetEvent <UpdateIPEvent>().Subscribe((ip) =>
{
//CurrentRemoteIP = ip;
});
_eventAggregator.GetEvent <UpdateSubsystemIdEvent>().Subscribe((subsystem) =>
{
CurrentRemoteSubsystemID = subsystem;
});
_eventAggregator.GetEvent <UpdateDriveModeEvent>().Subscribe((drive) =>
{
//CurrentDriveComponent = drive;
});
#endregion
localJausAddress = new JausAddress(GUI_SUBSYSTEM_ID, GUI_NODE_ID, GUI_COMPONENT_ID);
badgerControlSubsystemInstance = this;
remoteVelocityStateDriverService = new RemoteVelocityStateDriverService(this);
remotePrimitiveDriverService = new RemotePrimitiveDriverService(this);
discoveredDriveModes = new Dictionary <long, DriveModes>();
observableDriveModes = new ObservableCollection <DriveModes>();
}
public virtual void updateNodeIDAction(GrantNodeID msg, Receive.Body.ReceiveRec transportData)
{
// Convert incoming MAC to string
StringBuilder sb = new StringBuilder();
for (int i = 0; i < 7; i++)
{
sb.Append(((byte)msg.getBody().getGrantNodeIDRec().getRequesterID().getRequesterIDArrayField(i)).ToString("x2"));
if (i < 6)
{
sb.Append(":");
}
}
Console.WriteLine("Got GrantNodeID message for " + sb);
// Check the MAC address to see if this is for us.
if (String.Compare(sb.ToString(), mac_string) == 0)
{
Console.WriteLine("Updating NodeID to " + msg.getBody().getGrantNodeIDRec().getNodeID() + " for MAC: " + sb);
JausAddress id = new JausAddress((ushort)jausRouter.getJausAddress().getSubsystemID(),
(byte)msg.getBody().getGrantNodeIDRec().getNodeID(),
(byte)jausRouter.getJausAddress().getComponentID());
jausRouter.updateJausID(id);
}
else
{
Console.WriteLine("Ignoring grant message since " + sb + " != " + mac_string);
}
}
public virtual void StoreAddressAction(Receive.Body.ReceiveRec transportData)
{
Console.WriteLine("Storing address of controlling component as " +
(ushort)transportData.getSourceID().getSubsystemID(),
(byte)transportData.getSourceID().getNodeID(),
(byte)transportData.getSourceID().getComponentID());
currentController = new JausAddress((ushort)transportData.getSourceID().getSubsystemID(),
(byte)transportData.getSourceID().getNodeID(),
(byte)transportData.getSourceID().getComponentID());
}
private void QueryIDTask(Object source, ElapsedEventArgs e)
{
// If we know our ID fully, start the dynamic service discovery process
if (!jausRouter.getJausAddress().containsWildcards())
{
QueryIdentification query_msg = new QueryIdentification();
query_msg.getBody().getQueryIdentificationRec().setQueryType(2);
JausAddress local = new JausAddress(jausRouter.getJausAddress().getSubsystemID(), 0xFF, 0xFF);
sendJausMessage(query_msg, local);
}
}
public void Connect(int remoteSubsystemID, IPEndPoint remoteAddress)
{
Subsystem remoteSubsystem = new Subsystem(remoteSubsystemID, remoteAddress);
remoteSubsystem.Identification = "UnknownSubsystem";
discoveryService.AddRemoteSubsystem(remoteSubsystemID, remoteSubsystem);
JausAddress remoteJausAddress = new JausAddress(remoteSubsystemID, 255, 255);
QueryIdentification queryIdentification = new QueryIdentification();
queryIdentification.SetDestination(remoteJausAddress);
queryIdentification.SetSource(localJausAddress);
Transport.SendMessage(queryIdentification);
_eventAggregator.GetEvent <LoggerEvent>().Publish("Attempting to connect...");
}
public virtual bool isControllingClient(Receive.Body.ReceiveRec transportData)
{
JausAddress requester = new JausAddress((ushort)transportData.getSourceID().getSubsystemID(),
(byte)transportData.getSourceID().getNodeID(),
(byte)transportData.getSourceID().getComponentID());
if ((currentController != null) &&
(requester.getSubsystemID() == currentController.getSubsystemID() &&
requester.getNodeID() == currentController.getNodeID() &&
requester.getComponentID() == currentController.getComponentID()))
{
return(true);
}
return(false);
}
public virtual void SendAction(string arg0, Receive.Body.ReceiveRec transportData)
{
if (arg0.CompareTo("ReportStatus") == 0)
{
ReportStatus status_msg = new ReportStatus();
// Get the state from the context. Note that since we are inside a transition, the "current state"
// is ill-defined. We instead use the state we left to execute this transition. Recall that the state
// is actually an amalgamation of all parent states, we're only concerned with the management
string currentState = context.PreviousState.Name; //getPreviousState().getName();
if (currentState.Contains("_"))
{
currentState = currentState.Substring(currentState.LastIndexOf("_") + 1);
}
if (currentState.CompareTo("Init") == 0)
{
status_msg.getBody().getReportStatusRec().setStatus(0);
}
if (currentState.CompareTo("Ready") == 0)
{
status_msg.getBody().getReportStatusRec().setStatus(1);
}
if (currentState.CompareTo("Standby") == 0)
{
status_msg.getBody().getReportStatusRec().setStatus(2);
}
if (currentState.CompareTo("Shutdown") == 0)
{
status_msg.getBody().getReportStatusRec().setStatus(3);
}
if (currentState.CompareTo("Failure") == 0)
{
status_msg.getBody().getReportStatusRec().setStatus(4);
}
if (currentState.CompareTo("Emergency") == 0)
{
status_msg.getBody().getReportStatusRec().setStatus(5);
}
Console.WriteLine("Sending ReportStatus: " + currentState);
// Now send it to the requesting component
JausAddress sender = new JausAddress((ushort)transportData.getSourceID().getSubsystemID(),
(byte)transportData.getSourceID().getNodeID(),
(byte)transportData.getSourceID().getComponentID());
sendJausMessage(status_msg, sender);
}
}
public void OnActivate(object arg)
{
JausAddress targetAddress = GenerateCurrentAddress();
if (targetAddress == null)
{
return;
}
Resume resume = new Resume();
resume.SetSource(badgerControlSubsystem.LocalAddress);
resume.SetDestination(targetAddress);
Transport.SendMessage(resume);
}
public void OnDeactivate(object arg)
{
JausAddress targetAddress = GenerateCurrentAddress();
if (targetAddress == null)
{
return;
}
Shutdown shutdown = new Shutdown();
shutdown.SetSource(badgerControlSubsystem.LocalAddress);
shutdown.SetDestination(targetAddress);
Transport.SendMessage(shutdown);
}
public void OnReleaseControl(object arg)
{
JausAddress targetAddress = GenerateCurrentAddress();
if (targetAddress == null)
{
return;
}
ReleaseControl releaseControl = new ReleaseControl();
releaseControl.SetSource(badgerControlSubsystem.LocalAddress);
releaseControl.SetDestination(targetAddress);
Transport.SendMessage(releaseControl);
}
public void OnRequestControl(object arg)
{
JausAddress targetAddress = GenerateCurrentAddress();
if (targetAddress == null)
{
return;
}
RequestControl requestControl = new RequestControl();
requestControl.SetAuthorityCode(BadgerControlSubsystem.AUTHORITY_LEVEL);
requestControl.SetSource(badgerControlSubsystem.LocalAddress);
requestControl.SetDestination(targetAddress);
Transport.SendMessage(requestControl);
}
private void AllocatorTask(Object source, ElapsedEventArgs e)
{
// We first have to know the subsystem ID before we can start asking for a node ID
// If that's not set, return prematurely.
if ((jausRouter.getJausAddress().getSubsystemID() == 0xFFFF) ||
(jausRouter.getJausAddress().getSubsystemID() == 0))
{
return;
}
// Check to see if we have an ID. If we do, there's nothing to do
if (!jausRouter.getJausAddress().containsWildcards())
{
// Nothing to do here... we just fall through the rest of the function
}
else if (dynamic_iop_tries_so_far++ > MaxNumberOfDynamicTries)
{
// If we've exceeded the max number of tries, use the fallback
Console.WriteLine("Failed to obtain Node ID dynamically. Using fallback to assign node ID to default offset...\n");
JausAddress id = new JausAddress(jausRouter.getJausAddress().getSubsystemID(),
0x6F, // type + active offset
jausRouter.getJausAddress().getComponentID());
jausRouter.updateJausID(id);
}
else
{
// Broadcast a RequestNodeID message. Note that an actual implementation would have to get or know the MAC
Console.WriteLine("Sending RequestNodeID message since ID=" + jausRouter.getJausAddress().getNodeID());
RequestNodeID request_id = new RequestNodeID();
request_id.getBody().getReviewNodeIDRec().getRequesterID().setRequesterIDArrayField(6, 0x03);
request_id.getBody().getReviewNodeIDRec().getRequesterID().setRequesterIDArrayField(0, 0xDE);
request_id.getBody().getReviewNodeIDRec().getRequesterID().setRequesterIDArrayField(1, 0xAD);
request_id.getBody().getReviewNodeIDRec().getRequesterID().setRequesterIDArrayField(2, 0xBE);
request_id.getBody().getReviewNodeIDRec().getRequesterID().setRequesterIDArrayField(3, 0xEF);
request_id.getBody().getReviewNodeIDRec().getRequesterID().setRequesterIDArrayField(4, 0xCA);
request_id.getBody().getReviewNodeIDRec().getRequesterID().setRequesterIDArrayField(5, 0xFE);
JausAddress localBroadcast = new JausAddress(
jausRouter.getJausAddress().getSubsystemID(), 0xFF, 0xFF);
// Use the optional 'force' flag to make the transport layer send this message,
// even if the local JAUS ID may not be fully known.
sendJausMessage(request_id, localBroadcast, true);
}
}
public virtual void updateSubsystemIDAction(Receive.Body.ReceiveRec transportData)
{
// Check to see if we already have a subsystem ID. If so, nothing to do.
if ((jausRouter.getJausAddress().getSubsystemID() != 0xFFFF) && (jausRouter.getJausAddress().getSubsystemID() != 0))
{
return;
}
// We need to update the JAUS ID
Console.WriteLine("DiscoveryClient:: update subsystem ID to " + transportData.getSourceID().getSubsystemID());
JausAddress id = new JausAddress((ushort)transportData.getSourceID().getSubsystemID(),
(byte)jausRouter.getJausAddress().getNodeID(),
(byte)jausRouter.getJausAddress().getComponentID());
// We now know (and can set) the subsystem ID, but we might still not know the Node ID.
// Force the lower layers to accept that the ID might still contain wildcards using the
// optional boolean argument
jausRouter.updateJausID(id, true);
}
public virtual void sendRequestControlAction()
{
// what should really happen here is that an internal event is called, so that
// the send happens inside of a sendAction(), rather than here.
// Now, we need to send a RequestComponentControl to the management service
RequestControl request_control_msg = new RequestControl();
request_control_msg.getBody().getRequestControlRec().setAuthorityCode(200);
// send a message, to this subsystem, this node, component 11.
// Again, discovery should be used to find the JAUS ID of the service we want to
// address, rather than hardcoding the address
JausAddress dest = new JausAddress(jausRouter.getJausAddress().getSubsystemID(),
jausRouter.getJausAddress().getNodeID(),
(byte)160);
sendJausMessage(request_control_msg, dest);
}
private JausAddress GenerateCurrentAddress()
{
JausAddress targetAddress;
if (selectedSubsystem == null)
{
return(null);
}
targetAddress = new JausAddress(selectedSubsystem.SubsystemID, 255, 255);
if (selectedNode != null)
{
targetAddress.setNode(selectedNode.NodeID);
}
if (selectedComponent != null)
{
targetAddress.setComponent(selectedComponent.ComponentID);
}
return(targetAddress);
}
public virtual void registerServicesAction(Receive.Body.ReceiveRec transportData)
{
// Extract the sender information
JausAddress sender = new JausAddress((ushort)transportData.getSourceID().getSubsystemID(),
(byte)transportData.getSourceID().getNodeID(),
(byte)transportData.getSourceID().getComponentID());
// Found a discovery service. Register our stuffs with it.
RegisterServices register_msg = new RegisterServices();
RegisterServices.RegisterServicesBody.ServiceList.ServiceRec service =
new RegisterServices.RegisterServicesBody.ServiceList.ServiceRec();
service.setMinorVersionNumber(1);
service.setMajorVersionNumber(1);
// Register local services
service.setURI("urn:jaus:jss:core:Transport");
register_msg.getRegisterServicesBody().getServiceList().addElement(service);
// Send the actual message
sendJausMessage(register_msg, sender);
}
public virtual void serviceInitializedAction()
{
Console.WriteLine("In Ready state. Let's start adding...");
// This is the basic message type for our query.
QueryAddition query = new QueryAddition();
// The message contains a body, with a record.
query.getAdditionInputBody().getAdditionInput().setA1(500);
query.getAdditionInputBody().getAdditionInput().setA2(500);
// Send the response to the server on this subsystem and node. The
// Component ID is fixed at 150.
JausAddress server = new JausAddress(
jausRouter.getJausAddress().getSubsystemID(),
jausRouter.getJausAddress().getNodeID(),
150);
// Encode the request and send it to the server.
sendJausMessage(query, server);
Console.WriteLine("Send addition request");
}
protected override void Execute(Component component)
{
LocateJoystick();
if (!isEnabled || destinationAddress == null)
return;
// request control and update the GUI to denote this
if (!hasControl)
{
RequestControl requestControl = new RequestControl();
requestControl.SetDestination(destinationAddress);
requestControl.SetSource(component.JausAddress);
Transport.SendMessage(requestControl);
// update the connection icon for the correct component to orange
if (destinationAddress.getComponent() == 1)
connectionDetails.direct = ConnectionOption.REQUESTING_CONTROL;
if (destinationAddress.getComponent() == 2)
connectionDetails.remote = ConnectionOption.REQUESTING_CONTROL;
if (destinationAddress.getComponent() == 3)
connectionDetails.ai = ConnectionOption.REQUESTING_CONTROL;
_eventAggregator.GetEvent<ConnectionDetailsEvent>().Publish(connectionDetails);
return;
}
// shut down all components, then resume the one that should be active (i.e. jank hax)
if (!isReady)
{
// shut down all active components
Shutdown shutdown = new Shutdown();
int subsys = CurrentDestinationAddress.SubsystemID;
int node = CurrentDestinationAddress.getNode();
JausAddress newAddress = new JausAddress(subsys, node, 1);
QueryStatus queryStatus = new QueryStatus();
if (componentOneActive)
{
shutdown.SetDestination(newAddress);
Transport.SendMessage(shutdown);
queryStatus.SetDestination(newAddress);
queryStatus.SetSource(component.JausAddress);
Transport.SendMessage(queryStatus);
}
if (componentTwoActive)
{
newAddress.setComponent(2);
shutdown.SetDestination(newAddress);
Transport.SendMessage(shutdown);
queryStatus.SetDestination(newAddress);
queryStatus.SetSource(component.JausAddress);
Transport.SendMessage(queryStatus);
}
if (componentThreeActive)
{
newAddress.setComponent(3);
shutdown.SetDestination(newAddress);
Transport.SendMessage(shutdown);
queryStatus = new QueryStatus();
queryStatus.SetDestination(newAddress);
queryStatus.SetSource(component.JausAddress);
Transport.SendMessage(queryStatus);
}
// force component to boot
Resume resume = new Resume();
resume.SetDestination(destinationAddress);
resume.SetSource(component.JausAddress);
Transport.SendMessage(resume);
// see if the component is ready
queryStatus = new QueryStatus();
queryStatus.SetDestination(destinationAddress);
queryStatus.SetSource(component.JausAddress);
Transport.SendMessage(queryStatus);
return;
}
// send a drive message
SetLocalVector msg = new SetLocalVector();
msg.SetDestination(destinationAddress);
msg.SetSource(component.JausAddress);
// convert joystick degrees into radians
// TODO: THIS IS INCORRECT
msg.SetHeading(joystickQueryThread.XVelocity * (Math.PI / 180));
//adding 100 to fit into defined setLocalVector MAX_SPEED & MIN_SPEED
msg.SetSpeed(joystickQueryThread.YVelocity + 100);
Transport.SendMessage(msg);
}
public virtual void sendManagementMessageAction(MenuItemEntered msg)
{
Resume resume_msg = new Resume();
Standby standby_msg = new Standby();
SetEmergency set_emergency_msg = new SetEmergency();
ClearEmergency clear_emergency_msg = new ClearEmergency();
QueryStatus query_status_msg = new QueryStatus();
ReleaseControl release_control_msg = new ReleaseControl();
Shutdown shutdown_msg = new Shutdown();
JausAddress dest = new JausAddress(jausRouter.getJausAddress().getSubsystemID(),
jausRouter.getJausAddress().getNodeID(),
(byte)160);
switch (msg.getMenuItemEnteredBody().getMenuItemEnteredRecord().getSelection())
{
case 1:
Console.WriteLine("Sending Resume");
sendJausMessage(resume_msg, dest);
break;
case 2:
Console.WriteLine("Sending Standby");
sendJausMessage(standby_msg, dest);
break;
case 3:
Console.WriteLine("Sending SetEmergency");
sendJausMessage(set_emergency_msg, dest);
break;
case 4:
Console.WriteLine("Sending ClearEmergency");
sendJausMessage(clear_emergency_msg, dest);
break;
case 5:
Console.WriteLine("Sending QueryStatus");
sendJausMessage(query_status_msg, dest);
break;
case 6:
Console.WriteLine("Sending Shutdown");
sendJausMessage(shutdown_msg, dest);
break;
case 7:
Console.WriteLine("Sending ReleaseControl");
sendJausMessage(release_control_msg, dest);
break;
case 8:
Console.WriteLine("Sending Terminate");
terminateServiceAction();
break;
default:
Console.WriteLine("Unknown input: " + msg.getMenuItemEnteredBody().getMenuItemEnteredRecord().getSelection());
break;
}
}
public virtual void SendAction(string arg0, string arg1, Receive.Body.ReceiveRec transportData)
{
if (arg0.CompareTo("RejectControl") == 0)
{
RejectControl reject_msg = new RejectControl();
if (arg1.CompareTo("CONTROL_RELEASED") == 0)
{
reject_msg.getBody().getRejectControlRec().setResponseCode(0);
}
if (arg1.CompareTo("NOT_ACCEPTED") == 0)
{
reject_msg.getBody().getRejectControlRec().setResponseCode(1);
}
// Now send it to the requesting component
JausAddress sender = new JausAddress((ushort)transportData.getSourceID().getSubsystemID(),
(byte)transportData.getSourceID().getNodeID(),
(byte)transportData.getSourceID().getComponentID());
sendJausMessage(reject_msg, sender);
}
else if (arg0.CompareTo("ConfirmControl") == 0)
{
ConfirmControl confirm_msg = new ConfirmControl();
byte responseCode = 0;
if (arg1.CompareTo("CONTROL_ACCEPTED") == 0)
{
responseCode = 0;
}
if (arg1.CompareTo("NOT_AVAILABLE") == 0)
{
responseCode = 1;
}
if (arg1.CompareTo("INSUFFICIENT_AUTHORITY") == 0)
{
responseCode = 2;
}
Console.WriteLine("Sending ConfirmControl with argument " + arg1);
confirm_msg.getBody().getConfirmControlRec().setResponseCode(responseCode);
// Now send it to the requesting component
JausAddress sender = new JausAddress((ushort)transportData.getSourceID().getSubsystemID(),
(byte)transportData.getSourceID().getNodeID(),
(byte)transportData.getSourceID().getComponentID());
sendJausMessage(confirm_msg, sender);
}
else if (arg0.CompareTo("ReportControl") == 0)
{
ReportControl control_msg = new ReportControl();
control_msg.getBody().getReportControlRec().setSubsystemID((ushort)(
(currentController == null) ? 0 : currentController.getSubsystemID()));
control_msg.getBody().getReportControlRec().setNodeID((byte)(
(currentController == null) ? 0 : currentController.getNodeID()));
control_msg.getBody().getReportControlRec().setComponentID((byte)(
(currentController == null) ? 0 : currentController.getComponentID()));
// Now send it to the requesting component
JausAddress sender = new JausAddress((ushort)transportData.getSourceID().getSubsystemID(),
(byte)transportData.getSourceID().getNodeID(),
(byte)transportData.getSourceID().getComponentID());
sendJausMessage(control_msg, sender);
}
}
private JausAddress GenerateCurrentAddress()
{
JausAddress targetAddress;
if (selectedSubsystem == null)
return null;
targetAddress = new JausAddress(selectedSubsystem.SubsystemID, 255, 255);
if (selectedNode != null)
{
targetAddress.setNode(selectedNode.NodeID);
}
if (selectedComponent != null)
{
targetAddress.setComponent(selectedComponent.ComponentID);
}
return targetAddress;
}
public void Connect(int remoteSubsystemID, IPEndPoint remoteAddress)
{
Subsystem remoteSubsystem = new Subsystem(remoteSubsystemID, remoteAddress);
remoteSubsystem.Identification = "UnknownSubsystem";
discoveryService.AddRemoteSubsystem(remoteSubsystemID, remoteSubsystem);
JausAddress remoteJausAddress = new JausAddress(remoteSubsystemID, 255, 255);
QueryIdentification queryIdentification = new QueryIdentification();
queryIdentification.SetDestination(remoteJausAddress);
queryIdentification.SetSource(localJausAddress);
Transport.SendMessage(queryIdentification);
_eventAggregator.GetEvent<LoggerEvent>().Publish("Attempting to connect...");
}
private BadgerControlSubsystem()
: base(GUI_SUBSYSTEM_ID, "BadgerControlSubsystem")
{
// setup event listeners
#region EVENT_LISTENERS
_eventAggregator = ApplicationService.Instance.EventAggregator;
_eventAggregator.GetEvent<UpdateIPEvent>().Subscribe((ip) =>
{
//CurrentRemoteIP = ip;
});
_eventAggregator.GetEvent<UpdateSubsystemIdEvent>().Subscribe((subsystem) =>
{
CurrentRemoteSubsystemID = subsystem;
});
_eventAggregator.GetEvent<UpdateDriveModeEvent>().Subscribe((drive) =>
{
//CurrentDriveComponent = drive;
});
#endregion
localJausAddress = new JausAddress(GUI_SUBSYSTEM_ID, GUI_NODE_ID, GUI_COMPONENT_ID);
badgerControlSubsystemInstance = this;
remoteVelocityStateDriverService = new RemoteVelocityStateDriverService(this);
remotePrimitiveDriverService = new RemotePrimitiveDriverService(this);
discoveredDriveModes = new Dictionary<long, DriveModes>();
observableDriveModes = new ObservableCollection<DriveModes>();
}
protected override void Execute(Component component)
{
LocateJoystick();
if (!isEnabled || destinationAddress == null)
{
return;
}
// request control and update the GUI to denote this
if (!hasControl)
{
RequestControl requestControl = new RequestControl();
requestControl.SetDestination(destinationAddress);
requestControl.SetSource(component.JausAddress);
Transport.SendMessage(requestControl);
// update the connection icon for the correct component to orange
if (destinationAddress.getComponent() == 1)
{
connectionDetails.direct = ConnectionOption.REQUESTING_CONTROL;
}
if (destinationAddress.getComponent() == 2)
{
connectionDetails.remote = ConnectionOption.REQUESTING_CONTROL;
}
if (destinationAddress.getComponent() == 3)
{
connectionDetails.ai = ConnectionOption.REQUESTING_CONTROL;
}
_eventAggregator.GetEvent <ConnectionDetailsEvent>().Publish(connectionDetails);
return;
}
// shut down all components, then resume the one that should be active (i.e. jank hax)
if (!isReady)
{
// shut down all active components
Shutdown shutdown = new Shutdown();
int subsys = CurrentDestinationAddress.SubsystemID;
int node = CurrentDestinationAddress.getNode();
JausAddress newAddress = new JausAddress(subsys, node, 1);
QueryStatus queryStatus = new QueryStatus();
if (componentOneActive)
{
shutdown.SetDestination(newAddress);
Transport.SendMessage(shutdown);
queryStatus.SetDestination(newAddress);
queryStatus.SetSource(component.JausAddress);
Transport.SendMessage(queryStatus);
}
if (componentTwoActive)
{
newAddress.setComponent(2);
shutdown.SetDestination(newAddress);
Transport.SendMessage(shutdown);
queryStatus.SetDestination(newAddress);
queryStatus.SetSource(component.JausAddress);
Transport.SendMessage(queryStatus);
}
if (componentThreeActive)
{
newAddress.setComponent(3);
shutdown.SetDestination(newAddress);
Transport.SendMessage(shutdown);
queryStatus = new QueryStatus();
queryStatus.SetDestination(newAddress);
queryStatus.SetSource(component.JausAddress);
Transport.SendMessage(queryStatus);
}
// force component to boot
Resume resume = new Resume();
resume.SetDestination(destinationAddress);
resume.SetSource(component.JausAddress);
Transport.SendMessage(resume);
// see if the component is ready
queryStatus = new QueryStatus();
queryStatus.SetDestination(destinationAddress);
queryStatus.SetSource(component.JausAddress);
Transport.SendMessage(queryStatus);
return;
}
// send a drive message
SetLocalVector msg = new SetLocalVector();
msg.SetDestination(destinationAddress);
msg.SetSource(component.JausAddress);
// convert joystick degrees into radians
// TODO: THIS IS INCORRECT
msg.SetHeading(joystickQueryThread.XVelocity * (Math.PI / 180));
//adding 100 to fit into defined setLocalVector MAX_SPEED & MIN_SPEED
msg.SetSpeed(joystickQueryThread.YVelocity + 100);
Transport.SendMessage(msg);
}
