# 萬向輪底盤程式工程筆記

## 前言

在升上高三後，我持續利用課餘時間參與並協助FRC機器人競賽的程式組開發工作。而今年的自動程式，我選擇讓學弟妹撰寫，並提供必要的指導與建議，以培養他們的技術能力。

高三成員團體照:

## 1. 萬象輪底盤技術解析

### 1.1 萬向輪（Swerve）概念介紹

Swerve 是一種先進的底盤運行模式，其核心特點是每個輪子都能獨立控制其速度與方向。若採用四輪驅動的 Swerve 設計，其組成為四個獨立的驅動單元（模組），每個模組均配備：


- **Drive Motor**: 控制輪子的轉速（前進後退）
- **Turning Motor**: 控制輪子的旋轉角度（方向）
- **編碼器系統**:
- **Drive Encoder**: 監測輪子的轉速
- **Turning Encoder**: 監測輪子的角度
- **CANcoder**: 提供絕對角度測量

透過這種設計，Swerve 底盤可以實現向任意方向移動，同時能夠獨立控制底盤的旋轉角度，提供極高的機動性與靈活度。

## 2. 技術基礎：深入理解 Swerve 模組架構

### 2.1 核心組件解析

#### 2.1.1 驅動系統
- **驅動馬達 (Drive Motor)**：NEO 無刷馬達，負責控制輪子的轉速
- **轉向馬達 (Turning Motor)**：NEO 無刷馬達，負責控制輪子的方向
- **CANcoder**：專用絕對角度編碼器，提供精確的角度測量


#### 2.1.2 控制系統
- **SparkMax 控制器**：用於控制 NEO 無刷馬達
- **PID 控制器**：確保精確角度控制與平滑轉向

### 2.2 程式架構設計

#### 2.2.1 物件導向結構

public class SwerveMod {
    // 馬達宣告
    private final CANSparkMax driving;  // 控制輪子前進後退
    private final CANSparkMax turning;  // 控制輪子轉向
    
    // 編碼器宣告
    private final RelativeEncoder drivingEncoder;  // 測量驅動馬達轉速和距離
    private final RelativeEncoder turningEncoder;  // 測量轉向馬達轉速
    private final CANCoder cancoder;               // 提供轉向絕對角度
    
    // PID控制器宣告，用於精確控制轉向角度
    private final PIDController turningPidController = new PIDController(0.0113, 0.0115, 0);
    
    // 物理參數
    private final double wheelDiameter = 0.1016;  // 輪子直徑（公尺）
    private final double gearRatio = 6.12;        // 齒輪比
    
    // 建構子與方法（詳見後續內容）
}

#### 2.2.2 建構子實作

public SwerveMod(int drivingID, int turningID, int cancoderID, Rotation2d offset) {
    // 初始化馬達
    driving = new CANSparkMax(drivingID, MotorType.kBrushless);
    turning = new CANSparkMax(turningID, MotorType.kBrushless);
    
    // 初始化編碼器
    drivingEncoder = driving.getEncoder();
    turningEncoder = turning.getEncoder();
    cancoder = new CANCoder(cancoderID);
    
    // 配置硬體參數
    configCANcoder(offset);
    configSparkMax();
    
    // 設定PID為連續輸入，處理角度週期性(-180° 到 180°)
    turningPidController.enableContinuousInput(-180, 180);
}

### 2.3 核心配置方法

#### 2.3.1 CANcoder 配置

private void configCANcoder(Rotation2d offset) {
    CANcoderConfiguration config = new CANcoderConfiguration();
    config.sensorDirection = true;  // 設定感測器方向為逆時針為正向
    config.magnetOffsetDeg = offset.getDegrees();  // 設定磁性偏移角度
    config.sensorTimeBase = SensorTimeBase.PerSecond;
    cancoder.configAllSettings(config);
}

#### 2.3.2 SparkMax 配置

