Diff: Mixer/Mixer.cpp

Revision:

0:3871dc7bedf7

Child:

2:54fabc943f25

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mixer/Mixer.cpp	Wed Oct 10 11:13:07 2018 +0000
@@ -0,0 +1,64 @@
+#include "mbed.h"
+#include "Mixer.h"
+#include <math.h>       /* sqrt */
+#include "Library.h"
+
+//Declare motors as PWM outputs
+PwmOut motor[4] = {(PA_1),(PB_11),(PA_15),(PB_9_ALT1)};
+
+// Class constructor
+Mixer :: Mixer () : motor_1 ( PA_1 ) , motor_2 ( PB_11 ) , motor_3 ( PA_15 ) , motor_4 (
+PB_9_ALT1 )
+{
+}
+
+
+void Mixer :: actuate ( float f_t , float tau_phi , float tau_theta , float tau_psi )
+{
+    
+    // Define parameters
+    float const alpha = 1.081e-7;
+    float const beta = 2.678e-11;
+    
+    //calcula as velocidades angulares conforme a relacao entre toque e forca e velocidade angular
+    w1 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) - ((1/(4*kl))* tau_theta) - ((1/(4*kd))* tau_psi)  ;
+    w2 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
+    w3 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) - ((1/(4*kd))* tau_psi);
+    w4 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) - ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
+
+    //confere se nao existe algum valor de velocidade menor que 0 para nao resultar em raiz de numero negativo
+    if (w1 < 0) {
+        w1 = 0;
+    }
+    if (w2 < 0) {
+        w2 = 0;
+    }
+    if (w3 < 0) {
+        w3 = 0;
+    }
+    if (w4 < 0) {
+        w4 = 0;
+    }
+    //Valores de velocidade angular descoberto com os torques e forca resultante fornecidos
+    w1 = sqrt(w1);
+    w2 = sqrt(w2);
+    w3 = sqrt(w3);
+    w4 = sqrt(w4);
+    
+ 
+    
+        wait (5) ;
+    // Turn on all motors for 5s
+    motor [0] = alpha *pow(w1 ,2) + beta * w1;
+    motor [1] = alpha *pow(w2 ,2) + beta * w2;
+    motor [2] = alpha *pow(w3 ,2) + beta * w3;
+    motor [3] = alpha *pow(w4 ,2) + beta * w4;
+    
+
+}
+
+
+
+
+
+