/vrpn_Tracker_ThalmicLabsMyo.C
C | 246 lines | 182 code | 44 blank | 20 comment | 39 complexity | d6c379d0b2ceabb40da199b10962fe78 MD5 | raw file
- #include "vrpn_Tracker_ThalmicLabsMyo.h"
- #ifdef VRPN_INCLUDE_THALMICLABSMYO
- #include "vrpn_Connection.h" // for vrpn_CONNECTION_LOW_LATENCY, etc
- #include "quat.h"
- /*
- * TODO :
- * Use myo timestamp when possible
- * Timestamps are 64 bit unsigned integers that correspond to a number of microseconds since some (unspecified) period in time. Timestamps are monotonically non-decreasing.
- * Stream raw emg data ?
- */
- // --------------------------------------------------------------------------
- // Constructor:
- // name (i): device name
- // c (i): vrp8n_Connection
- // useLock (b): require unlock gesture before detecting any gesture
- // armSide (ARMSIDE) : specify (or don't) the arm side for the myo
- vrpn_Tracker_ThalmicLabsMyo::vrpn_Tracker_ThalmicLabsMyo(const char *name, vrpn_Connection *c, bool useLock, ARMSIDE armSide) :
- vrpn_Tracker(name, c),
- vrpn_Analog(name, c),
- vrpn_Button_Filter(name, c),
- _armSide(armSide),
- hub(NULL),
- _myo(NULL),
- _analogChanged(false)
- {
- while (hub == NULL)
- {
- try
- {
- hub = new myo::Hub();
- hub->setLockingPolicy(useLock ? myo::Hub::lockingPolicyStandard : myo::Hub::lockingPolicyNone);
- hub->addListener(this);
- /// Set up sensor counts
- vrpn_Analog::num_channel = 9; // 0,1,2 : euler rotation - 3,4,5 : acceleration - 6,7,8 : gyroscope
- vrpn_Button::num_buttons = libmyo_num_poses + 1; // we add one for lock/unlock
- vrpn_Tracker::num_sensors = 1; // we have orientation and acceleration for the tracker
- vrpn_gettimeofday(&_timestamp, NULL);
- }
- catch (const std::exception& e) {
- std::cerr << "Error: " << e.what() << std::endl;
- }
- }
- }
- vrpn_Tracker_ThalmicLabsMyo::~vrpn_Tracker_ThalmicLabsMyo()
- {
- if (hub != NULL)
- delete hub;
- }
- void vrpn_Tracker_ThalmicLabsMyo::mainloop() {
- if (hub == NULL)
- return;
- // server update. We only need to call this once for all three
- // base devices because it is in the unique base class.
- server_mainloop();
-
- hub->runOnce(1);
- // only report once per loop for analog changes
- if (_analogChanged)
- {
- vrpn_Analog::report_changes(vrpn_CONNECTION_LOW_LATENCY);
- _analogChanged = false;
- }
- }
- void vrpn_Tracker_ThalmicLabsMyo::_report_lock() {
- vrpn_gettimeofday(&_timestamp, NULL);
- double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
- vrpn_Button::timestamp = _timestamp;
- buttons[0] = _locked;
- vrpn_Button::report_changes();
- }
- void vrpn_Tracker_ThalmicLabsMyo::onPair(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion)
- {
- std::cout << "Paired with Myo." << std::endl;
- myo->vibrate(myo->vibrationShort);
- }
- void vrpn_Tracker_ThalmicLabsMyo::onConnect(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion)
- {
- std::cout << "Myo Connected." << std::endl;
- myo->vibrate(myo->vibrationShort);
- }
- void vrpn_Tracker_ThalmicLabsMyo::onDisconnect(myo::Myo* myo, uint64_t timestamp)
- {
- std::cout << "Myo disconnected." << std::endl;
- if (myo == _myo)
- _myo = NULL;
- }
- void vrpn_Tracker_ThalmicLabsMyo::onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose pose)
- {
- if (myo != _myo)
- return;
- vrpn_gettimeofday(&_timestamp, NULL);
- double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
- vrpn_Button::timestamp = _timestamp;
-
- // std::cout << "Myo switched to pose " << pose.toString() << "." << std::endl;
- buttons[0] = _locked;
- // reset all buttons to 0. Maybe we should only do this if rest is on ?
- for (int i = 1; i < libmyo_num_poses + 1; ++i)
- buttons[i] = 0;
- buttons[pose.type() + 1] = 1;
- vrpn_Button::report_changes();
- }
- void vrpn_Tracker_ThalmicLabsMyo::onArmSync(myo::Myo* myo, uint64_t timestamp, myo::Arm arm, myo::XDirection xDirection)
- {
- std::cout << "Myo on arm : " << (arm == myo::armLeft ? "Left." : "Right.") << std::endl;
- if (_armSide == ARMSIDE::ANY || (_armSide == ARMSIDE::RIGHT && arm == myo::armRight) || (_armSide == ARMSIDE::LEFT && arm == myo::armLeft))
- {
- std::cout << "Myo : " << d_servicename << " accepted" << std::endl;
- myo->vibrate(myo->vibrationShort);
- _myo = myo;
- }
- else
- {
- std::cout << "Myo : " << d_servicename << " NOT accepted : wrong arm detected" << std::endl;
- }
-
- }
- void vrpn_Tracker_ThalmicLabsMyo::onArmUnsync(myo::Myo* myo, uint64_t timestamp)
- {
- if (myo != _myo)
- return;
- std::cout << "Myo : " << d_servicename <<" removed from arm."<< std::endl;
- myo->vibrate(myo->vibrationLong);
- _myo = NULL;
- }
- void vrpn_Tracker_ThalmicLabsMyo::onUnlock(myo::Myo* myo, uint64_t timestamp)
- {
- if (myo != _myo)
- return;
- _locked = false;
- _report_lock();
- }
- void vrpn_Tracker_ThalmicLabsMyo::onLock(myo::Myo* myo, uint64_t timestamp)
- {
- if (myo != _myo)
- return;
- _locked = true;
- _report_lock();
- }
- void vrpn_Tracker_ThalmicLabsMyo::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& rotation)
- {
- if (myo != _myo)
- return;
- if (!d_connection) {
- return;
- }
- vrpn_gettimeofday(&_timestamp, NULL);
- double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
- vrpn_Tracker::timestamp = _timestamp;
- vrpn_Analog::timestamp = _timestamp;
- d_quat[0] = rotation.x();
- d_quat[1] = rotation.y();
- d_quat[2] = rotation.z();
- d_quat[3] = rotation.w();
- // do the same as analog, with euler angles (maybe offset from when OnArmSync?)
- q_vec_type euler;
- q_to_euler(euler, d_quat);
- channel[ANALOG_ROTATION_X] = euler[Q_ROLL];
- channel[ANALOG_ROTATION_Y] = euler[Q_PITCH];
- channel[ANALOG_ROTATION_Z] = euler[Q_YAW];
- char msgbuf[1000];
- int len = vrpn_Tracker::encode_to(msgbuf);
- if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
- fprintf(stderr, "Thalmic Lab's myo tracker: can't write message: tossing\n");
- }
- _analogChanged = true;
- }
- void vrpn_Tracker_ThalmicLabsMyo::onAccelerometerData(myo::Myo* myo, uint64_t timestamp, const myo::Vector3<float>& accel)
- {
- if (myo != _myo)
- return;
- if (!d_connection) {
- return;
- }
- vrpn_gettimeofday(&_timestamp, NULL);
- double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
- vrpn_Tracker::timestamp = _timestamp;
- vrpn_Analog::timestamp = _timestamp;
-
- acc[0] = accel[0];
- acc[1] = accel[1];
- acc[2] = accel[2];
- // same for analog
- channel[ANALOG_ACCEL_X] = accel[0];
- channel[ANALOG_ACCEL_Y] = accel[1];
- channel[ANALOG_ACCEL_Z] = accel[2];
- char msgbuf[1000];
- int len = encode_acc_to(msgbuf);
- if (d_connection->pack_message(len, _timestamp, accel_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
- fprintf(stderr, "Thalmic Lab's myo tracker: can't write message: tossing\n");
- }
- _analogChanged = true;
- }
- void vrpn_Tracker_ThalmicLabsMyo::onGyroscopeData(myo::Myo* myo, uint64_t timestamp, const myo::Vector3<float>& gyro)
- {
- if (myo != _myo)
- return;
- vrpn_gettimeofday(&_timestamp, NULL);
- double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
- vrpn_Analog::timestamp = _timestamp;
- channel[ANALOG_GYRO_X] = gyro[0];
- channel[ANALOG_GYRO_Y] = gyro[1];
- channel[ANALOG_GYRO_Z] = gyro[2];
- _analogChanged = true;
- }
- #endif