private void configSparkMax() {
    // 重置馬達控制器配置
    driving.restoreFactoryDefaults();
    turning.restoreFactoryDefaults();
    
    // 設定驅動馬達編碼器轉換因子（從旋轉數到移動距離）
    drivingEncoder.setPositionConversionFactor(
        wheelDiameter * Math.PI / gearRatio
    );
    drivingEncoder.setVelocityConversionFactor(
        wheelDiameter * Math.PI / (60.0 * gearRatio)
    );
    
    // 設定轉向馬達編碼器轉換因子（從旋轉數到角度）
    turningEncoder.setPositionConversionFactor(360.0 / 12.8);
    
    // 設定馬達控制器參數
    driving.setIdleMode(IdleMode.kBrake);
    turning.setIdleMode(IdleMode.kBrake);
    driving.setSmartCurrentLimit(40);
    turning.setSmartCurrentLimit(20);
    
    // 將設定燒入控制器永久儲存
    driving.burnFlash();
    turning.burnFlash();
}

## 3. 系統整合：底盤控制架構

### 3.1 SwerveChassis 類設計

public class SwerveChassis extends SubsystemBase {
    // 四個 Swerve 模組的定義
    private final SwerveMod frontLeft, frontRight, backLeft, backRight;
    
    // 陀螺儀，用於場地中心控制
    private final AHRS gyro;
    
    // 模組間距參數（單位：米）
    private final double L = 0.487;  // 對角線距離
    private final double trackWidth = 0.37;  // 寬度
    private final double wheelBase = 0.315;  // 長度
    
    // 速度限制
    private final double maxSpeed = 4.935;  // 最大平移速度 (m/s)
    private final double maxOmega = 10.13;  // 最大旋轉速度 (rad/s)
    
    public SwerveChassis() {
        // 初始化陀螺儀
        gyro = new AHRS(SPI.Port.kMXP);
        resetGyro();
        
        // 初始化四個 Swerve 模組
        frontLeft = new SwerveMod(1, 2, 9, Rotation2d.fromDegrees(91.23));
        frontRight = new SwerveMod(3, 4, 10, Rotation2d.fromDegrees(10.10));
        backLeft = new SwerveMod(5, 6, 11, Rotation2d.fromDegrees(15.73));
        backRight = new SwerveMod(7, 8, 12, Rotation2d.fromDegrees(69.08));
    }
    
    // 重設陀螺儀方法
    public void resetGyro() {
        gyro.reset();
    }
    
    // 主要驅動方法
    public void drive(Translation2d targetVelocity, double targetOmega, boolean fieldRelative) {
        // 限制最大速度
        targetVelocity = limitVelocity(targetVelocity);
        targetOmega = Math.min(Math.max(targetOmega, -maxOmega), maxOmega);
        
        // 場地相對坐標轉換
        if (fieldRelative) {
            targetVelocity = targetVelocity.rotateBy(
                Rotation2d.fromDegrees(-gyro.getAngle())
            );
        }
        
        // 計算每個模組的速度和角度
        SwerveModuleState[] states = new SwerveModuleState[4];
        
        // 計算各點相對於中心的位置
        Translation2d fl = new Translation2d(wheelBase/2, trackWidth/2);
        Translation2d fr = new Translation2d(wheelBase/2, -trackWidth/2);
        Translation2d bl = new Translation2d(-wheelBase/2, trackWidth/2);
        Translation2d br = new Translation2d(-wheelBase/2, -trackWidth/2);
        
        // 計算各點的速度向量
        Translation2d flVel = targetVelocity.plus(new Translation2d(0, targetOmega).times(fl.getNorm()));
        Translation2d frVel = targetVelocity.plus(new Translation2d(0, targetOmega).times(fr.getNorm()));
        Translation2d blVel = targetVelocity.plus(new Translation2d(0, targetOmega).times(bl.getNorm()));
        Translation2d brVel = targetVelocity.plus(new Translation2d(0, targetOmega).times(br.getNorm()));
        
        // 設置每個模組的目標狀態
        frontLeft.setDesiredState(new SwerveModuleState(flVel.getNorm(), flVel.getAngle()));
        frontRight.setDesiredState(new SwerveModuleState(frVel.getNorm(), frVel.getAngle()));
        backLeft.setDesiredState(new SwerveModuleState(blVel.getNorm(), blVel.getAngle()));
        backRight.setDesiredState(new SwerveModuleState(brVel.getNorm(), brVel.getAngle()));
    }
    
