Diff: Mixer/Mixer.cpp

Revision:

2:9e07bed8e8ed

Parent:

1:2abfa02e99d5

--- a/Mixer/Mixer.cpp	Tue Oct 09 18:00:51 2018 +0000
+++ b/Mixer/Mixer.cpp	Wed Nov 21 10:46:29 2018 +0000
@@ -20,22 +20,27 @@
 // Converts desired force (N) and torques (N.m) to angular velocities ( rad /s)
 void Mixer::ft_to_omega( float ft , float tau_phi , float tau_theta , float tau_psi )
 {
-    float omega02 = (1/(4*kl))*ft - (1/(4*kl*l))*tau_phi - (1/(4*kl*l))*tau_theta - (1/(4*kd))*tau_psi;
-    float omega12 = (1/(4*kl))*ft - (1/(4*kl*l))*tau_phi + (1/(4*kl*l))*tau_theta + (1/(4*kd))*tau_psi;
-    float omega22 = (1/(4*kl))*ft + (1/(4*kl*l))*tau_phi + (1/(4*kl*l))*tau_theta - (1/(4*kd))*tau_psi;
-    float omega32 = (1/(4*kl))*ft + (1/(4*kl*l))*tau_phi - (1/(4*kl*l))*tau_theta + (1/(4*kd))*tau_psi;
+    float a1 = (1.0f/(4.0f*kl))*ft;
+    float a2 = (1.0f/(4.0f*kl*l))*tau_phi;
+    float a3 = (1.0f/(4.0f*kl*l))*tau_theta;
+    float a4 = (1.0f/(4.0f*kd))*tau_psi;
     
-    if (omega02 < 0){
-        omega02 = 0;
+    float omega02 = a1 - a2 - a3 - a4;
+    float omega12 = a1 - a2 + a3 + a4;
+    float omega22 = a1 + a2 + a3 - a4;
+    float omega32 = a1 + a2 - a3 + a4;
+    
+    if (omega02 < 0.0f){
+        omega02 = 0.0f;
     }
-    if (omega12 < 0){
-        omega12 = 0;
+    if (omega12 < 0.0f){
+        omega12 = 0.0f;
     }
-    if (omega22 < 0){
-        omega22 = 0;
+    if (omega22 < 0.0f){
+        omega22 = 0.0f;
     }
-    if (omega32 < 0){
-        omega32 = 0;
+    if (omega32 < 0.0f){
+        omega32 = 0.0f;
     }
     
     omega0 = sqrt(omega02);