Пример #1
0
    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a unit quaternion.
	void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
	{
		// Calculate the normalized quaternion.
		float norm = sqrtf(quat.x() * quat.x() + quat.y() * quat.y() + quat.z() * quat.z() + quat.w() * quat.w());
		myo::Quaternion<float> normalized(quat.x() / norm, quat.y() / norm, quat.z() / norm, quat.w() / norm);
		// Calculate pitch and yaw from the normalized quaternion.
		float pitch = asinf(2.0f * (normalized.w() * normalized.y() - normalized.z() * normalized.x()));
		float yaw = atan2f(2.0f * (normalized.w() * normalized.z() + normalized.x() * normalized.y()),
			1.0f - 2.0f * (normalized.y() * normalized.y() + normalized.z() * normalized.z()));

		// Calculate the change in pitch and yaw since the last update.
		float deltaPitch = pitch - _previousPitch;
		float deltaYaw = yaw - _previousYaw;

		// Calculate the amount by which to move the cursor.
		// The value SENSITIVITY_X and SENSITIVITY_Y control how sensitive the cursor is
		//  to movements made by the Myo in the x- and y-directions, respectively.
		int dx = deltaYaw * SENSITIVITY_X;
		int dy = deltaPitch * SENSITIVITY_Y;


		if (cameraMode) {

			// Actually move the cursor.
			POINT cursorPos;
			bool result = GetCursorPos(&cursorPos);
			SetCursorPos(cursorPos.x - dx, cursorPos.y - dy);
		}

		// Store the new pitch and yaw values for next time.
		_previousPitch = pitch;
		_previousYaw = yaw;
	}
Пример #2
0
    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a unit quaternion.
    void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
    {
        using std::atan2;
        using std::asin;
        using std::sqrt;

        std::cout<<"orientation fucntion called"<<std::endl;
        // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
        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("/myo/orientation")
        << MAC
        << quat.x() << quat.y() << quat.z() << quat.w() << roll << pitch << yaw << osc::EndMessage;
		transmitSocket->Send(p.Data(), p.Size());
    
        // Convert the floating point angles in radians to a scale from 0 to 20.
        roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
        yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
    }
Пример #3
0
char DataCollector::quat_to_note(myo::Quaternion<float> q){
	/*
	A- X .51	Y -.16	Z 0.85
	B- X .43	Y -.36	Z .81
	C- X .26	Y -.45	Z .69
	D- X .40	Y .12	Z .89
	E- X .30	Y .04	Z .76
	F- X .32	Y .39	Z .77
	G- X .27	Y .35	Z .60	
	*/
	myo::Vector3<float> v(q.x(), q.y(), q.z());
	if (angle_between(v, a_vec) <=30){
		return 'A';
	}
	else if (angle_between(v, b_vec) <= 30){
		return 'B';
	}
	else if (angle_between(v, c_vec) <= 30){
		return 'C';
	}
	else if (angle_between(v, d_vec) <= 30){
		return 'D';
	}
	else if (angle_between(v, e_vec) <= 30){
		return 'E';
	}
	else if (angle_between(v, f_vec) <= 30){
		return 'F';
	}
	else if (angle_between(v, g_vec) <= 30){
		return 'G';
	}
	else
		return 'X';
}
Пример #4
0
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;
}
Пример #5
0
 // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented as a unit quaternion.
 // This function takes the position values from the quaternion and assign new values to roll_w, pitch_w and yaw_w every time new data are provided by the Myo.
 void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
 {
     using std::atan2;
     using std::asin;
     using std::sqrt;
     using std::max;
     using std::min;
     
     // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
     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()));
     
     // Convert the floating point angles in radians to a clockwise scale from 1 to 127 (good for PureData) with the zero angle in π.
     // Need to change the format of the angle? DO IT HERE!
     
     roll_tmp = static_cast<int>( ( roll * 180/M_PI) );   // the value of 200 ensures a positive value for the angle.
 }