    // 速度限制方法
    private Translation2d limitVelocity(Translation2d velocity) {
        double magnitude = velocity.getNorm();
        if (magnitude > maxSpeed) {
            return velocity.times(maxSpeed / magnitude);
        }
        return velocity;
    }
}

### 3.2 速度參數計算方法詳解

#### 3.2.1 最大平移速度計算
最大平移速度(maxSpeed)是根據馬達規格、齒輪比和輪徑計算得出：

1. **將馬達轉速從 RPM 轉為每秒轉數**：

馬達每秒轉速 = freespeed / 60
= 5676 / 60
= 94.6 轉/秒

2. **考慮減速比，計算輪子每秒轉速**：

輪子每秒轉速 = 馬達每秒轉速 / kDriveMotorGearRatio
= 94.6 / 6.12
≈ 15.46 轉/秒

3. **計算輪子每秒行駛距離（maxspeed）**：
- 輪子周長：

kWheelDiameterMeters * Math.PI
= 0.1016 * 3.14159
≈ 0.3192 米/轉

- 最大速度：

輪子每秒轉速 * 周長
= 15.46 * 0.3192
≈ 4.935 米/秒

#### 3.2.2 最大旋轉速度計算
最大旋轉速度(maxOmega)是根據最大平移速度和轉動半徑計算得出：

1. **計算轉動半徑 L**：

L = √(wheelBase² + trackWidth²)
= √(0.315² + 0.37²)
≈ √(0.0992 + 0.1369)
≈ √0.2361
≈ 0.487 米

2. **計算最大旋轉速度**：

maxOmega = maxSpeed / L
= 4.935 / 0.487
≈ 10.13 弧度/秒

### 3.3 RobotContainer 整合

public class RobotContainer {
    // 底盤系統初始化
    private final SwerveChassis chassis = new SwerveChassis();
    
    // 控制器宣告
    private final CommandXboxController controller = new CommandXboxController(0);
    private final CommandJoystick joystick = new CommandJoystick(1);
    
    // 速度參數
    private final double maxSpeed = 4.935; // 米/秒
    private final double maxOmega = 10.13; // 弧度/秒
    
    public RobotContainer() {
        // 底盤預設命令：使用RunCommand持續執行chassis.drive
        chassis.setDefaultCommand(
            new RunCommand(
                () -> chassis.drive(
                    // 從左搖桿獲取平移指令(X、Y軸)
                    new Translation2d(
                        -controller.getLeftY() * maxSpeed,
                        -controller.getLeftX() * maxSpeed
                    ),
                    // 從右搖桿獲取旋轉指令(X軸)
                    -controller.getRightX() * maxOmega,
                    true  // 使用場地視角控制
                ),
                chassis
            )
        );
        
        // 配置按鈕綁定
        configureBindings();
    }
    
    private void configureBindings() {
        // 重設陀螺儀按鈕
        controller.start().onTrue(new InstantCommand(() -> chassis.resetGyro()));
        
        // 其他按鈕綁定...
    }
}

## 4. 技術挑戰與解決方案

### 4.1 校正偏移量挑戰

#### 4.1.1 問題分析
在使用 CANcoder 進行絕對角度測量時，我們面臨了各模組間偏移量校正需要反覆調整的問題。這主要由以下因素導致：
- 機械安裝誤差
- 外部環境干擾
- 初始校準偏差
- 使用過程中的累積誤差

#### 4.1.2 解決方案
1. **精確測量初始位置**：
- 使用精密量具測量每個模組的初始角度
- 建立標準化的校準流程

2. **軟體補償**：
- 在 `configCANcoder` 方法中應用偏移量補償
- 根據測試數據動態調整偏移量

3. **機械優化**：
- 加強 CANcoder 安裝結構，減少晃動
- 定期檢查和維護機械連接處

### 4.2 PID 參數調整

#### 4.2.1 問題分析
精確控制 Swerve 模組的角度需要適當的 PID 參數，但調整過程中我們遇到了以下挑戰：
- 過度補償導致系統振盪
- 不足的補償導致響應緩慢
- 負載變化導致的參數失效

#### 4.2.2 解決方案：系統性 PID 調參方法
我們採用了以下科學步驟進行 PID 參數調整：

