Crazyflie 2.0 Controller
Dependents: Drone_Controlador_Atitude
Mixer/Mixer.cpp@0:332926683ee3, 2018-10-08 (annotated)
- Committer:
- IgneousGuikas
- Date:
- Mon Oct 08 11:07:14 2018 +0000
- Revision:
- 0:332926683ee3
- Child:
- 1:2abfa02e99d5
First version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
IgneousGuikas | 0:332926683ee3 | 1 | #include "mbed.h" |
IgneousGuikas | 0:332926683ee3 | 2 | #include "Mixer.h" |
IgneousGuikas | 0:332926683ee3 | 3 | |
IgneousGuikas | 0:332926683ee3 | 4 | // Class constructor |
IgneousGuikas | 0:332926683ee3 | 5 | Mixer::Mixer() : motor0(PA_1), motor1(PB_11), motor2(PA_15), motor3(PB_9_ALT1) |
IgneousGuikas | 0:332926683ee3 | 6 | { |
IgneousGuikas | 0:332926683ee3 | 7 | } |
IgneousGuikas | 0:332926683ee3 | 8 | |
IgneousGuikas | 0:332926683ee3 | 9 | // Actuate motors with the desired force (N) and torques (N.m) |
IgneousGuikas | 0:332926683ee3 | 10 | void Mixer::actuate( float ft , float tau_phi , float tau_theta , float tau_psi ) |
IgneousGuikas | 0:332926683ee3 | 11 | { |
IgneousGuikas | 0:332926683ee3 | 12 | ft_to_omega(ft,tau_phi,tau_theta,tau_psi); |
IgneousGuikas | 0:332926683ee3 | 13 | motor0 = omega_to_pwm(omega0); |
IgneousGuikas | 0:332926683ee3 | 14 | motor1 = omega_to_pwm(omega1); |
IgneousGuikas | 0:332926683ee3 | 15 | motor2 = omega_to_pwm(omega2); |
IgneousGuikas | 0:332926683ee3 | 16 | motor3 = omega_to_pwm(omega3); |
IgneousGuikas | 0:332926683ee3 | 17 | } |
IgneousGuikas | 0:332926683ee3 | 18 | |
IgneousGuikas | 0:332926683ee3 | 19 | // Converts desired force (N) and torques (N.m) to angular velocities ( rad /s) |
IgneousGuikas | 0:332926683ee3 | 20 | void Mixer::ft_to_omega( float ft , float tau_phi , float tau_theta , float tau_psi ) |
IgneousGuikas | 0:332926683ee3 | 21 | { |
IgneousGuikas | 0:332926683ee3 | 22 | float omega02 = (1/(4*kl))*ft - (1/(4*kl*l))*tau_phi - (1/(4*kl*l))*tau_theta - (1/(4*kd))*tau_psi; |
IgneousGuikas | 0:332926683ee3 | 23 | float omega12 = (1/(4*kl))*ft - (1/(4*kl*l))*tau_phi + (1/(4*kl*l))*tau_theta + (1/(4*kd))*tau_psi; |
IgneousGuikas | 0:332926683ee3 | 24 | float omega22 = (1/(4*kl))*ft + (1/(4*kl*l))*tau_phi + (1/(4*kl*l))*tau_theta - (1/(4*kd))*tau_psi; |
IgneousGuikas | 0:332926683ee3 | 25 | float omega32 = (1/(4*kl))*ft + (1/(4*kl*l))*tau_phi - (1/(4*kl*l))*tau_theta + (1/(4*kd))*tau_psi; |
IgneousGuikas | 0:332926683ee3 | 26 | |
IgneousGuikas | 0:332926683ee3 | 27 | if (omega02 < 0){ |
IgneousGuikas | 0:332926683ee3 | 28 | omega02 = 0; |
IgneousGuikas | 0:332926683ee3 | 29 | } |
IgneousGuikas | 0:332926683ee3 | 30 | if (omega12 < 0){ |
IgneousGuikas | 0:332926683ee3 | 31 | omega12 = 0; |
IgneousGuikas | 0:332926683ee3 | 32 | } |
IgneousGuikas | 0:332926683ee3 | 33 | if (omega22 < 0){ |
IgneousGuikas | 0:332926683ee3 | 34 | omega22 = 0; |
IgneousGuikas | 0:332926683ee3 | 35 | } |
IgneousGuikas | 0:332926683ee3 | 36 | if (omega32 < 0){ |
IgneousGuikas | 0:332926683ee3 | 37 | omega32 = 0; |
IgneousGuikas | 0:332926683ee3 | 38 | } |
IgneousGuikas | 0:332926683ee3 | 39 | |
IgneousGuikas | 0:332926683ee3 | 40 | omega0 = sqrt(omega02); |
IgneousGuikas | 0:332926683ee3 | 41 | omega1 = sqrt(omega12); |
IgneousGuikas | 0:332926683ee3 | 42 | omega2 = sqrt(omega22); |
IgneousGuikas | 0:332926683ee3 | 43 | omega3 = sqrt(omega32); |
IgneousGuikas | 0:332926683ee3 | 44 | } |
IgneousGuikas | 0:332926683ee3 | 45 | |
IgneousGuikas | 0:332926683ee3 | 46 | // Converts desired angular velocity (rad/s) to PWM signal |
IgneousGuikas | 0:332926683ee3 | 47 | float Mixer::omega_to_pwm( float omega ) |
IgneousGuikas | 0:332926683ee3 | 48 | { |
IgneousGuikas | 0:332926683ee3 | 49 | float pwm = alpha*pow(omega,2) + beta*omega; |
IgneousGuikas | 0:332926683ee3 | 50 | return pwm; |
IgneousGuikas | 0:332926683ee3 | 51 | } |