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';
}
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;
}
// 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;
}
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;
}
// 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.
}
// 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()));
//M_PI doesnt work
// Convert the floating point angles in radians to a scale from 0 to 18.
roll_w = static_cast<int>((roll + (float)3.14) / (3.14 * 2.0f) * 18);
pitch_w = static_cast<int>((pitch + (float)3.14 / 2.0f) / 3.14 * 18);
yaw_w = static_cast<int>((yaw + (float)3.14) / (3.14 * 2.0f) * 18);
}
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;
}
// 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));
}
}
// 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);
}
// 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);
}
// 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()))));
}
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;
}
// 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;
}
}
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;
}
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;
}
}
// 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));
}
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()));
}
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()))));
}
void DataCollector::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) * 100);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 100);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 100);
}
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);
}
}