Пример #6
0
    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a unit quaternion.
    void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
    {
        using std::atan2;
        using std::asin;
        using std::sqrt;

        // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
        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()));

        //hacky globals
        ROLL = roll;
        PITCH = pitch;
        YAW = yaw;

        // Convert the floating point angles in radians to a scale from 0 to 20.
        roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);

        yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
        if (CURRENTPOSE == myo::Pose::waveIn) {
            WAVEINCOUNTER += 2;
        }
        if (CURRENTPOSE == myo::Pose::fingersSpread) {
            FINGSERSPREADCOUNTER += 2;
        }
        if (WAVEINCOUNTER != 1) {
            WAVEINCOUNTER -= 1;
        }
    }
Пример #7
0
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
    using std::atan2;
    using std::asin;
    using std::sqrt;
    
    // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
    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()));
    
    // Convert the floating point angles in radians to a scale from 0 to 20.
    float roll_f = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
    float pitch_f = ((pitch + (float)M_PI/2.0f)/M_PI * 18);
    float yaw_f = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
    
    int roll_w = static_cast<int>(roll_f);
    int pitch_w = static_cast<int>(pitch_f);
    int yaw_w = static_cast<int>(yaw_f);
    
    MyoData &data = knownMyosData[myo];
    data.roll = roll_f/18.0*100.0;
    data.pitch = pitch_f/18.0*100.0;
    data.yaw = yaw_f/18.0*100.0;
    
    sendOrientation(myo, data.roll, data.pitch, data.yaw);
}
void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
    using std::atan2;
    using std::asin;
    using std::sqrt;
    using std::max;
    using std::min;
	controlSystem = controller->getControlSystem();
	systemUnlocked = controller->getSystemStatus();
	if (controlSystem==2 && systemUnlocked){
		roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
                       1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
		roll_w = static_cast<int>((roll + (float)M_PI/2.0f)/M_PI * 18);		   
		controller->setRoll(roll_w);	
	} else {
		pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
		pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
		controller->setPitch(pitch_w);
		//cout << "SPEED: " << controller->longSpeed[pitch_w] << endl;
	}
    if (controlSystem == 0 && systemUnlocked){
		yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
                    1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
		yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
		controller->setYaw(yaw_w);
		}
}
Пример #9
0
	// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
	// as a unit quaternion.
	void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
	{
		using std::atan2;
		using std::asin;
		using std::sqrt;
		using std::max;
		using std::min;
		// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
		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(max(-1.0f, min(1.0f, 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()));
		// Convert the floating point angles in radians to a scale from 0 to 18.
		roll_w = static_cast<int>((roll + (float)M_PI) / (M_PI * 2.0f) * 18);
		pitch_w = static_cast<int>((pitch + (float)M_PI / 2.0f) / M_PI * 18);
		yaw_w = static_cast<int>((yaw + (float)M_PI) / (M_PI * 2.0f) * 18);
	}
Пример #10
0
void myouse::MyouseListener::onOrientationData(myo::Myo * myo, uint64_t timestamp,
	const myo::Quaternion<float> & rotation)
{
	using std::atan2;
	using std::asin;
	using std::sin;

	double newRoll = atan2(2.0f * (rotation.w() * rotation.x() + rotation.y() * rotation.z()),
		1.0f - 2.0f * (rotation.x() * rotation.x() + rotation.y() * rotation.y()));
	double newPitch = asin(2.0f * (rotation.w() * rotation.y() - rotation.z() * rotation.x()));
	double newYaw = atan2(2.0f * (rotation.w() * rotation.z() + rotation.x() * rotation.y()),
		1.0f - 2.0f * (rotation.y() * rotation.y() + rotation.z() * rotation.z()));

	double roll = newRoll - rollOffset;
	double pitch = newPitch - pitchOffset;
	double yaw = newYaw - yawOffset;

	if (xDir == myo::xDirectionTowardElbow) pitch *= -1;

	if (isScrolling)
	{
		scroll(-pitch * SCROLL_SPEED);
	}
	else
	{
		int x = SCREEN_WIDTH * (0.5 - X_SPEED * yaw);
		int y = SCREEN_HEIGHT * (0.5 + Y_SPEED * pitch);

		bool dragging = leftDown || rightDown || middleDown;
		float dist = sqrt((x - lastX) * (x - lastX) + (y - lastY) * (y - lastY));

		if (!dragging || dist > DRAG_THRESHOLD)
		{
			moveMouse(x, y);

			lastX = x;
			lastY = y;
		}
	}

	rawRoll = newRoll;
	rawPitch = newPitch;
	rawYaw = newYaw;
}
Пример #11
0
    jobject getJavaQuaternion(JNIEnv * env, myo::Quaternion<float> quaternion) {
        jclass quaternionClass = env->FindClass("net/johnluetke/myojni/jni/Quaternion");
        jmethodID quaternionConstructor = env->GetMethodID(quaternionClass, "<init>", "(DDDD)V");
        jobject javaQuaternion = env->NewObject(quaternionClass,
                                                quaternionConstructor,
                                                quaternion.x(),
                                                quaternion.y(),
                                                quaternion.z(),
                                                quaternion.w());

        return javaQuaternion;
    }
Пример #12
0
void DeviceCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
    Device * device = findDevice(myo);
    if ( device ) {
        using std::atan2;
        using std::asin;
        using std::sqrt;
        
        // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
        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()));
        
        device->q = quat;//set(quat.x(), quat.y(), quat.z(), quat.w());
        
        
        device->roll = roll;
        device->pitch = pitch;
        device->yaw = yaw;
        
        
        // Convert the floating point angles in radians to a scale from 0 to 20.
        device->roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        device->pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
        device->yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
        
        float gyoro_g = sqrt(device->gyro.x()*device->gyro.x() + device->gyro.y()*device->gyro.y() + device->gyro.z()*device->gyro.z());
        float g = sqrt(device->accel.x()*device->accel.x() + device->accel.y()*device->accel.y() + device->accel.z()*device->accel.z());
        //            cout << gyoro_g << endl;
        //            cout << g << endl;
        if ( gyoro_g <= 0.2 ) device->gravity = g;
        
