# Swerve drive (TalonFx with CANcoder)
###### tags: `2023`, `swerve`, `frc`, `PID`, `MK4`
## 格式
### Swerve Module 順序
1, 2, 3, 4, 分別為frontLeft, frontRight, rearLeft, rearRight (儘量都按照這個順序寫程式比較不會亂掉)
<br/>
### Swerve Module Motor 稱呼
<font color="#377D22">turning</font> 用於控制驅動方向
<font color="#ED1C24">drive</font> 用於驅動機器人
### 常數稱呼 (公制單位 m, m/s, s)
<font color="#377D22">軌道寬度 (track width)</font> 兩邊輪子的距離
<font color="#ED1C24">長度 (length) </font> 前後輪子中心的距離
## 程式
### 概念 (前言)
大部分邏輯 WPI 的函式庫裡都已經有了，需要做的是把範例程式所使用的馬達改成實際使用的。
:::info
詳細原理可以大概參考：
https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html
因為官方的 Swerve 程式比較晚推出，所以可以在一些早期的文檔中找到其他團隊自己寫的控制程式，有助於理解原理，但非必要
:::
<br/>
### 第一步 (建立專案)
在創建專案的時候選擇：example -> SwerveControllerCommand
:::info
主要以 Command-based programming 為架構
下面會稍微介紹，想要瞭解更多可以參考 WPI 的文檔
:::
#### Subsystem
打開專案後會看到叫做 Subsystems 的資料夾，主要用來存放實體機器人在程式中對應的部分，例如：底盤控制程式都會在 DriveSubsystem 裡面。
而 SwerveModule 則不是一個 Subsystem, 關鍵差別在於有沒有繼承 WPI 的 SubsystemBase.
SwerveModule 對應到一組 Swerve, 包含一顆turning motor, drive motor, CANcoder.
#### Constant
存放機器人常數的地方，把常數都寫在一起，方便之後修改(改一次而不是在所有用到的地方都要改)
包含軌道寬度、長度、馬達 can ID、CANcoder can ID...
#### RobotContainer
可以當作把程式中機器人各個部位組裝起來的地方。
會建立各個子系統的物件，像是 DriveSubsystem
以及控制器 (Joystick, Xbox controller)
<br/>
### 第二步 (修改 SwerveModule)
:::info
有許多使用到的常數尚未提及，可以先在 Constant 中宣告，讓程式可以編譯，數值取得的方法會在後面說到
:::
#### 2-1 馬達設定
:::info
建議確實理解每一個設定的意義，在尋找錯誤的時候會比較容易
:::
##### 2-1-1
建立 drive motor, turning motor, cancoder
```java
private final WPI_TalonFX m_driveMotor;
private final WPI_TalonFX m_turningMotor;
private final CANCoder m_turningEncoder;
```
<br/>
##### 2-1-2
在建構式建立馬達實例
```java
m_driveMotor = new WPI_TalonFX(driveMotorChannel);
m_turningMotor = new WPI_TalonFX(turningMotorChannel);
m_turningEncoder = new CANCoder(turningMotorEncoderChannel);
```
<br/>
##### 2-1-3
在建構式(與類別同名的方法)中初始化、設定馬達
```java
// 避免有其他以前的設定
m_driveMotor.configFactoryDefault();
m_turningMotor.configFactoryDefault();
m_turningEncoder.configFactoryDefault();
```
<br/>
##### 2-1-4
感測器設定
```java
// 讓 CANcoder 以絕對位置模式啟動，而非相對位置
// 絕對位置模式即使機器人斷電重開還是能夠辨認目前的角度，方便我們知道 turning motor 的朝向
m_turningEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
// 讓 encoder 的數值範圍在 -180 ~ 180 之間
m_turningEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180);
// turningEncoderReversed 是建構式的參數，設定正反轉
m_turningEncoder.configSensorDirection(turningEncoderReversed);
// cancoderOffset 是建構式的參數，設置絕對位置的偏移量
// 之後會寫在 Constants 裡面的，後面會詳細講
m_turningEncoder.configMagnetOffset(cancoderOffset);
// 把絕對位置作為相對位置的初始值，之後取用相對位置就可以
m_turningEncoder.setPositionToAbsolute();
```
<br/>
##### 2-1-5
把 CANcoder 設成 turning motor 的感測器，方便使用 MotionMagic 或 Position closed loop 等 Talon 內建的閉環
:::info
使用馬達控制器的閉環可以減少 Roborio 的計算量，並且由於馬達控制器通常可以以更高的頻率運行閉環控制，會更加精準，減少測量的延遲
:::
```java
// talon 可以設置兩個感測器，第二個參數0 表示將 0號感測器設置成 CANcoder
m_turningMotor.configRemoteFeedbackFilter(m_turningEncoder, 0);
// 選中設置好的0號遠端感測器 (遠端表示不是 falcon 內建的感測器)
m_turningMotor.configSelectedFeedbackSensor(RemoteFeedbackDevice.RemoteSensor0, 0, 10);
// 把 1號感測器設為 NONE, 避免自動選中集成編碼器
m_turningMotor.configSelectedFeedbackSensor(FeedbackDevice.None, 1, 10);
```
<br/>
drive 的感測器則使用集成編碼器就可以了
```java
m_driveMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
```
<br/>
##### 2-1-6
設定馬達輸出正反轉、傳感器相位
三個變數都用建構式的參數傳入，方便我們把正反轉、及上面的絕對編碼器偏移都寫在 Constants 中，並給每個 Swerve Module 不同的值
```java
m_driveMotor.setInverted(driveReversed);
m_driveMotor.setSensorPhase(driveEncoderReversed);
m_turningMotor.setInverted(turningMotorReversed);
```
<br/>
##### 2-1-7
設定沒有輸出時的模式
```java
// 設定為 Brake, 避免 drive 馬達在轉動時也改變到角度
m_turningMotor.setNeutralMode(NeutralMode.Brake);
// drive 馬達則是 Coast, Brake 都可以，個人習慣是 Coast, 方便推動機器人測試
m_driveMotor.setNeutralMode(NeutralMode.Coast);
```
<br/>
#### 2-2 新增得到感測器狀態的方法 (方便修改其他地方)
##### 2-2-1
角度傳換成原始單位的方法
```java
// 把從 CANcoder 取得的角度值轉換成原始單位
// CANcoder 轉一圈會增加 4096 個原始單位
// 因此將 角度 / 360 可以知道目前是轉了幾分之幾圈
// 乘上 4096，將幾分之幾圈以原始單位表示
private double deg2raw(double deg) {
return deg / 360 * 4096;
}
```
:::info
falcon 的集成編碼器旋轉一圈是增加 2048 (分辨率)，但是將 CANcoder設作 falcon 的傳感器之後，分辨率就會變成 CANcoder 的分辨率 4096
:::
<br/>
##### 2-2-2
drive encoder position 的方法
```java
// 回傳 drive 的 encoder 位置，並將原始單位變為公尺
public double getDriveEncoderPosition() {
return m_driveMotor.getSelectedSensorPosition() * ModuleConstants.kDriveCoefficient; // m
}
```
<br/>
##### 2-2-3
drive encoder velocity 的方法
```java
// 回傳 drive 的速度，因為 talon 函式回傳的是 raw unit per 100ms，所以除了將長度單位換成公尺外，也需要乘10，將100毫秒變成秒
public double getDriveEncoderVelocity() {
return m_driveMotor.getSelectedSensorVelocity() * ModuleConstants.kDriveCoefficient * 10; // m/s
}
```
<br/>
##### 2-2-4
turning encoder angle 的方法
```java
// 回傳 turning 的角度，並將原始單位換成角度
public double getTurningEncoderAngle() {
return m_turningMotor.getSelectedSensorPosition() / 4096.0 * 360.0; // deg
}
```
<br/>
##### 2-2-5
建立好上面4個方法之後，就可以將其他地方改成對應的函式，下面只提供一個例子
```java
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(getDriveEncoderPosition(), Rotation2d.fronDegrees(getTurningEncoderAngle()));
}
```
<br/>
#### 2-3 修改 setDesiredState 方法
##### 2-3-1
因為希望使用 Talon 內建的 PID, 所以我們需要在剛剛設定馬達的地方加上 PID 值，以及 MotionMagic 會需要的兩個設定
```java
// 設定 PID, 數值需要自己調整 (未調整前可能會沒辦法動)
// 附上純底盤時的參考數值，如果要運作得更好仍然需要依實際情況調整
m_driveMotor.config_kF(0, 0); // 0.065
m_driveMotor.config_kP(0, 0); // 0.15
m_driveMotor.config_kI(0, 0);
m_driveMotor.config_kD(0, 0);
m_turningMotor.config_kF(0, 0); // 0.14
m_turningMotor.config_kP(0, 0); // 1.2
m_turningMotor.config_kI(0, 0);
m_turningMotor.config_kD(0, 0);
```
<br/>
##### 2-3-2
MotionMagic 的參數
:::info
MotionMagic 和 position closed loop 功能類似，都能讓電機轉到指定的 encoder 位置
但是 MotionMagic 可以遵循我們設定的最大加速度、最大速度，讓動作更流暢，避免一般 PID 的大震盪
詳細可以參考 CTRE DOC
:::
```java
// 設定最大加速度
m_turningMotor.configMotionAcceleration(4096);
// 設定最大速度
m_turningMotor.configMotionCruiseVelocity(5108);
```
<br/>
##### 2-3-3
setDesiredState 方法中，
```java
SwerveModuleState state =
SwerveModuleState.optimize(desiredState, Rotation2d.fronDegrees(getTurningEncoderAngle()));
```
是用來優化轉向的，例如，當馬達需要 180 度時，可以優化成旋轉 0 度，並反轉 drive 的輸出
<br/>
##### 2-3-4
接著，將優化過的目標狀態設給馬達
```java
// 將 m/s 轉換成 raw unit / 100ms, 並設給 drive
m_driveMotor.set(ControlMode.Velocity, state.speedMetersPerSecond / ModuleConstants.kDriveCoefficient / 10.0);
// 將角度轉換成 raw unit, 並設給 turning
m_turningMotor.set(ControlMode.MotionMagic, deg2raw(state.angle.getDegrees()));
```
:::success
到此大概就改完這個部份了，其實主要的部份都圍繞在單位轉換上，需要注意個個函式輸入輸出的單位。
:::
<br/>
### 第三步 (修改 DriveSubsystem)
將慣性測量單元換成我們使用的硬體，以 Pigeon2 為例
```java
// 宣告
private final WPI_Pigeon2 m_gyro = new WPI_Pigeon2(DriveConstants.kPigeon2Port);
// 建構式中的初始化
public DriveSubsystem() {
// 避免之前的設定殘留
m_gyro.configFactoryDefault();
// 把所有設定都包裝成一個物件
Pigeon2Configuration config = new Pigeon2Configuration();
// 安裝的姿態
config.MountPosePitch = 0;
config.MountPoseRoll = 0;
config.MountPoseYaw = 0;
config.EnableCompass = true;
config.DisableNoMotionCalibration = false;
config.DisableTemperatureCompensation = false;
config.enableOptimizations = false;
m_gyro.configAllSettings(config);
// 重設方位
m_gyro.setYaw(0);
}
```
:::info
使用帶有 WPI_ 前綴的 Pigeon2 類別，會自動反轉 Yaw 讀值，自動符合其他 WPI 庫的習慣
:::
<br/>
### 第四步 (取得各項常數)
:::warning
需要有一台機器人!!!!!!!!
:::
#### 4-1 取得 CANcoder 偏移量
將輪子都轉向同一個方向
**確保輸出為正的時候，輪子都向前轉**
e.g., percentOutput 0.3 每顆輪子都是向前轉
確認完成後，就可以用 Phoenix Tuner 的 self-test 功能得到 目前 CANcoder 的值，加上負號即是該顆 CANcoder 的偏移量
```java
// example (in Constants.java)
public static final double kCancoderOffset1 = -212.43164062;
public static final double kCancoderOffset2 = -164.1796825;
public static final double kCancoderOffset3 = -319.921875;
public static final double kCancoderOffset4 = -68.46679688;
```
:::info
因為我們設置的偏移量會在讀值時加進感測器的數值中，因此需要加上負號
:::
:::warning
確定馬達轉向可以避免之後出現奇怪的問題
:::
<br/>
#### 4-2 計算 kDriveCoefficient
該常數用來把 Drive 馬達的讀數(raw unit) 轉換成以公尺為單位
我們知道 falcon 轉一圈是 2048, 輪胎轉一圈在不打滑的情況下會前進輪胎的圓周長；若有齒輪比(通常是減小)，則輪子轉一圈實際上可能是馬達轉好幾圈，因此計算方式為：
<br/>
$WheelDiameter * Math.PI\; /\; 2048\; /\; gearRatio$
```java
public static final double kDriveCoefficient =
0.1 * Math.PI / kEncoderCPR / 8.14;
```
:::info
齒輪比通常可以在產品介紹上找到，以 MK4 standard 為例，是8.14，代表馬達轉 8.14 圈，輪胎轉一圈
:::
<br/>
#### 4-3 馬達、感測器正反轉
如果在 4-1 有確實做好，馬達的部分應該不需要調整，否則就是反轉到輸出的正向與機器人正向相同
**感測器部分需要確定馬達輸出正向會讓讀值增加**
:::info
可以用 Phoenix Tuner 的 Control, self-test 功能
:::
```java
public static final boolean kFrontLeftDriveEncoderReversed = true;
public static final boolean kRearLeftDriveEncoderReversed = false;
public static final boolean kFrontRightDriveEncoderReversed = true;
public static final boolean kRearRightDriveEncoderReversed = false;
```
<br/>
#### 4-4 Track width, length
測量左右輪子中心距離、前後輪子中心距離
```java
public static final double kTrackWidth = 0.5;
public static final double kWheelBase = 0.5;
```
### 第五步 (馬達設定優化)
#### 5-1 Current limit
由於底盤馬達需要承載整個機器人的重量，加上一些寬鬆的電流保護可以避免馬達受損
```java
// 啟用電流限制, 最大電流 30A, 瞬間最大電流 50A, 0.1秒以下視為瞬間
var voltage = new SupplyCurrentLimitConfiguration(true, 30, 50, 0.1);
m_driveMotor.configSupplyCurrentLimit(voltage);
```
:::warning
太嚴格的電流限制可能會導致馬達不能動作
:::
<br/>
#### 5-2 Voltage compensation
設定電壓補償可以讓機器人在該設定電壓之上都正常運作，避免因為電池狀態造成輸出不一致
```java
m_driveMotor.enableVoltageCompensation(true);
m_driveMotor.configVoltageCompSaturation(9);
m_turningMotor.enableVoltageCompensation(true);
m_turningMotor.configVoltageCompSaturation(9);
```
:::info
設定為 9, 表示當 percentOutput 1 時，會輸出 9V 的電壓；
但若是電池實際電壓低於 9V, 仍會造成輸出不一致，但是實際比賽中不應該讓電池電壓那麼低
:::
<br/>
#### 5-3 Neutral Deadband
設定死區，當目標輸出小於設定值，就不輸出，可以讓無用的過小輸出歸零，避免在閉環控制時不斷消耗電量
```java
m_driveMotor.configNeutralDeadband(0.05);
m_turningMotor.configNeutralDeadband(0.1);
```
:::info
更多 NeutralDeadband 的資訊：https://v5.docs.ctr-electronics.com/en/stable/ch13_MC.html#peak-nominal-outputs
:::
<br/>
#### 5-4 Nominal output
設定 Nominal output, 可以用來取代 PID 中的 I, 尤其轉向馬達需要完全精準
```java
m_turningMotor.configNominalOutputForward(0.095);
m_turningMotor.configNominalOutputReverse(-0.095);
```
:::info
Nominal output 可以將高於 Neutral Deadband 設定值的輸出提高，確保該輸出能夠讓馬達運動
更多資訊：https://v5.docs.ctr-electronics.com/en/stable/ch13_MC.html#peak-nominal-outputs
:::
### 最後
調整 PID 值，第一個可以從 turning 的精確度開始，再調整 drive 的速度精確度；
:::info
turning 應該調到完全精準，即使是微小的誤差，也會在長距離的直線行駛中變得明顯
:::
~~希望會有因為這篇文章而成功的人類థ౪థ~~
Sign in with Wallet
Connect another wallet