1. **初始設置**：
- 清零所有參數 (Kp=0, Ki=0, Kd=0)
- 從最簡單的 P 控制開始

2. **調節 Kp（比例項）**：
- 小步漸進增加 Kp 值
- 目標：系統能大致接近設定值，但允許存在穩態誤差
- 觀察點：速度

3. **調節 Ki（積分項）**：
- 在 Kp 基礎上，緩慢增加 Ki
- 目標：減少或消除穩態誤差
- 觀察點：系統穩定性和積分飽和問題

4. **調節 Kd（微分項）**：
- 謹慎增加 Kd 值
- 目標：減少超調和振盪
- 觀察點：噪聲敏感度和響應平滑度

5. **整體優化**：
- 在實際運行環境中測試參數
- 根據不同運行狀態微調參數

最終確定的 PID 參數是經過大量測試後得出的最佳值：Kp=0.0113, Ki=0.0115, Kd=0。
## 5. 進階子系統：電梯控制機構

電梯系統用於將機器人的抓取機構提升到不同高度以完成得分任務。


### 5.1 電梯子系統設計

public class Elevator extends SubsystemBase {
    // 馬達與控制器宣告
    private final CANSparkMax elevatorMotor;
    private final SparkMaxPIDController pidController;
    private final RelativeEncoder encoder;
    private final DigitalInput limitSwitch;  // 極限開關防止電梯過度下降
    
    // 高度轉換參數
    private final double gear_diameter = 1.45 * 0.0254;  // 齒輪直徑，單位轉為公尺
    private final double gear_circle = gear_diameter * Math.PI;  // 齒輪周長
    
    public Elevator() {
        // 初始化馬達和控制器
        elevatorMotor = new CANSparkMax(13, MotorType.kBrushless);
        pidController = elevatorMotor.getPIDController();
        encoder = elevatorMotor.getEncoder();
        limitSwitch = new DigitalInput(0);
        
        // 配置馬達控制器
        configSparkMax();
    }
    
    // 配置SparkMax控制器
    private void configSparkMax() {
        elevatorMotor.restoreFactoryDefaults();
        encoder.setPosition(0); // 設定初始位置
        elevatorMotor.setIdleMode(IdleMode.kBrake); // 設定煞車模式
        elevatorMotor.setSmartCurrentLimit(40); // 限制電流
        
        // 設定PID參數
        pidController.setP(0.15);
        pidController.setI(0.0);
        pidController.setD(0.1);
        
        elevatorMotor.burnFlash(); // 將設定寫入控制器
    }
    
    // 設定馬達功率方法
    public void setPower(double p) {
        // 安全機制：碰到底部極限開關且要往下移動時停止
        if (limitSwitch.get() && p < 0) {
            elevatorMotor.set(0);
            encoder.setPosition(0);  // 設定碰到極限開關位置為初始位置
        } else {
            elevatorMotor.set(p);
        }
    }
    
    // 停止電梯
    public void stop() {
        elevatorMotor.set(0);
    }
    
    // 獲取當前高度
    public double getPosition() {
        return encoder.getPosition() / 4 * gear_circle;  // 將馬達旋轉數轉換為高度
    }
    
    // 移動到指定高度方法
    public void moveToPosition(double position) {
        // 根據齒比轉換高度為馬達轉數
        double setpoint = position / gear_circle * 4;  // 4為齒比
        pidController.setReference(setpoint, CANSparkMax.ControlType.kPosition);
    }
    
    @Override
    public void periodic() {
        // 定期檢查極限開關
        if (limitSwitch.get()) {
            encoder.setPosition(0);  // 重設位置
        }
    }
}

### 5.2 電梯命令設計

public class ElevatorMoveToPositionCmd extends CommandBase {
    private final Elevator elevator;
    private final double targetPosition;
    
    public ElevatorMoveToPositionCmd(Elevator elevator, double targetPosition) {
        this.elevator = elevator;
        this.targetPosition = targetPosition;
        addRequirements(elevator);
    }
    
    @Override
    public void initialize() {
        // 命令初始化時無需操作
    }
    
    @Override
    public void execute() {
        elevator.moveToPosition(targetPosition);
    }
    
