public BadgerControlStatusView() { InitializeComponent(); joystickQueryThread = null; CompositionTarget.Rendering += OnRender; _eventAggregator = ApplicationService.Instance.EventAggregator; _eventAggregator.GetEvent<DeliverJoystickEvent>().Subscribe((joystick) => { // send a confirmation back when we receive a joystick joystickQueryThread = joystick; _eventAggregator.GetEvent<ConfirmJoystickEvent>().Publish(JOYSTICK_ID); }); _eventAggregator.GetEvent<ConnectionDetailsEvent>().Subscribe((connection) => { UpdateConnection(connection); }); // create a ConnectionDetails to keep around. its fields will be used to help organize which // components have which connection statuses ConnectionDetails connectionDetails = new ConnectionDetails(); connectionDetails.ai = ConnectionOption.DISCONNECTED; connectionDetails.remote = ConnectionOption.DISCONNECTED; connectionDetails.direct = ConnectionOption.DISCONNECTED; UpdateConnection(connectionDetails); }
public BadgerControlStatusView() { InitializeComponent(); joystickQueryThread = null; CompositionTarget.Rendering += OnRender; _eventAggregator = ApplicationService.Instance.EventAggregator; _eventAggregator.GetEvent <DeliverJoystickEvent>().Subscribe((joystick) => { // send a confirmation back when we receive a joystick joystickQueryThread = joystick; _eventAggregator.GetEvent <ConfirmJoystickEvent>().Publish(JOYSTICK_ID); }); _eventAggregator.GetEvent <ConnectionDetailsEvent>().Subscribe((connection) => { UpdateConnection(connection); }); // create a ConnectionDetails to keep around. its fields will be used to help organize which // components have which connection statuses ConnectionDetails connectionDetails = new ConnectionDetails(); connectionDetails.ai = ConnectionOption.DISCONNECTED; connectionDetails.remote = ConnectionOption.DISCONNECTED; connectionDetails.direct = ConnectionOption.DISCONNECTED; UpdateConnection(connectionDetails); }
public BadgerControlVisualViewModel(IEventAggregator eventAggregator) { joystick = new JoystickQueryThread(); Thread joystickThread = new Thread(joystick.QueryJoystick); joystickThread.Start(); this._eventAggregator = eventAggregator; }
public BadgerControlVisualView() { InitializeComponent(); joystickQueryThread = null; CompositionTarget.Rendering += OnRender; _eventAggregator = ApplicationService.Instance.EventAggregator; _eventAggregator.GetEvent<DeliverJoystickEvent>().Subscribe((joystick) => { // send a confirmation back when we receive a joystick joystickQueryThread = joystick; _eventAggregator.GetEvent<ConfirmJoystickEvent>().Publish(JOYSTICK_ID); }); }
public BadgerControlVisualView() { InitializeComponent(); joystickQueryThread = null; CompositionTarget.Rendering += OnRender; _eventAggregator = ApplicationService.Instance.EventAggregator; _eventAggregator.GetEvent <DeliverJoystickEvent>().Subscribe((joystick) => { // send a confirmation back when we receive a joystick joystickQueryThread = joystick; _eventAggregator.GetEvent <ConfirmJoystickEvent>().Publish(JOYSTICK_ID); }); }
public void LocateJoystick() { // if we have the instance and all parties have confrmed, return if (JoyStickConfirmed && joystickQueryThread != null) { return; } if (joystickQueryThread == null) { // reset these because we have no joystick joystickConfirmedFromStatusView = false; joystickConfirmedFromVisualView = false; // try to create a joystick try { joystickQueryThread = new JoystickQueryThread(2); joystickQueryThread.Start(); } catch (Exception) { // TODO: make this work. this event is activated before the subscriber is attached if (!triedJoystick) { triedJoystick = true; _eventAggregator.GetEvent <LoggerEvent>().Publish("Joystick Not found."); } } } // deliver if we actually produced a joystick if (joystickQueryThread != null) { _eventAggregator.GetEvent <DeliverJoystickEvent>().Publish(joystickQueryThread); } }
private void HandleQuasi() { long primaryXVelocity, primaryYVelocity, primaryZRotation; long secondaryXVelocity, secondaryYVelocity, secondaryZRotation; bool[] primaryButtons, secondaryButtons; int primaryJoystickID = JoystickQueryThread.PRIMARY_JOYSTICK_UNASSIGNED; int secondaryJoystickID = JoystickQueryThread.PRIMARY_JOYSTICK_UNASSIGNED; bool success; double turntableSpeed = 0; double shoulderSpeed = 0; double elbowSpeed = 0; double wristLinearSpeed = 0; double clawRotateSpeed = 0; double clawGripSpeed = 0; if (joystickQuery == null) { lock (initSetup) // indetermined behavior when multiple JoystickQueryThreads get created - ensure only one gets created { if (joystickQuery == null) { joystickQuery = new JoystickQueryThread(2); joystickQuery.JoystickDisconnected += OnJoystickDisconnected; Thread joystickThread = new Thread(joystickQuery.QueryJoystick); joystickThread.Start(); } } } if (joystickDisconnected) { bool isButtonPressed; List<int> ids = joystickQuery.GetJoystickIDs(); foreach (int id in ids) { if (!joystickQuery.GetButton(id, JoystickButton.Trigger, out isButtonPressed)) continue; if (isButtonPressed) { joystickDisconnected = false; break; } } } int numJoysticks = 0; if (joystickQuery != null) numJoysticks = joystickQuery.GetJoystickIDs().Count; if (numJoysticks == 2) { if (leftJoystickId == 0 || rightJoystickId == 0) { if (joystickQuery.PrimaryIDAssigned()) { rightJoystickId = joystickQuery.GetPrimaryID(); leftJoystickId = joystickQuery.GetSecondaryID(); } } success = true; success &= joystickQuery.GetButtons(rightJoystickId, out primaryButtons); success &= joystickQuery.GetButtons(leftJoystickId, out secondaryButtons); success &= joystickQuery.GetXVelocity(rightJoystickId, out primaryXVelocity); success &= joystickQuery.GetYVelocity(rightJoystickId, out primaryYVelocity); success &= joystickQuery.GetZRotation(rightJoystickId, out primaryZRotation); success &= joystickQuery.GetXVelocity(leftJoystickId, out secondaryXVelocity); success &= joystickQuery.GetYVelocity(leftJoystickId, out secondaryYVelocity); success &= joystickQuery.GetZRotation(leftJoystickId, out secondaryZRotation); if (!success) { return; } RemotePrimitiveDriverService remotePimitiveDriverService = badgerControlSubsystem.CurrentDriveMode.remoteDriveService as RemotePrimitiveDriverService; if (remotePimitiveDriverService != null) { // SendWrenchCommand(long primaryXJoystickValue, long primaryYJoystickValue, long primaryZJoystickValue, long secondaryXJoystickValue, long secondaryYJoystickValue, long secondaryZJoystickValue, Component parentComponent); // sign gives direction, value gives magnitude (speed) -> [turntable, shoulder, elbow, wrist, claw rotation, claw grip] // Get speed if (primaryButtons[(int)JoystickButton.Button7]) { multiplier = SPEED6; speed = SPEED6; _eventAggregator.GetEvent<LoggerEvent>().Publish("Speed = 6"); } else if (primaryButtons[(int)JoystickButton.Button8]) { multiplier = SPEED5; speed = SPEED5; _eventAggregator.GetEvent<LoggerEvent>().Publish("Speed = 5"); } else if (primaryButtons[(int)JoystickButton.Button9]) { multiplier = SPEED4; speed = SPEED4; _eventAggregator.GetEvent<LoggerEvent>().Publish("Speed = 4"); } else if (primaryButtons[(int)JoystickButton.Button10]) { multiplier = SPEED3; speed = SPEED3; _eventAggregator.GetEvent<LoggerEvent>().Publish("Speed = 3"); } else if (primaryButtons[(int)JoystickButton.Button11]) { multiplier = SPEED2; speed = SPEED2; _eventAggregator.GetEvent<LoggerEvent>().Publish("Speed = 2"); } else if (primaryButtons[(int)JoystickButton.Button12]) { multiplier = SPEED1; speed = SPEED1; _eventAggregator.GetEvent<LoggerEvent>().Publish("Speed = 1"); } // Rotate or Non Rotate Mode if (secondaryButtons[(int)JoystickButton.Button2]) { if (++rotateButtonCount == rotateButtonDebounce) { rotateMode = !rotateMode; if (rotateMode) _eventAggregator.GetEvent<LoggerEvent>().Publish("Rotate Mode"); else _eventAggregator.GetEvent<LoggerEvent>().Publish("Non Rotate Mode"); } } else { rotateButtonCount = 0; rotateButtonToggled = false; } // Turntable [0] and claw rotate [4] if (rotateMode) { // Claw rotation if (primaryButtons[(int)JoystickButton.Button4]) clawRotateSpeed = speed; else if (primaryButtons[(int)JoystickButton.Button3]) clawRotateSpeed = -1 * speed; else clawRotateSpeed = 0; // Turntable if (secondaryButtons[(int)JoystickButton.Button4]) turntableSpeed = speed; else if (secondaryButtons[(int)JoystickButton.Button3]) turntableSpeed = -1 * speed; else turntableSpeed = 0; // All others shoulderSpeed = 0; elbowSpeed = 0; wristLinearSpeed = 0; clawGripSpeed = 0; } // Shoulder [1], elbow [2], wrist [3], claw grip [5] else { // Elbow if (primaryButtons[(int)JoystickButton.Button5]) elbowSpeed = -1 * speed; else if (primaryButtons[(int)JoystickButton.Button3]) elbowSpeed = speed; else elbowSpeed = 0; // Shoulder if (secondaryButtons[(int)JoystickButton.Button5]) shoulderSpeed = -1 * speed; else if (secondaryButtons[(int)JoystickButton.Button3]) shoulderSpeed = speed; else shoulderSpeed = 0; // Wrist if (secondaryButtons[(int)JoystickButton.Button6]) wristLinearSpeed = -1 * speed; else if (secondaryButtons[(int)JoystickButton.Button4]) wristLinearSpeed = speed; else wristLinearSpeed = 0; // Claw grip if (primaryButtons[(int)JoystickButton.Button6]) clawGripSpeed = speed; else if (primaryButtons[(int)JoystickButton.Button4]) clawGripSpeed = -1 * speed; else clawGripSpeed = 0; // All others turntableSpeed = 0; clawRotateSpeed = 0; } badgerControlSubsystem.CurrentDriveMode.SendWrenchCommandQuasi((long)turntableSpeed, (long)shoulderSpeed, (long)elbowSpeed, (long)wristLinearSpeed, (long)clawRotateSpeed, (long)clawGripSpeed); } else { primaryZRotation = 0; // temporary stopgap for e-stop if (primaryButtons[(int)JoystickButton.Button2]) { primaryYVelocity = 0; secondaryYVelocity = 0; primaryZRotation = 0; } if (badgerControlSubsystem.CurrentDriveMode != null) badgerControlSubsystem.CurrentDriveMode.SendDriveCommand(secondaryYVelocity, primaryYVelocity, primaryZRotation); } } else { // ? } }
private void HandleQuasi() { long primaryXVelocity, primaryYVelocity, primaryZRotation; long secondaryXVelocity, secondaryYVelocity, secondaryZRotation; bool[] primaryButtons, secondaryButtons; int primaryJoystickID = JoystickQueryThread.PRIMARY_JOYSTICK_UNASSIGNED; int secondaryJoystickID = JoystickQueryThread.PRIMARY_JOYSTICK_UNASSIGNED; bool success; double turntableSpeed = 0; double shoulderSpeed = 0; double elbowSpeed = 0; double wristLinearSpeed = 0; double clawRotateSpeed = 0; double clawGripSpeed = 0; if (joystickQuery == null) { lock (initSetup) // indetermined behavior when multiple JoystickQueryThreads get created - ensure only one gets created { if (joystickQuery == null) { joystickQuery = new JoystickQueryThread(2); joystickQuery.JoystickDisconnected += OnJoystickDisconnected; Thread joystickThread = new Thread(joystickQuery.QueryJoystick); joystickThread.Start(); } } } if (joystickDisconnected) { bool isButtonPressed; List <int> ids = joystickQuery.GetJoystickIDs(); foreach (int id in ids) { if (!joystickQuery.GetButton(id, JoystickButton.Trigger, out isButtonPressed)) { continue; } if (isButtonPressed) { joystickDisconnected = false; break; } } } int numJoysticks = 0; if (joystickQuery != null) { numJoysticks = joystickQuery.GetJoystickIDs().Count; } if (numJoysticks == 2) { if (leftJoystickId == 0 || rightJoystickId == 0) { if (joystickQuery.PrimaryIDAssigned()) { rightJoystickId = joystickQuery.GetPrimaryID(); leftJoystickId = joystickQuery.GetSecondaryID(); } } success = true; success &= joystickQuery.GetButtons(rightJoystickId, out primaryButtons); success &= joystickQuery.GetButtons(leftJoystickId, out secondaryButtons); success &= joystickQuery.GetXVelocity(rightJoystickId, out primaryXVelocity); success &= joystickQuery.GetYVelocity(rightJoystickId, out primaryYVelocity); success &= joystickQuery.GetZRotation(rightJoystickId, out primaryZRotation); success &= joystickQuery.GetXVelocity(leftJoystickId, out secondaryXVelocity); success &= joystickQuery.GetYVelocity(leftJoystickId, out secondaryYVelocity); success &= joystickQuery.GetZRotation(leftJoystickId, out secondaryZRotation); if (!success) { return; } RemotePrimitiveDriverService remotePimitiveDriverService = badgerControlSubsystem.CurrentDriveMode.remoteDriveService as RemotePrimitiveDriverService; if (remotePimitiveDriverService != null) { // SendWrenchCommand(long primaryXJoystickValue, long primaryYJoystickValue, long primaryZJoystickValue, long secondaryXJoystickValue, long secondaryYJoystickValue, long secondaryZJoystickValue, Component parentComponent); // sign gives direction, value gives magnitude (speed) -> [turntable, shoulder, elbow, wrist, claw rotation, claw grip] // Get speed if (primaryButtons[(int)JoystickButton.Button7]) { multiplier = SPEED6; speed = SPEED6; _eventAggregator.GetEvent <LoggerEvent>().Publish("Speed = 6"); } else if (primaryButtons[(int)JoystickButton.Button8]) { multiplier = SPEED5; speed = SPEED5; _eventAggregator.GetEvent <LoggerEvent>().Publish("Speed = 5"); } else if (primaryButtons[(int)JoystickButton.Button9]) { multiplier = SPEED4; speed = SPEED4; _eventAggregator.GetEvent <LoggerEvent>().Publish("Speed = 4"); } else if (primaryButtons[(int)JoystickButton.Button10]) { multiplier = SPEED3; speed = SPEED3; _eventAggregator.GetEvent <LoggerEvent>().Publish("Speed = 3"); } else if (primaryButtons[(int)JoystickButton.Button11]) { multiplier = SPEED2; speed = SPEED2; _eventAggregator.GetEvent <LoggerEvent>().Publish("Speed = 2"); } else if (primaryButtons[(int)JoystickButton.Button12]) { multiplier = SPEED1; speed = SPEED1; _eventAggregator.GetEvent <LoggerEvent>().Publish("Speed = 1"); } // Rotate or Non Rotate Mode if (secondaryButtons[(int)JoystickButton.Button2]) { if (++rotateButtonCount == rotateButtonDebounce) { rotateMode = !rotateMode; if (rotateMode) { _eventAggregator.GetEvent <LoggerEvent>().Publish("Rotate Mode"); } else { _eventAggregator.GetEvent <LoggerEvent>().Publish("Non Rotate Mode"); } } } else { rotateButtonCount = 0; rotateButtonToggled = false; } // Turntable [0] and claw rotate [4] if (rotateMode) { // Claw rotation if (primaryButtons[(int)JoystickButton.Button4]) { clawRotateSpeed = speed; } else if (primaryButtons[(int)JoystickButton.Button3]) { clawRotateSpeed = -1 * speed; } else { clawRotateSpeed = 0; } // Turntable if (secondaryButtons[(int)JoystickButton.Button4]) { turntableSpeed = speed; } else if (secondaryButtons[(int)JoystickButton.Button3]) { turntableSpeed = -1 * speed; } else { turntableSpeed = 0; } // All others shoulderSpeed = 0; elbowSpeed = 0; wristLinearSpeed = 0; clawGripSpeed = 0; } // Shoulder [1], elbow [2], wrist [3], claw grip [5] else { // Elbow if (primaryButtons[(int)JoystickButton.Button5]) { elbowSpeed = -1 * speed; } else if (primaryButtons[(int)JoystickButton.Button3]) { elbowSpeed = speed; } else { elbowSpeed = 0; } // Shoulder if (secondaryButtons[(int)JoystickButton.Button5]) { shoulderSpeed = -1 * speed; } else if (secondaryButtons[(int)JoystickButton.Button3]) { shoulderSpeed = speed; } else { shoulderSpeed = 0; } // Wrist if (secondaryButtons[(int)JoystickButton.Button6]) { wristLinearSpeed = -1 * speed; } else if (secondaryButtons[(int)JoystickButton.Button4]) { wristLinearSpeed = speed; } else { wristLinearSpeed = 0; } // Claw grip if (primaryButtons[(int)JoystickButton.Button6]) { clawGripSpeed = speed; } else if (primaryButtons[(int)JoystickButton.Button4]) { clawGripSpeed = -1 * speed; } else { clawGripSpeed = 0; } // All others turntableSpeed = 0; clawRotateSpeed = 0; } badgerControlSubsystem.CurrentDriveMode.SendWrenchCommandQuasi((long)turntableSpeed, (long)shoulderSpeed, (long)elbowSpeed, (long)wristLinearSpeed, (long)clawRotateSpeed, (long)clawGripSpeed); } else { primaryZRotation = 0; // temporary stopgap for e-stop if (primaryButtons[(int)JoystickButton.Button2]) { primaryYVelocity = 0; secondaryYVelocity = 0; primaryZRotation = 0; } if (badgerControlSubsystem.CurrentDriveMode != null) { badgerControlSubsystem.CurrentDriveMode.SendDriveCommand(secondaryYVelocity, primaryYVelocity, primaryZRotation); } } } else { // ? } }
public void LocateJoystick() { // if we have the instance and all parties have confrmed, return if (JoyStickConfirmed && joystickQueryThread != null) return; if (joystickQueryThread == null) { // reset these because we have no joystick joystickConfirmedFromStatusView = false; joystickConfirmedFromVisualView = false; // try to create a joystick try { joystickQueryThread = new JoystickQueryThread(2); joystickQueryThread.Start(); } catch (Exception) { // TODO: make this work. this event is activated before the subscriber is attached if (!triedJoystick) { triedJoystick = true; _eventAggregator.GetEvent<LoggerEvent>().Publish("Joystick Not found."); } } } // deliver if we actually produced a joystick if (joystickQueryThread != null) { _eventAggregator.GetEvent<DeliverJoystickEvent>().Publish(joystickQueryThread); } }