//        Quaternion4f q;
//        q = quat;
//        //.set(quat.x(), quat.z(), quat.y(), quat.w());
//        Vector3f linear_accel;
//        ofMatrix4x4 mat;
//        mat.translate(ofVec3f(0,device->gravity,0));
//        mat.rotate(q);
//        ofVec3f trans = mat.getTranslation();
//        
//        linear_accel = device->getAccel();
//        linear_accel.x = linear_accel.x - trans.x;
//        linear_accel.y = linear_accel.y - trans.z;
//        linear_accel.z = linear_accel.z - trans.y;
//
//        device->linear_accel.set(linear_accel);
        //            cout << device->getAccel() << endl;
        //            cout << mat.getTranslation() << endl;
        //            cout << linear_accel << endl;
    }
}
    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a unit quaternion.
    void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
    {
        using std::atan2;
        using std::asin;
        using std::sqrt;

        // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
        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()));
		
        // Convert the floating point angles in radians to a scale from 0 to 20.
        roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
        yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
		float drop = 1.1;
		roll = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch = ((pitch + (float)M_PI/2.0f)/M_PI * 18);
		if(pitch > 10)
		{	if(pitch < 20)
			{	pitch *= (drop*(20-pitch));
			}
		}
		else
		{	if(pitch != 10)
			{	pitch *= (drop*(10-pitch));
			}
		}	
        yaw = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
		if(yaw > 10.0000)
		{	if(yaw < 20.0000)
			{	yaw *= (drop*(20-yaw));
			}
			else
			{	yaw = 19.9999;
			}
		}
		else
		{	if(yaw != 10.0000)
			{	yaw *= (drop*(10-yaw));
			}
		}	
		if(color_input == true)
		{	float x = (GetSystemMetrics(SM_CXSCREEN));
			float y = (GetSystemMetrics(SM_CYSCREEN));
			SetCursorPos((((((20)-yaw))/(20))*x), ((pitch/(20))*y));
		}
    }