    @Override
    public boolean isFinished() {
        return Math.abs(elevator.getPosition() - targetPosition) < 0.05;  // 容差5cm
    }
    
    @Override
    public void end(boolean interrupted) {
        elevator.stop();  // 命令結束時停止電梯
    }
}

### 5.3 電梯按鈕綁定

// 在RobotContainer的configureBindings方法中
private void configureBindings() {
    // 電梯控制按鈕
    joystick.x().onTrue(new ElevatorMoveToPositionCmd(elevator, 0.15));  // X鍵將電梯移動到0.15米高度
    joystick.y().onTrue(new ElevatorMoveToPositionCmd(elevator, 0.475));  // Y鍵將電梯移動到0.475米高度
    joystick.b().onTrue(new ElevatorMoveToPositionCmd(elevator, 0.35));  // B鍵將電梯移動到0.35米高度
}

## 6. 高級機械控制：機械臂系統

機械臂用於控制機器人的抓取角度，可以提前抓取道具，抬高後放進得分區。

### 6.1 機械臂子系統設計

public class Arm extends SubsystemBase {
    // 馬達與控制器宣告
    private final CANSparkMax armMotor;
    private final SparkMaxPIDController pidController;
    private final RelativeEncoder encoder;
    
    // 角度計算與前饋控制
    private final double angle_per_rotation = 45.0 / 180.0;  // 馬達轉到45°所需的rotation
    private final ArmFeedforward feedforward = new ArmFeedforward(0, 0.05, 0);  // 避免手臂因重力下滑
    
    public Arm() {
        // 初始化馬達和控制器
        armMotor = new CANSparkMax(14, MotorType.kBrushless);
        pidController = armMotor.getPIDController();
        encoder = armMotor.getEncoder();
        
        // 配置馬達控制器
        configSparkMax();
    }
    
    // 配置SparkMax控制器
    private void configSparkMax() {
        armMotor.restoreFactoryDefaults();
        encoder.setPosition(0);
        armMotor.setIdleMode(IdleMode.kBrake);
        armMotor.setSmartCurrentLimit(20);
        
        // 設定PID參數
        pidController.setP(0.1);
        pidController.setI(0.0001);
        pidController.setD(0.01);
        
        armMotor.burnFlash();
    }
    
    // 設定馬達功率
    public void setPower(double p) {
        armMotor.set(p);
    }
    
    // 停止機械臂
    public void stop() {
        armMotor.set(0);
    }
    
    // 獲取當前角度
    public double getAngle() {
        return encoder.getPosition() * angle_per_rotation;
    }
    
    // 移動到指定角度方法
    public void moveToAngle(double targetAngle) {
        double targetRotation = targetAngle / angle_per_rotation;
        double ff = feedforward.calculate(Math.toRadians(targetAngle), 0);
        
        pidController.setReference(targetRotation, CANSparkMax.ControlType.kPosition, 0, ff);
    }
    
    @Override
    public void periodic() {
        // 可以添加診斷資訊輸出
    }
}

### 6.2 機械臂命令設計

public class ArmMoveToAngleCmd extends CommandBase {
    private final Arm arm;
    private final double targetAngle;
    
    public ArmMoveToAngleCmd(Arm arm, double targetAngle) {
        this.arm = arm;
        this.targetAngle = targetAngle;
        addRequirements(arm);
    }
    
    @Override
    public void initialize() {
        // 命令初始化時無需操作
    }
    
    @Override
    public void execute() {
        arm.moveToAngle(targetAngle);
    }
    
    @Override
    public boolean isFinished() {
        return Math.abs(arm.getAngle() - targetAngle) < 3.0;  // 容差3度
    }
    
    @Override
    public void end(boolean interrupted) {
        // 命令結束時無需特別停止，因為已達到目標角度
    }
}

### 6.3 機械臂按鈕綁定

// 在RobotContainer的configureBindings方法中
private void configureBindings() {
    // 機械臂控制按鈕
    joystick.leftBumper().onTrue(new ArmMoveToAngleCmd(arm, 0));  // 左肩鈕使機械臂移動到0度
    joystick.rightBumper().onTrue(new ArmMoveToAngleCmd(arm, 45));  // 右肩鈕使機械臂移動到45度
}

