Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
Diff: Mixer/Mixer.cpp
- Revision:
- 5:b9947e3d20cf
- Parent:
- 0:b1f2c9e88e32
- Child:
- 6:3188b00263e8
--- a/Mixer/Mixer.cpp Wed Sep 26 21:47:37 2018 +0000 +++ b/Mixer/Mixer.cpp Thu Sep 27 13:51:54 2018 +0000 @@ -2,32 +2,27 @@ #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) +// Actuate motors with desired total trust 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) +// Converts total trust 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 { @@ -50,10 +45,9 @@ } } -// Converts desired angular velocity (rad/s) to PWM signal +// 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