Diff: Mixer/Mixer.cpp

Revision:

0:b1f2c9e88e32

Child:

5:b9947e3d20cf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mixer/Mixer.cpp	Fri Aug 31 18:41:31 2018 +0000
@@ -0,0 +1,59 @@
+#include "mbed.h"
+#include "Mixer.h"
+
+// Class constructor
+//Mixer::Mixer(PinName pin_1, PinName pin_2, PinName pin_3, PinName pin_4) : motor_1(pin_1) , motor_2(pin_2) , motor_3(pin_3) , motor_4(pin_4)
+Mixer::Mixer() : motor_1(PA_1), motor_2(PB_11), motor_3(PA_15), motor_4(PB_9_ALT1)
+{
+}
+
+// Actuate motors with the desired force (N) and torques (N.m)
+void Mixer::actuate(float f_t, float tau_phi, float tau_theta, float tau_psi)
+{
+    // Convert desired force (N) and torques (N.m) to angular velocities (rad/s)
+    force_and_torques_to_angular_velocities(f_t,tau_phi,tau_theta,tau_psi);
+    // Convert desired angular velocity (rad/s) to PWM signal and actuate motors
+    motor_1 = angular_velocity_to_pwm(omega_1);
+    motor_2 = angular_velocity_to_pwm(omega_2);
+    motor_3 = angular_velocity_to_pwm(omega_3);
+    motor_4 = angular_velocity_to_pwm(omega_4);
+}
+
+// Converts desired force (N) and torques (N.m) to angular velocities (rad/s)
+void Mixer::force_and_torques_to_angular_velocities(float f_t, float tau_phi, float tau_theta, float tau_psi)
+{
+    // Convert desired force (N) and torques (N.m) to angular velocities squared (rad^2/s^2)
+    float omega_1_squared = (1.0f/4.0f)*(f_t/kl-tau_phi/(kl*l)-tau_theta/(kl*l)-tau_psi/kd);
+    float omega_2_squared = (1.0f/4.0f)*(f_t/kl-tau_phi/(kl*l)+tau_theta/(kl*l)+tau_psi/kd);
+    float omega_3_squared = (1.0f/4.0f)*(f_t/kl+tau_phi/(kl*l)+tau_theta/(kl*l)-tau_psi/kd);
+    float omega_4_squared = (1.0f/4.0f)*(f_t/kl+tau_phi/(kl*l)-tau_theta/(kl*l)+tau_psi/kd);
+    // Convert angular velocities squared (rad^2/s^2) to angular velocities (rad/s) only if value is positive
+    if(omega_1_squared>=0) {
+        omega_1 = sqrt(omega_1_squared);
+    } else {
+        omega_1 = 0.0f;
+    }
+    if(omega_2_squared>=0) {
+        omega_2 = sqrt(omega_2_squared);
+    } else {
+        omega_2 = 0.0f;
+    }
+    if(omega_3_squared>=0) {
+        omega_3 = sqrt(omega_3_squared);
+    } else {
+        omega_3 = 0.0f;
+    }
+    if(omega_4_squared>=0) {
+        omega_4 = sqrt(omega_4_squared);
+    } else {
+        omega_4 = 0.0f;
+    }
+}
+
+// Converts desired angular velocity (rad/s) to PWM signal
+float Mixer::angular_velocity_to_pwm(float omega)
+{
+    // Convert desired angular velocity (rad/s) to PWM signal
+    float pwm = alpha*pow(omega,2)+beta*omega;
+    return pwm;
+}
\ No newline at end of file