Rodrigo Gikas / CrazyflieController

Crazyflie 2.0 Controller

Dependents:   Drone_Controlador_Atitude

Mixer/Mixer.cpp@1:2abfa02e99d5, 2018-10-09 (annotated)

Committer:
IgneousGuikas
Date:
Tue Oct 09 18:00:51 2018 +0000
Revision:
1:2abfa02e99d5
Parent:
0:332926683ee3
Child:
2:9e07bed8e8ed
Vers?o atualizada com par?metros de seguran?a.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IgneousGuikas 0:332926683ee3 1 #include "mbed.h"
IgneousGuikas 1:2abfa02e99d5 2 #include "Parameters.h"
IgneousGuikas 0:332926683ee3 3 #include "Mixer.h"
IgneousGuikas 0:332926683ee3 4
IgneousGuikas 0:332926683ee3 5 // Class constructor
IgneousGuikas 0:332926683ee3 6 Mixer::Mixer() : motor0(PA_1), motor1(PB_11), motor2(PA_15), motor3(PB_9_ALT1)
IgneousGuikas 0:332926683ee3 7 {
IgneousGuikas 0:332926683ee3 8 }
IgneousGuikas 0:332926683ee3 9
IgneousGuikas 0:332926683ee3 10 // Actuate motors with the desired force (N) and torques (N.m)
IgneousGuikas 0:332926683ee3 11 void Mixer::actuate( float ft , float tau_phi , float tau_theta , float tau_psi )
IgneousGuikas 0:332926683ee3 12 {
IgneousGuikas 0:332926683ee3 13 ft_to_omega(ft,tau_phi,tau_theta,tau_psi);
IgneousGuikas 0:332926683ee3 14 motor0 = omega_to_pwm(omega0);
IgneousGuikas 0:332926683ee3 15 motor1 = omega_to_pwm(omega1);
IgneousGuikas 0:332926683ee3 16 motor2 = omega_to_pwm(omega2);
IgneousGuikas 0:332926683ee3 17 motor3 = omega_to_pwm(omega3);
IgneousGuikas 0:332926683ee3 18 }
IgneousGuikas 0:332926683ee3 19
IgneousGuikas 0:332926683ee3 20 // Converts desired force (N) and torques (N.m) to angular velocities ( rad /s)
IgneousGuikas 0:332926683ee3 21 void Mixer::ft_to_omega( float ft , float tau_phi , float tau_theta , float tau_psi )
IgneousGuikas 0:332926683ee3 22 {
IgneousGuikas 0:332926683ee3 23 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 24 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 25 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 26 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 27
IgneousGuikas 0:332926683ee3 28 if (omega02 < 0){
IgneousGuikas 0:332926683ee3 29 omega02 = 0;
IgneousGuikas 0:332926683ee3 30 }
IgneousGuikas 0:332926683ee3 31 if (omega12 < 0){
IgneousGuikas 0:332926683ee3 32 omega12 = 0;
IgneousGuikas 0:332926683ee3 33 }
IgneousGuikas 0:332926683ee3 34 if (omega22 < 0){
IgneousGuikas 0:332926683ee3 35 omega22 = 0;
IgneousGuikas 0:332926683ee3 36 }
IgneousGuikas 0:332926683ee3 37 if (omega32 < 0){
IgneousGuikas 0:332926683ee3 38 omega32 = 0;
IgneousGuikas 0:332926683ee3 39 }
IgneousGuikas 0:332926683ee3 40
IgneousGuikas 0:332926683ee3 41 omega0 = sqrt(omega02);
IgneousGuikas 0:332926683ee3 42 omega1 = sqrt(omega12);
IgneousGuikas 0:332926683ee3 43 omega2 = sqrt(omega22);
IgneousGuikas 0:332926683ee3 44 omega3 = sqrt(omega32);
IgneousGuikas 0:332926683ee3 45 }
IgneousGuikas 0:332926683ee3 46
IgneousGuikas 0:332926683ee3 47 // Converts desired angular velocity (rad/s) to PWM signal
IgneousGuikas 0:332926683ee3 48 float Mixer::omega_to_pwm( float omega )
IgneousGuikas 0:332926683ee3 49 {
IgneousGuikas 0:332926683ee3 50 float pwm = alpha*pow(omega,2) + beta*omega;
IgneousGuikas 0:332926683ee3 51 return pwm;
IgneousGuikas 0:332926683ee3 52 }