# KOP底盤撰寫
使用 Neo (SparkMax) 馬達為範例

### 前置觀念
1. 左右邊各為一個 Module (程式模塊)
2. 透過搖桿控制 前後、左右

## 常數設置
1. 於 `src\main\java\frc\robot\DeviceId.java` 寫入馬達ID（編號）

package frc.robot;

public class DeviceId {
    public static final class DriveMotor {
        public static final int FRONT_LEFT = 1;
        public static final int FRONT_RIGHT = 3;
        public static final int BACK_LEFT = 2;
        public static final int BACK_RIGHT = 4;
    }
}

2. 於 `src\main\java\frc\robot\Constants.java` 寫入馬達前進和旋轉最大速度與 Deadband

> Deadband 用於防止搖桿無法完全歸零, 搖桿輸入需大於 Deadband 否則視為 0

public final class Drive {
    public static final double MAX_SPEED = 0.5; // 底盤最大速度 (%)
    public static final double MAX_TURN_SPEED = 0.7;
    public static final double DEAD_BAND = 0.05; // 當前面的值小於0.05則視為0
}

3. 一樣於 `Constants` 中寫入馬達的正反轉 (同一邊的馬達轉向相同)

public final class MotorReverse {
    public static final boolean FRONT_LEFT = false;
    public static final boolean FRONT_RIGHT = true;
    public static final boolean BACK_LEFT = false;
    public static final boolean BACK_RIGHT = true;
}

## 模組化
如果一組機構中使用多個相同作用的馬達, 模組可以使程式更加簡潔

1. 於 `src\man\java\frc\robot\subsystems` 創建 `DriveMotorModule.java`
2. 引入函示庫

package frc.robot.subsystems;

import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.lib.helpers.IDashboardProvider;

3. 於 `DriveMotorModule` 宣告單顆馬達

public class DriveModule {
    private final SparkMax frontSpark; // 模塊前面的馬達
    private final SparkMax backSpark; // 模塊後面的馬達

    public DriveMotorModule(
        int frontId, int backId,
        boolean frontReverse, boolean backReverse
    ) {
        // 創建馬達
        this.frontSpark = new SparkMax(frontId, MotorType.kBrushless);
        this.backSpark = new SparkMax(backId, MotorType.kBrushless);

        // 前面跟後面的 設定
        SparkMaxConfig frontConfig = new SparkMaxConfig();
        frontConfig
            .idleMode(IdleMode.kCoast)
            .inverted(frontReverse)
            .smartCurrentLimit(30);
        SparkMaxConfig backConfig = new SparkMaxConfig();
            backConfig
                .idleMode(IdleMode.kCoast)
                .inverted(backReverse)
                .smartCurrentLimit(30);

        // 馬達 Apply設定
        this.frontSpark.configure(frontConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
        this.backSpark.configure(backConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
    }
}

4. 寫入轉動與停止方法

public void setDesiredState(double speed) {
    this.frontSpark.set(speed);
    this.backSpark.set(speed);
}

public void stop() {
    this.frontSpark.stopMotor();
    this.backSpark.stopMotor();
}

## 創建兩個模塊的底盤
完成 DriveMotorModule 後可於 Subsystem 中使用

1. 於 `src\man\java\frc\robot\subsystems` 創建 `DriveMotorSubsystem.java`
2. 引入函式庫

package frc.robot.subsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.MotorReverse;
import frc.robot.DeviceId.DriveMotor;

3. 於 `DriveMotorSubsystem` 宣告四顆馬達

public class DriveMotorSubsystem extends SubsystemBase {
    private final DriveMotorModule leftModule;
    private final DriveMotorModule rightModule;

    public DriveMotorSubsystem() {
        // 創建 左右邊角塊
        this.leftModule = new DriveMotorModule(DriveMotor.FRONT_LEFT, DriveMotor.BACK_LEFT,
            MotorReverse.FRONT_LEFT, MotorReverse.BACK_LEFT
        );
        this.rightModule = new DriveMotorModule(DriveMotor.FRONT_RIGHT, DriveMotor.BACK_RIGHT,
            MotorReverse.FRONT_RIGHT, MotorReverse.BACK_RIGHT
        );
    }
}

4. 創建讓 KOP 移動的方法, 左右兩邊各需要一個速度讓底盤旋轉，故有 left 和 right Speed

public void move(double leftSpeed, double rightSpeed) {
    this.leftModule.setDesiredState(leftSpeed);
    this.rightModule.setDesiredState(rightSpeed);
}

5. 創建讓底盤停止的方法

public void stopModules() {
    this.leftModule.stop();
    this.rightModule.stop();
}

## 控制底盤
1. 於 `src\main\java\frc\robot` 創建 `GamepadJoystick.java` , 用於搖桿操作

package frc.robot;

import edu.wpi.first.wpilibj.XboxController;

public class GamepadJoystick extends XboxController {
    public GamepadJoystick(int port) {
        super(port);
    }
    public static final int CONTROLLER_PORT = 0;
}

2. 於 `src\main\java\frc\robot\commands` 創建 `DriveCmd.java`
3. 引入函式庫

package frc.robot.commands;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants.Drive;
import frc.robot.subsystems.DriveSubsystem;

3. 於 `DriveCmd` 導入 `DriveMotorSubsystem` 與搖桿

public class DriveCmd extends Command {
	private final DriveSubsystem driveSubsystem;
	private final XboxController controller;

	public DriveCmd(DriveSubsystem driveSubsystem, XboxController controller) {
	    this.driveSubsystem = driveSubsystem;
	    this.controller = controller;
	    this.addRequirements(this.driveSubsystem);
	}
}

### Arcade Drive
透過運算將移動與轉向的控制分開

@Override
public void execute() {
    double driveSpeed = -MathUtil.applyDeadband(this.controller.getLeftY(), Drive.DEAD_BAND) * Drive.MAX_SPEED;
    double turnSpeed = MathUtil.applyDeadband(this.controller.getRightX(), Drive.DEAD_BAND) * Drive.MAX_TURN_SPEED;

    double leftSpeed = driveSpeed + turnSpeed;
    double rightSpeed = driveSpeed - turnSpeed;

    this.driveSubsystem.move(leftSpeed, rightSpeed);
    SmartDashboard.putNumber("LeftSpeed", leftSpeed);
    SmartDashboard.putNumber("RightSpeed", rightSpeed);
}

5. 於 `end()` 寫入指令執行完成後動作

@Override
public void end(boolean interrupted) {
    this.driveMotorSubsystem.stopModules();
}

6. 於 `isFinished()` 回傳 `false` 表示程式 Command 不停止

@Override
public boolean isFinished() {
    return false;
}

7. 把 `DriveCmd.java` 寫入 `RobotContainer.java`

package frc.robot;

import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.commands.DriveCmd;
import frc.robot.subsystems.DriveSubsystem;

public class RobotContainer {
    private final GamepadJoystick joystick = new GamepadJoystick(GamepadJoystick.CONTROLLER_PORT);
    private final DriveMotorSubsystem driveMotorSubsystem = new DriveMotorSubsystem();
    private final DriveJoystickCmd driveJoystickCmd = new DriveJoystickCmd(driveMotorSubsystem, joystick);

    public RobotContainer() {
	this.driveMotorSubsystem.setDefaultCommand(this.driveJoystickCmd);
    }

    public Command getAutonomousCommand() {
	return null;
    }
}