# FRC 底盤教學 (C++ / WPILib)

在 FRC 中我們主要是用 WPILib 來編寫程式，可以從 FRC 官網下載到：
🔗 https://docs.wpilib.org/en/stable/docs/zero-to-robot/step-2/wpilib-setup.html

我們隊主要是用 C++ 來編程的，以下的程式碼範例也都會用 C++ 來編寫。
開始寫機台程式之前建議熟悉一下 C++ 的指標，Command Base 編碼會用到。

會用到的程式庫有：
**REV 的 CANSparkMax**（動力輸出）
**CTRE Phoenix 的 TalonSRX**（轉向控制）

如果使用其他馬達控制器，就使用對應的程式庫。

## 底盤種類
### 1.小白輪(Tank Drive)
**最常見，也最簡單編程的底盤**
### 2.麥輪(Mecanum Drive)
**移動比起坦克式還要靈活，可以左右飄移，但容易打滑，抓地力比較小**
### 3.全向輪(Swerve Drive)
(只有一個輪子的圖片，其實有四個)
**最靈活的底盤，但在編程上是最難的。**

## 1. 小白輪 (Tank Drive)
**假設：**
1. 左輪力為 l_power
2. 右輪力為 r_power
3. 搖桿輸入為 x（轉向）與 y（前進）
4. k 為轉向靈敏度常數

**狀況分析**
1. 向前進時 (y = 1, x = 0)
**左右輪都向前**
2. 向右轉時 (y = 0, x = 1)
**左輪向前、右輪向後**

所以：
**l_power = y + x * k
r_power = y - x * k**

**Bounding**
因為馬達輸出範圍是 **[-1, 1]**，需要限制最大值。

**範例 (cpp)**

void Robot::TeleopPeriodic(){
    
    // 搖桿輸入
    double x = joystick.GetX();
    double y = joystick.GetY();
    
    // 計算馬達輸出的力
    double k = 1.0;
    double l_power = y + x * k;
    double r_power = y - x * k;
    
    // Bounding
    l_power = std::clamp(l_power, -1.0, 1.0);
    r_power = std::clamp(r_power, -1.0, 1.0);
    
    // 輸出馬達
    left_motor.Set(l_power);
    right_motor.Set(r_power);
}

## 2. 麥輪 (Mecanum Drive)
麥輪有 4 個輪子，需考慮三種輸入：
**x：左右移動
y：前後移動
rx：旋轉**

**狀況分析：**
1. 前進 (y = 1, x = 0, rx = 0)
**所有輪子都往前。**

2. 右飄移 (y = 0, x = 1, rx = 0)
**左前與右後往前；右前與左後往後。**

3. 順時鐘旋轉 (y = 0, x = 0, rx = 1)
**左邊輪往前、右邊輪往後。**

綜合公式
**front_left = y + x + rx
back_left = y - x + rx
front_right = y - x - rx
back_right = y + x - rx**

**Bounding**
四個輪子的力會互相影響，不能單獨限制。
正確做法是用：
**d = max(|x| + |y| + |rx|, 1)**
所有輸出都除以 d。

**範例 (cpp)**

void Robot::TeleopPeriodic(){

    // 搖桿輸入
    double x  = joystick.GetX();
    double y  = joystick.GetY();
    double rx = joystick.GetRawAxis(4); // 右搖桿的 X 軸
    
    // 最大值做分母
    double d = std::max({fabs(x) + fabs(y) + fabs(rx), 1.0});
    
    // 計算輸出
    double front_left  = (y + x + rx) / d;
    double back_left   = (y - x + rx) / d;
    double front_right = (y - x - rx) / d;
    double back_right  = (y + x - rx) / d;
    
    // 輸出馬達
    front_left_motor.Set(front_left);
    back_left_motor.Set(back_left);
    front_right_motor.Set(front_right);
    back_right_motor.Set(back_right);
}

## 3. 全向輪 (Swerve Drive)

全向輪的輪力計算較複雜。
首先需要知道：

1. 機器人長度 L
2. 機器人寬度 W
3. 搖桿輸入：speed（前後）、drift（左右）、turn（旋轉）
4. 對角線長度 **r = sqrt(L² + W²)**

力的組合公式
**A = drift - turn * (L / r)
B = drift + turn * (L / r)
C = speed - turn * (W / r)
D = speed + turn * (W / r)**

各輪速度
**front_right = sqrt(B² + C²)
front_left = sqrt(B² + D²)
back_left = sqrt(A² + D²)
back_right = sqrt(A² + C²)**

Normalize
**d = max(front_left, back_left, front_right, back_right, 1)**

然後每個輪速都除以 d。

各輪角度
**front_right_angle = atan2(B, C)
front_left_angle = atan2(B, D)
back_left_angle = atan2(A, D)
back_right_angle = atan2(A, C)**

角度單位轉成度數：
**angle = atan2(y, x) * 180 / π**

**範例 (cpp)**

void Robot::TeleopPeriodic(){
    // 搖桿輸入
    double speed = joystick.GetY();
    double drift = joystick.GetX();
    double turn  = joystick.GetRawAxis(4);
    
    // 機器人尺寸
    double L = 0.5;
    double W = 0.5;
    double r = sqrt(L*L + W*W);

    // 計算力分量
    double A = drift - turn * (L / r);
    double B = drift + turn * (L / r);
    double C = speed - turn * (W / r);
    double D = speed + turn * (W / r);

    // 輪速
    double front_right = sqrt(B*B + C*C);
    double front_left  = sqrt(B*B + D*D);
    double back_left   = sqrt(A*A + D*D);
    double back_right  = sqrt(A*A + C*C);

    // Normalize
    double d = std::max({front_left, back_left, front_right, back_right, 1.0});
    front_left  /= d;
    back_left   /= d;
    front_right /= d;
    back_right  /= d;

    // 計算角度 (degree)
    double front_right_angle = atan2(B, C) * 180 / M_PI;
    double front_left_angle  = atan2(B, D) * 180 / M_PI;
    double back_left_angle   = atan2(A, D) * 180 / M_PI;
    double back_right_angle  = atan2(A, C) * 180 / M_PI;

    // 輸出馬達
    front_left_motor.Set(front_left);
    back_left_motor.Set(back_left);
    front_right_motor.Set(front_right);
    back_right_motor.Set(back_right);
    
    // 設定轉向馬達角度
    front_left_angle_motor.Set(ControlMode::Position, front_left_angle);
    back_left_angle_motor.Set(ControlMode::Position, back_left_angle);
    front_right_angle_motor.Set(ControlMode::Position, front_right_angle);
    back_right_angle_motor.Set(ControlMode::Position, back_right_angle);
}