Пример #14
0
	// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
	// as a unit quaternion.
	void Filter::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
	{
		quatX = quat.x(); //Return the x-component of this quaternion's vector
		quatY = quat.y(); //Return the y -component of this quaternion's vector
		quatZ = quat.z(); //Return the z-component of this quaternion's vector
		quatW = quat.w(); //Return the w-component (scalar) of this quaternion's vector
		using std::atan2;
		using std::asin;
		using std::sqrt;
		using std::cos;
		using std::sin;
		using std::max;
		using std::min;

		roll = -1 * atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
				1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
		pitch = -1 * atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
				1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
		yaw = -1 * asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
	}
Пример #15
0
void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
    using std::atan2;
    using std::asin;
    using std::sqrt;


    // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
    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()));

	MyoFrameOrientationData & orient_data = m_orientation_data;

	orient_data.timestamp	= timestamp;
	orient_data.quaternion	= ofQuaternion( quat.x(), quat.y(), quat.z(), quat.w() );
    orient_data.euler_roll	= roll;
    orient_data.euler_pitch = pitch;
	orient_data.euler_yaw	= yaw;
}
 // Orientaion
 void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
 {
   const int count = 4;
   float values[count] = { quat.x(), quat.y(), quat.z(), quat.w() };
   this->addEventData(myo, timestamp, "orientation", new VectorEventData(values, count));
 }
void WifiDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) {
	
	using std::atan2;
	using std::asin;
	using std::sqrt;
	using std::max;
	using std::min;
	ClientInfo* clientinfo = new ClientInfo();
	clientinfo->ID = 0;
	clientinfo->message = new std::string("message");
	clientinfo->name = "Client";

	// Calculate Euler angles (roll and pitch) from the unit quaternion.
	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(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));

	//Transforms the radians to degrees angles. 
	roll_w = (int)(roll * 180 / M_PI);
	pitch_w = (int)(pitch * 180 / M_PI);


	//To use in the robot servo motor, roll_w mut be between 45 and 135 degrees

	//Gives an interval of 30 degrees to stabilize at 0 degrees.
	//The 0 degree on the servo motor is at 90 degrees.
	if (roll_w >= -15 && roll_w <= 15) {
		roll_w = 90;
	}

	else if (roll_w < -15) {
		//roll_w min value is 45.
		if (roll_w < -60)
			roll_w = 45;
		else
			roll_w += 105;
	}
	else if (roll_w > 15) {
		//roll_w max value is 135
		if (roll_w > 60)
			roll_w = 135;
		else
			roll_w += 75;
	}

	//The robot has 6 diffirent speeds 

	//Gives an interval of 30 degrees to stabilize at 0 degrees.
	// Parking state
	if (pitch_w >= -15 && pitch_w <= 15) {
		pitch_w = 0;
	}

	//Reverse speeds
	else if (pitch_w < -15) {
		if (pitch_w >= -35) pitch_w = 4;
		else if (pitch_w >= -55) pitch_w = 5;
		else pitch_w = 6;
	}
	//Foward speeds
	else if (pitch_w > 15) {
		if (pitch_w <= 35) pitch_w = 1;
		else if (pitch_w <= 55) pitch_w = 2;
		else pitch_w = 3;
	}

	clientinfo->servo = roll_w;
	clientinfo->speed = pitch_w;
	client->sendToServer(clientinfo);

	delete clientinfo;


}
float QuaternionToPitch(const myo::Quaternion<float>& quat) {
  return std::asin(std::max(
      -1.0f,
      std::min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
}
float QuaternionToRoll(const myo::Quaternion<float>& quat) {
  return std::atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
                    1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
}