#define MAC "00:00:00:00:00:00" #define _USE_MATH_DEFINES #include #include #include #include #include #include #include #include #include #include "osc/OscOutboundPacketStream.h" #include "ip/UdpSocket.h" #define OUTPUT_BUFFER_SIZE 1024 UdpTransmitSocket* transmitSocket1; UdpTransmitSocket* transmitSocket2; UdpTransmitSocket* transmitSocket3; UdpTransmitSocket* transmitSocket4; class DataCollector : public myo::DeviceListener{ public: DataCollector(): roll_w(0), pitch_w(0), yaw_w(0), emg() {} int roll_w, pitch_w, yaw_w; float w, x, y, z, roll, pitch, yaw, a_x, a_y, a_z, g_x, g_y, g_z; char buffer[OUTPUT_BUFFER_SIZE]; std::array emg; void onPair(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion){ knownMyos.push_back(myo); emg.fill(0); } void onConnect(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion){ std::cout << "Myo " << identifyMyo(myo) << " has connected." << std::endl; } void onDisconnect(myo::Myo* myo, uint64_t timestamp){ std::cout << "Myo " << identifyMyo(myo) << " has disconnected." << std::endl; } size_t identifyMyo(myo::Myo* myo){ for (size_t i = 0; i < knownMyos.size(); ++i){ if (knownMyos[i] == myo){ return i + 1; } } return 0; } std::vector knownMyos; void onEmgData(myo::Myo* myo, uint64_t timestamp, const int8_t* emgData){ for (int i = 0; i < 8; i++){ emg[i] = emgData[i]; } osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE); p << osc::BeginMessage("EMG") << MAC << emg[0] << emg[1] << emg[2] << emg[3] << emg[4] << emg[5] << emg[6] << emg[7] << osc::EndMessage; if (identifyMyo(myo) == 1){ transmitSocket2->Send(p.Data(), p.Size()); } else if (identifyMyo(myo) == 2){ transmitSocket4->Send(p.Data(), p.Size()); } } void onAccelerometerData(myo::Myo* myo, uint64_t timestamp, const myo::Vector3& accel){ a_x = accel.x(); a_y = accel.y(); a_z = accel.z(); osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE); p << osc::BeginMessage("Accel") << MAC << a_x << a_y << a_z << osc::EndMessage; if (identifyMyo(myo) == 1){ transmitSocket1->Send(p.Data(), p.Size()); } else if (identifyMyo(myo) == 2){ transmitSocket3->Send(p.Data(), p.Size()); } } void onGyroscopeData(myo::Myo* myo, uint64_t timestamp, const myo::Vector3& gyro){ g_x = gyro.x(); g_y = gyro.y(); g_z = gyro.z(); osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE); p << osc::BeginMessage("Gyro") << MAC << g_x << g_y << g_z << osc::EndMessage; if (identifyMyo(myo) == 1){ transmitSocket1->Send(p.Data(), p.Size()); } else if (identifyMyo(myo) == 2){ transmitSocket3->Send(p.Data(), p.Size()); } } void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion& quat){ using std::atan2; using std::asin; using std::sqrt; float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x())); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE); p << osc::BeginMessage("Orient") << MAC << roll << pitch << yaw << osc::EndMessage; if (identifyMyo(myo) == 1){ transmitSocket1->Send(p.Data(), p.Size()); } else if (identifyMyo(myo) == 2){ transmitSocket3->Send(p.Data(), p.Size()); } roll_w = static_cast((roll + (float)M_PI)/(M_PI * 2.0f) * 18); pitch_w = static_cast((pitch + (float)M_PI/2.0f)/M_PI * 18); yaw_w = static_cast((yaw + (float)M_PI)/(M_PI * 2.0f) * 18); } }; int main(int argc, char** argv){ try{ std::cout << "OSC to 127.0.0.1:7777\n"; transmitSocket1 = new UdpTransmitSocket(IpEndpointName("127.0.0.1", 7777)); std::cout << "OSC to 127.0.0.1:7778\n"; transmitSocket2 = new UdpTransmitSocket(IpEndpointName("127.0.0.1", 7778)); std::cout << "OSC to 127.0.0.1:7779\n"; transmitSocket3 = new UdpTransmitSocket(IpEndpointName("127.0.0.1", 7779)); std::cout << "OSC to 127.0.0.1:7780\n"; transmitSocket4 = new UdpTransmitSocket(IpEndpointName("127.0.0.1", 7780)); myo::Hub hub("com.samy.myo-osc"); // myo->setStreamEmg(myo::Myo::streamEmgEnabled); DataCollector collector; hub.addListener(&collector); while (1){ hub.run(1000/20); } } catch (const std::exception& e){ std::cerr << "Error: " << e.what() << std::endl; std::cerr << "Press enter to continue."; std::cin.ignore(); return 1; } }