Diff: Mixer/Mixer.cpp

Revision:

0:332926683ee3

Child:

1:2abfa02e99d5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mixer/Mixer.cpp	Mon Oct 08 11:07:14 2018 +0000
@@ -0,0 +1,51 @@
+#include "mbed.h"
+#include "Mixer.h"
+
+// Class constructor
+Mixer::Mixer() : motor0(PA_1), motor1(PB_11), motor2(PA_15), motor3(PB_9_ALT1)
+{
+}
+
+// Actuate motors with the desired force (N) and torques (N.m)
+void Mixer::actuate( float ft , float tau_phi , float tau_theta , float tau_psi )
+{
+    ft_to_omega(ft,tau_phi,tau_theta,tau_psi);
+    motor0 = omega_to_pwm(omega0);
+    motor1 = omega_to_pwm(omega1);
+    motor2 = omega_to_pwm(omega2);
+    motor3 = omega_to_pwm(omega3);
+}
+
+// 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;
+    
+    if (omega02 < 0){
+        omega02 = 0;
+    }
+    if (omega12 < 0){
+        omega12 = 0;
+    }
+    if (omega22 < 0){
+        omega22 = 0;
+    }
+    if (omega32 < 0){
+        omega32 = 0;
+    }
+    
+    omega0 = sqrt(omega02);
+    omega1 = sqrt(omega12);
+    omega2 = sqrt(omega22);
+    omega3 = sqrt(omega32);
+}
+
+// Converts desired angular velocity (rad/s) to PWM signal
+float Mixer::omega_to_pwm( float omega )
+{
+    float pwm = alpha*pow(omega,2) + beta*omega;
+    return pwm;
+}
\ No newline at end of file