Crazyflie 2.0 Controller
Dependents: Drone_Controlador_Atitude
Mixer/Mixer.cpp
- Committer:
- IgneousGuikas
- Date:
- 2018-11-21
- Revision:
- 2:9e07bed8e8ed
- Parent:
- 1:2abfa02e99d5
File content as of revision 2:9e07bed8e8ed:
#include "mbed.h" #include "Parameters.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 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; 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.0f){ omega12 = 0.0f; } if (omega22 < 0.0f){ omega22 = 0.0f; } if (omega32 < 0.0f){ omega32 = 0.0f; } 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; }