## 7. 抓取與Roller機構

### 7.1 Roller系統設計

Roller系統負責吸入或排出道具，並將它們傳遞給抓取機構。


public class Roller extends SubsystemBase {
    // 馬達宣告
    private final CANSparkMax rollerMotor;
    
    public Roller() {
        rollerMotor = new CANSparkMax(15, MotorType.kBrushless);
        configSparkMax();
    }
    
    // 配置SparkMax控制器
    private void configSparkMax() {
        rollerMotor.restoreFactoryDefaults();
        rollerMotor.setIdleMode(IdleMode.kCoast);  // 設定滑行模式
        rollerMotor.setSmartCurrentLimit(30);
        rollerMotor.burnFlash();
    }
    
    // 控制滾輪速度方法
    public void runRoller(double speed) {
        rollerMotor.set(speed);  // 正值(1.0)表示吸入，負值(-1.0)表示排出
    }
    
    // 停止方法
    public void stop() {
        rollerMotor.set(0);
    }
}

### 7.2 Roller命令設計

// 吸入命令
public class CoralInCmd extends CommandBase {
    private final Roller roller;
    
    public CoralInCmd(Roller roller) {
        this.roller = roller;
        addRequirements(roller);
    }
    
    @Override
    public void execute() {
        roller.runRoller(1.0);  // 以100%速度吸入
    }
    
    @Override
    public void end(boolean interrupted) {
        roller.stop();  // 停止馬達
    }
}

// 排出命令
public class CoralOutCmd extends CommandBase {
    private final Roller roller;
    
    public CoralOutCmd(Roller roller) {
        this.roller = roller;
        addRequirements(roller);
    }
    
    @Override
    public void execute() {
        roller.runRoller(-1.0);  // 以100%速度排出
    }
    
    @Override
    public void end(boolean interrupted) {
        roller.stop();  // 停止馬達
    }
}

## 8. 抓取機構(Grab)設計

抓取機構用於夾住道具，配合電梯和機械臂完成得分動作。


### 8.1 Grab子系統設計

public class Grab extends SubsystemBase {
    // 馬達宣告
    private final CANSparkMax grabMotor;
    
    public Grab() {
        grabMotor = new CANSparkMax(16, MotorType.kBrushless);
        configSparkMax();
    }
    
    // 配置SparkMax控制器
    private void configSparkMax() {
        grabMotor.restoreFactoryDefaults();
        grabMotor.setIdleMode(IdleMode.kBrake);  // 設定煞車模式
        grabMotor.setSmartCurrentLimit(20);
        grabMotor.burnFlash();
    }
    
    // 抓取方法
    public void intake() {
        grabMotor.set(0.5);  // 以50%速度抓取
    }
    
    // 釋放方法
    public void outtake() {
        grabMotor.set(-0.5);  // 以50%速度釋放
    }
    
    // 停止方法
    public void stop() {
        grabMotor.set(0);
    }
}

### 8.2 Grab命令設計

// 抓取命令
public class GrabIntakeCmd extends CommandBase {
    private final Grab grab;
    
    public GrabIntakeCmd(Grab grab) {
        this.grab = grab;
        addRequirements(grab);
    }
    
    @Override
    public void execute() {
        grab.intake();  // 執行抓取
    }
    
    @Override
    public void end(boolean interrupted) {
        grab.stop();  // 停止馬達
    }
}

// 釋放命令
public class GrabOuttakeCmd extends CommandBase {
    private final Grab grab;
    
    public GrabOuttakeCmd(Grab grab) {
        this.grab = grab;
        addRequirements(grab);
    }
    
    @Override
    public void execute() {
        grab.outtake();  // 執行釋放
    }
    
    @Override
    public void end(boolean interrupted) {
        grab.stop();  // 停止馬達
    }
}

## 9. Yati系統設計（Roller抬升機構）

Yati系統用於控制Roller的抬升和放下，以便於道具的收取和傳遞。

### 9.1 Yati子系統設計

public class Yati extends SubsystemBase {
    // 馬達宣告
    private final CANSparkMax yatiMotor;
    private final RelativeEncoder encoder;
    
