1. Roll의 균형 값과 모터 속도

1.1 Roll의 균형 값과 모터 속도

드론의 후면을 본 그림이다. 드론이 전진하는 방향이 X축이다. 즉 그림에서 들어가는 축이 x축 +방향. 드론이 ROLL이 시계방향으로 기울어져있기 때문에 $\theta_{roll}$>0이다. 이때 원래 수평으로 되돌아올려면, AD 모터의 속도가 줄고, BC 모터의 속도가 늘어야. BC 쪽 알짜 돌림힘이 강해져 원래 수평으로 돌아온다.

드론이 ROLL이 반시계방향으로 기울어져있기 때문에 $\theta_{roll}$<0이다. 이때 원래 수평으로 되돌아올려면, AD 모터의 속도가 늘고, BC 모터의 속도가 증가해야. AD 쪽 알짜 돌림힘이 강해져 원래 수평으로 돌아온다.

$\theta_{roll}$ > 0이면 수평을 맞춘다고 했을 때
$Error=setPoint - Input$
$Error=0-\theta_{roll}$ <0이다.
즉 Roll의 균형 힘 값은 음수가 되어야 하므로
$Roll_{balancing}<0$으로 표현한다.

Roll의 균형 힘	A, D프로펠러	B, C프로펠러
$Roll_{balancing}<0$	AD 속도 감소	BC 속도 증가
$Roll_{balancing}>0$	AD 속도 증가	BC 속도 감소

---

1.2 Pitch의 균형 값과 모터 속도

드론의 옆면을 본 그림이다. 드론이 전진하는 방향이 X축이다. 그림에서 들어가는 축은 Y축 +방향. 드론이 Pitch이 시계방향으로 기울어져있기 때문에 $\theta_{pitch}$>0이다. 이때 원래 수평으로 되돌아올려면, AB 모터의 속도가 늘고, BC 모터의 속도가 감소해야. AB 쪽 알짜 돌림힘이 강해져 원래 수평으로 돌아온다.

드론의 Pitch가 반시계방향으로 기울어져있기 때문에
$\theta_{pitch}$<0이다. 이때 원래 수평으로 되돌아올려면, AB 모터의 속도가 줄고, CD 모터의 속도가 증가해야. CD 쪽 알짜 돌림힘이 강해져 원래 수평으로 돌아온다.

$\theta_{pitch}$ > 0이면 수평을 맞춘다고 했을 때
$Error=setPoint - Input$
$Error=0-\theta_{pitch}$ <0이다.
즉 Pitch의 균형 힘 값은 음수가 되어야 하므로
$Pitch_{balancing}<0$으로 표현한다.

Pitch의 균형 힘	A, B프로펠러	C, D프로펠러
$Pitch_{balancing}<0$	AB 속도 증가	BC 속도 감소
$Pitch_{balancing}>0$	AB 속도 감소	BC 속도 증가

---

1.3 Yaw의 균형 값과 모터 속도

드론의 윗면을 본 그림이다. 드론이 전진하는 방향이 X축. 그림에서 들어가는 축은 Z축 -방향. 드론을 Yaw가 반시계방향으로 회전하고 싶다면, AC 모터의 속도가 늘고, BC 모터의 속도가 감소해야. 각운동량 보존 법칙에 의해서 프로펠러의 알짜 각속도가 시계방향이기에 드론이 반시계방향으로 회전한다.

• $\theta_{yaw}$>0으로 회전하고 싶다면 $Yaw_{balancing}<0$이어야 하고 AC 속도 증가, BD 속도 감소.
  

드론을 Yaw가 시계방향으로 회전하고 싶다면, BD 모터의 속도가 늘고, AC 모터의 속도가 감소해야. 각운동량 보존 법칙에 의해서 프로펠러의 알짜 각속도가 반시계방향이기에 드론이 시계방향으로 회전한다.

• $\theta_{yaw}$<0으로 회전하고 싶다면 $Yaw_{balancing}>0$이어야 하고 AC 속도 감소, BD 속도 증가.
  

Yaw의 균형 힘	A, C프로펠러	B, D프로펠러
$Yaw_{balancing}>0$	AC 속도 증가	BD 속도 감소
$Yaw_{balancing}>0$	AC 속도 감소	BD 속도 증가

A의 속도가 증가해야 하는 경우

• $Roll_{balancing}>0$
• $Pitch_{balancing}<0$
• $Yaw_{balancing}>0$

B의 속도가 증가해야 하는 경우

• $Roll_{balancing}<0$
• $Pitch_{balancing}<0$
• $Yaw_{balancing}<0$

C의 속도가 증가해야 하는 경우

• $Roll_{balancing}<0$
• $Pitch_{balancing}>0$
• $Yaw_{balancing}>0$

D의 속도가 증가해야 하는 경우

• $Roll_{balancing}>0$
• $Pitch_{balancing}>0$
• $Yaw_{balancing}<0$

throttle은 드론이 상승하기 위해 필요한 프로펠러 회전

motorA_speed = throttle + roll_balancing - pitch_balancing + yaw_balancing;
motorB_speed = throttle - roll_balancing - pitch_balancing - yaw_balancing;
motorC_speed = throttle - roll_balancing + pitch_balancing + yaw_balancing;
motorD_speed = throttle + roll_balancing + pitch_balancing - yaw_balancing;