# Corgi Fpga Driver
## Hardware Configuration
### Actual Motor installed on robot (Motor_F, Motor_H)
| | LF [A] | RF [B] |
| ----------- | -------------------------- | -------------------------- |
| **Motor F** | `CAN_ID 19` `PowerBoard 1` | `CAN_ID 11` `PowerBoard 3` |
| **Motor H** | `CAN_ID 20` `PowerBoard 2` | `CAN_ID 17` `PowerBoard 4` |
| | **LH [D]** | **RH [C]** |
| **Motor F** | `CAN_ID 14` `PowerBoard 5` | `CAN_ID 16` `PowerBoard 7` |
| **Motor H** | `CAN_ID 13` `PowerBoard 6` | `CAN_ID 15` `PowerBoard 8` |

### Relationship between Linkage and Motor

* **LF Module** [A]
* C_Module: `Mod1/CAN0`
* link R (Motor H) `CAN_ID 20`
* link L (Motor F) `CAN_ID 19`
* **RF Module** [B]
* C_Module: `Mod1/CAN1`
* link R (Motor H) `CAN_ID 17`
* link L (Motor F) `CAN_ID 11`
* **RH Module** [C]
* C_Module: `Mod2/CAN0`
* link R (Motor F) `CAN_ID 16`
* link L (Motor H) `CAN_ID 15`
* **LH Module** [D]
* C_Module: `Mod2/CAN1`
* link R (Motor F) `CAN_ID 14`
* link L (Motor H) `CAN_ID 13`

## Frame Definition

## Configuration
使用前請先根據使用需求，更改`./config/config.yaml`內各項參數
### Parameter Definition
* 設定**TCP 通訊架構** 之 `IP` `Port` `Topic`
* `Master_IP` Nodecore 於區網中的 IP
* `Master_port` Nodecore 於區網中的 port
* `Local_IP` fpga_driver 於區網中的 IP
* `Command_topic` FPGA 接收指令的 topic 名稱
* `Publish_topic` FPGA 發布馬達資訊的 topic 名稱

# TCP node communication setup
Master_IP: "127.0.0.1"
Master_port: 9999
Local_IP: "127.0.0.1"

Command_topic: "/fpga/command"
Publish_topic: "/fpga/state"

:::success
**如上述設定，通訊界面為**
Common Message : (開關電、Powerboard Voltage Current ...)
`/fpga/common_control`
`/fpga/common_status`
Publish to :
A -> `/fpga/command1`
B -> `/fpga/command2`
C -> `/fpga/command3`
D -> `/fpga/command4`
Subscribe to :
A -> `/fpga/state1`
B -> `/fpga/state2`
C -> `/fpga/state3`
D -> `/fpga/state4`
:::

### 設定訊息之座標系統
* `Scenario` 設置單足模式或四足模式
* `SingleModule` 模式下之 `Theta-Beta` 座標為學長姐論文上所定義
* `Robot` 模式下由於機器人設計上各模組方向不一，因此統一以B, C模組之`Theta-Beta` frame 為主（亦即對 A D 兩個模組之Beta frame 轉換）

* `Command_type` 為發送指令之模式
* `Theta_Beta`
* `phiR_phiL`

# Experiment scenario: "Robot" or "SingleModule"
Scenario: "Robot"
# Scenario: "SingleModule"

# Command Frame Type: "theta_beta" or "phiR_phiL"
Command_type: "theta_beta"
# Command_type: "phiR_phiL"

### 設定控制頻率

由 FPGA 之 Interrupt Request Handler (IRQ) 觸發
* `MainLoop_period_us` 為主迴圈控制週期 (負責收發指令及回授資訊)
* `CANLoop_period_us` 為CAN迴圈控制週期 (負責CAN-BUS 通訊)

通常以上者設置為相同即可，頻率最快只能設置至1Khz

# Interrupt Setup, Triggered By FPGA IRQ
# IRQ 0
# Loop Period (type int) of the main loop, where controllers were implemented
MainLoop_period_us: 1000
# IRQ 1
# Loop Period (type int) of the loop handling CAN transceiving, at least 1000 us
CANLoop_period_us: 1000

# CAN Bus timeout
CAN_Timeout_us: 500

### 設定各模組之參數
以 `Module A` 為例
* `Enable`
* `CAN_PORT` `C_Module` 連接腳位
* `Motor_R` 右馬達參數 `Motor_L` 左馬達參數
* `FW_Version` Stm32 Firmware Version
* `CAN_ID`
* `Calibration_Bias` 使用制具Hall Calibration 校正之偏移量
* `KP`
* `KI`
* `KD`
* `Torque_Feedfoward` FOC 位置環的扭舉前饋, 通常設置為 0

# MOD A Bias represented in radian
LF_Module:
  Enable: 1
  CAN_PORT: "MOD1CAN0"
  Motor_R:
    FW_Version: 3
    CAN_ID: 20
    Calibration_Bias: 0.056087
    KP: 50
    KI: 0
    KD: 1.5
    Torque_Feedfoward: 0
  Motor_L:
    FW_Version: 3
    CAN_ID: 19
    Calibration_Bias: 0.884449
    KP: 50
    KI: 0
    KD: 1.5
    Torque_Feedfoward: 0

### Powerboard ADC 校正參數

Powerboard_Scaling_Factor:
  - # 1 Power Battery
    Current_Factor: 0.00991631707792460
    Voltage_Factor: 0.013216521172
  - # 2 CPU SBRIO, ORIN
    Current_Factor: 0.00252211539099489
    Voltage_Factor: 0.013184717564 
  - # 3 SPARE 1
    Current_Factor: 0.00253429871857981
    Voltage_Factor: 0.013179732144
  - # 4 SPARE 2
    Current_Factor: 0.00261639240519919
    Voltage_Factor: 0.013202809119
  - # 5 Module 1
    Current_Factor: 0.00259123591590584
    Voltage_Factor: 0.013207462244
  - # 6 Module 2
    Current_Factor: 0.00258549349955540
    Voltage_Factor: 0.013218091064
  - # 7 Module 3
    Current_Factor: 0.00258359558577896
    Voltage_Factor: 0.013152769912
  - # 8 Module 4
    Current_Factor: 0.00258064472112894
    Voltage_Factor: 0.013145054959
  - # 9 Module 5
    Current_Factor: 0.00258864769050496
    Voltage_Factor: 0.013187199073
  - # 10 Module 6
    Current_Factor: 0.00258642289474003
    Voltage_Factor: 0.013175387725
  - # 11 Module 7
    Current_Factor: 0.00258369194007044
    Voltage_Factor: 0.013255058691
  - # 12 Module 8
    Current_Factor: 0.00258666541022275
    Voltage_Factor: 0.013219184356