    public Yati() {
        yatiMotor = new CANSparkMax(17, MotorType.kBrushless);
        encoder = yatiMotor.getEncoder();
        configSparkMax();
    }
    
    // 配置SparkMax控制器
    private void configSparkMax() {
        yatiMotor.restoreFactoryDefaults();
        encoder.setPosition(0);
        yatiMotor.setIdleMode(IdleMode.kBrake);
        yatiMotor.setSmartCurrentLimit(20);
        yatiMotor.burnFlash();
    }
    
    // 控制Yati移動方法
    public void runYati(double speed) {
        yatiMotor.set(speed);
    }
    
    // 獲取當前位置
    public double intakePos() {
        return encoder.getPosition();
    }
    
    // 停止方法
    public void stop() {
        yatiMotor.set(0);
    }
}

### 9.2 Yati命令設計

// 抬升命令
public class YatiUpCmd extends CommandBase {
    private final Yati yati;
    
    public YatiUpCmd(Yati yati) {
        this.yati = yati;
        addRequirements(yati);
    }
    
    @Override
    public void execute() {
        yati.runYati(0.3);  // 以30%速度抬升
    }
    
    @Override
    public void end(boolean interrupted) {
        yati.stop();  // 停止馬達
    }
}

// 放下命令
public class YatiDownCmd extends CommandBase {
    private final Yati yati;
    
    public YatiDownCmd(Yati yati) {
        this.yati = yati;
        addRequirements(yati);
    }
    
    @Override
    public void execute() {
        yati.runYati(-0.3);  // 以30%速度放下
    }
    
    @Override
    public void end(boolean interrupted) {
        yati.stop();  // 停止馬達
    }
}

## 10. 控制器整合與操作介面設計

### 10.1 RobotContainer中的系統初始化

public class RobotContainer {
    // 底盤系統初始化
    private final SwerveChassis chassis = new SwerveChassis();
    
    // 電梯系統初始化
    private final Elevator elevator = new Elevator();
    
    // 機械臂系統初始化
    private final Arm arm = new Arm();
    
    // Roller系統初始化
    private final Roller roller = new Roller();
    
    // 抓取系統初始化
    private final Grab grab = new Grab();
    
    // Yati系統初始化
    private final Yati yati = new Yati();
    
    // 控制器宣告
    private final CommandXboxController controller = new CommandXboxController(0);
    private final CommandJoystick joystick = new CommandJoystick(1);
    
    // 速度參數
    private final double maxSpeed = 4.935; // 米/秒
    private final double maxOmega = 10.13; // 弧度/秒
    
    public RobotContainer() {
        // 底盤預設命令設置
        configureDefaultCommands();
        
        // 配置按鈕綁定
        configureBindings();
    }
    
    private void configureDefaultCommands() {
        // 底盤預設命令
        chassis.setDefaultCommand(
            new RunCommand(
                () -> chassis.drive(
                    // 從左搖桿獲取平移指令(X、Y軸)
                    new Translation2d(
                        -controller.getLeftY() * maxSpeed,
                        -controller.getLeftX() * maxSpeed
                    ),
                    // 從右搖桿獲取旋轉指令(X軸)
                    -controller.getRightX() * maxOmega,
                    true  // 使用場地視角控制
                ),
                chassis
            )
        );
    }
    
    // 其他配置方法...
}

### 10.2 按鈕綁定與命令觸發

private void configureBindings() {
    // 底盤控制
    controller.start().onTrue(new InstantCommand(() -> chassis.resetGyro()));
    
    // 電梯控制按鈕
    joystick.x().onTrue(new ElevatorMoveToPositionCmd(elevator, 0.15));  // X鍵將電梯移動到0.15米高度
    joystick.y().onTrue(new ElevatorMoveToPositionCmd(elevator, 0.475));  // Y鍵將電梯移動到0.475米高度
    joystick.b().onTrue(new ElevatorMoveToPositionCmd(elevator, 0.35));  // B鍵將電梯移動到0.35米高度
    
    // 機械臂控制
    joystick.leftBumper().onTrue(new ArmMoveToAngleCmd(arm, 0));  // 左肩鈕使機械臂移動到0度
    joystick.rightBumper().onTrue(new ArmMoveToAngleCmd(arm, 45));  // 右肩鈕使機械臂移動到45度
    
    // Roller控制按鈕
    controller.x().whileTrue(new CoralInCmd(roller));  // X鍵按住時持續吸入
    controller.y().whileTrue(new CoralOutCmd(roller));  // Y鍵按住時持續排出
    
    // 抓取機構控制
    joystick.leftTrigger().whileTrue(new GrabIntakeCmd(grab));  // 左扳機按住時持續抓取
    joystick.rightTrigger().whileTrue(new GrabOuttakeCmd(grab));  // 右扳機按住時持續釋放
    
    // Yati控制
    controller.a().whileTrue(new YatiUpCmd(yati));  // A鍵按住時Roller抬升
    controller.b().whileTrue(new YatiDownCmd(yati));  // B鍵按住時Roller放下
}

## 11. 綜合測試與優化

### 11.1 底盤調試流程

在完成底盤程式撰寫後，我們進行了一系列的測試和調整：

1. **模組零點校準**：
- 使用特製的校準器(如鋁管)將所有輪子對齊到相同方向
- 記錄每個模組的初始絕對角度
- 根據測量結果計算並設定CANcoder的數值

2. **PID參數調整**：
- 在實際運行環境中測試不同的PID參數組合
- 收集數據分析轉向精度和穩定性
- 最終確定Kp=0.0113, Ki=0.0115, Kd=0作為最佳參數

3. **場地測試**：
- 驗證陀螺儀在連續運行中的漂移情況
- 根據測試結果調整代碼以提高精度

### 11.2 整合系統測試

在各子系統獨立測試完成後，我們進行了完整機器人的整合測試：

1. **協同動作測試**：
- 測試電梯和機械臂的協同運動
- 驗證抓取機構與Roller的配合效果
- 在不同速度下測試底盤與上層機構的互動

2. **操作穩定性測試**：
- 模擬比賽的操作測試
- 監控各馬達的溫度和電流消耗
- 分析可能的故障點和系統瓶頸

3. **極限條件測試**：
- 測試最大負載下的系統反應(如速度最大化)
- 驗證極限位置和角度的安全措施

## 12. 學習心得與反思

### 12.1 技術能力的提升

在這次專案中，我深入學習了機器人底盤系統的設計與實作，從單個Swerve模組的控制到整體底盤的運動協調。透過實際編程和調試，我掌握了PID控制器的參數調整技巧，以及如何處理機械系統中的誤差和干擾。

萬向輪底盤的設計與控制不僅要求紮實的編程基礎，還需要對機械原理有充分理解。在程式撰寫過程中，我學會了如何將數學模型轉化為實際的控制邏輯，這種轉化能力對於未來的工程項目至關重要。

### 12.2 問題解決經驗

面對校正偏移量和PID參數調整的挑戰，我學會了精確分析問題，並進行實驗和優化。這種反覆測試和微調的過程，培養了我的耐心和精細能力，也讓我理解到理論與實際應用之間的差距。

特別是在解決馬達振盪問題時，我通過逐步調整PID參數並分析系統響應，最終找到了穩定系統的最佳參數組合。這種問題解決的方法論不僅適用於機器人控制，也適用於其他工程領域。

### 12.3 整合系統的複雜性認知

整合多個子系統（底盤、電梯、機械臂、抓取機構）的過程中，我意識到系統設計的複雜性和各組件間的相互影響。這促使我學習如何設計模組化的程式架構，使各子系統既能獨立運作，又能協同工作。

在設計RobotContainer時，我體會到良好的系統架構對於後期開發和維護的重要性。合理的結構設計、清晰的變數定義以及完善的命令系統，都是確保複雜機器人程式順利運行的關鍵。

### 12.4 工程實踐的寶貴經驗

從程式碼撰寫到硬體調整，再到系統整合測試，每一步都是寶貴的工程實踐經驗。這些經驗不僅提升了我的技術能力，也讓我更深入理解機器人系統的工作原理和設計思路，為未來參與更複雜的專案奠定了堅實基礎。

同時，這次專案也讓我認識到團隊協作的重要性。與機械組、電子組的緊密合作，確保了軟體設計能夠充分考慮硬體限制，最終打造出一個高度整合的機器人系統。