# Swerve Drive ## Introduction of Swerve ### What is Swerve swerve 是一種底盤的運行模式，如果是四輪傳動的swerve ，組成為四個獨立的腳，每腳皆有 Drive Motor & Turning Motor & Encoder * Drive Motor 負責控制腳的轉速 * Turning Motor 負責控制腳輪子的旋轉角度 * Encoder * Drive Encoder 負責了解當前腳的轉速 * Turning Encoder 負責了解當前腳輪子的角度 因此 Swerve 可以向四面八方移動，同時也可以轉動其底盤的面對角度 附註： Encoder 為紀錄某裝置狀態的電子設備 ### How to Control Swerve Drive Swerve Drive 有兩種控制模式，分別為機器人視角及場地視角 機器人視角就是以機器人為中心來控制機器人 場地視角則是以場地為中心來控制機器人，機器人的坐標軸方向不變，因此需要使用 Gyro 來控制 控制 Swerve Drive 需要將四隻腳分別給予不同的角度及速度來控制機器人轉動及移動 附註： Gyro 為陀螺儀，可以得知機器人方位 ## Swerve Module Control ![image](https://hackmd.io/_uploads/SyTS9SR5C.png) swerve 的控制是使用給予底盤整體的方向及速度，再利用矩陣運算及三角函數來分別計算各腳的速度及角度 ### Definition ![Swerve Virtual Drawing](https://hackmd.io/_uploads/SJiit1SsC.png) *Rot speed 為機器人的角速度 Turning Degree ![Turning Degree Definition](https://hackmd.io/_uploads/SJPNywCqC.png) 本次計算的 Swerve Drive 為正方形，所以所有的 r 值皆相同 Heading = Swerve Heading S = Module Speed ( Value of All Modules' Speed is equal ) LF = LeftForward Module LB = LeftBackward Module RF = RightForward Module RB = RightForward Module ### Caculation Swerve 可以使用場地座標及機器人座標來控制，但這邊以場地座標來進行計算 Speed 方面先不計正負，Swerve Module 上一狀態的角度，影響 Speed 的正反轉，因為反轉等同於 Turning Degree 轉 180 度 $$ S = \sqrt{x^{2}+y^{2}+(r*rot)^{2}} $$ 我們可以知道角速度 * 半徑，就知道每秒物體在單位圓上所行走的面積，當時間行走時間趨近 0，那角速度 * 半徑 = 物體的切線速度 ![Turning Degree Caculation](https://hackmd.io/_uploads/SyKf5yHj0.png) ![Virtual drawing of swerve degree](https://hackmd.io/_uploads/ByurdlrjR.png) 由於本次計算的機器人為正方形，因此 θ 就只有正負 45、135 度而已，如果是其他形狀的機器人，就需要依著長度及寬度來計算角度 $$ \begin{aligned} LF:&Vx = x + rot*r*\sin(135^\circ+\alpha) \\ &Vy = y+rot*r*\cos(135^\circ+\alpha) \\ LB:&Vx = x+rot*r*\sin(-135^\circ+\alpha) \\ &Vy = y+rot*r*\cos(-135^\circ+\alpha)\\RF:&Vx = x+rot*r*\sin(-45^\circ+\alpha)\\&Vy = y+rot*r*\cos(-45^\circ+\alpha)\\RB:&Vx = x+rot*r*\sin(45^\circ+\alpha)\\&Vy = y+rot*r*cos(45^\circ+\alpha)\\Deg&ree=\arctan(Vy/Vx) \end{aligned} $$ ## Pre Software ### Commandbase 撰寫 Swerve 不一定要用 Commandbase 寫，但這份文件檔，將會以 Commandbase 的形式來教學 Swerve #### What is Commandbase ![image](https://hackmd.io/_uploads/S1r_-eHs0.png) Commandbase 這套撰寫模式是由 Subsystems 及 Commands 組成，Subsystem 就是撰寫所有馬達及其行為模式的地方，Subsystems 為馬達及其他裝置建立行為準則，則外界使用 Commands 來使用這些已經建構的行為準則來操縱裝置，Commands 就如同其中文意思-命令，就是用來命令 Subsystem 動作的 #### Commandbase Learning Resourse [WPI Commandbase](https://docs.wpilib.org/en/stable/docs/software/commandbased/index.html) ### PID Control 將會使用 PID 來控制 Module 的 Turning Degree PID Control 由三項控制項來控制，分別為常數項、微分項、積分項 ![image](https://hackmd.io/_uploads/BJW2DPAcA.png) * 常數項將會控制 Motor 驅動到目標 * 微分項將會修飾過於劇烈的常數項 * 積分項將會驅使 Motor 更為接近目標，使較不會在目標附近反覆進行大躍動 ![image](https://hackmd.io/_uploads/Hy25PwRqC.png) #### PID Control Learning Resourse [Control Theory](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/introduction/index.html) ### Turning Encoder Offset ![Turning Motor Offset](https://hackmd.io/_uploads/BJ_3tP09R.png) **+字號為斜齒輪所面向的面，也就是說以所有斜齒輪面向 swerve 內部來進行 Turning Degree 的校正** 將每個 Turning Degree 的初始值皆校正為 0，才可讓機器人穩定行走 ### Record Device Specification ![Swerve Specification](https://hackmd.io/_uploads/BJC6jwA50.png) 還要記錄 Drive Motor 的齒輪比、輪子直徑、輪子極速、最大加速度、速度、角速度等等 ## Software 在撰寫 Swerve 的程式碼時，一般會使用兩個 class 來撰寫，以減少程式碼數量，達到規格化及精簡的程式碼，會有 Swerve Module 這個 class 和 Drivebase 這個 class Drivebase 將會是一個 Subsystem 此外，為了方便需要程式參數，將會使用一個 Constants 的檔案來存放所有易更動參數 ### Swerve Module #### Construct all things ``` private final CANSparkMax driveMotor; private final CANSparkMax turningMotor; private final RelativeEncoder driveEncoder; private final CANcoder turningEncoder; private final PIDController rotController; private final String moduleName; ``` #### Construction of Module ``` public SwerveModule(int driveMotorChannel, int turningMotorChannel, int turningEncoderChannel, boolean driveInverted, double canCoderMagOffset, String moduleName) { this.moduleName = moduleName; driveMotor = new CANSparkMax(driveMotorChannel, MotorType.kBrushless); turningMotor = new CANSparkMax(turningMotorChannel, MotorType.kBrushless); // 設定馬達正反轉 driveMotor.setInverted(driveInverted); turningMotor.setInverted(ModuleConstants.kTurningMotorInverted); turningEncoder = new CANcoder(turningEncoderChannel); // 設定 Turning Encoder 的參數 CANcoderConfiguration turningEncoderConfiguration = new CANcoderConfiguration(); // 將 Turning Encoder 定義成 0度~180度 的形式 turningEncoderConfiguration.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; // 設定 Turning Encoder 的初始值，使其初始值歸零 turningEncoderConfiguration.MagnetSensor.MagnetOffset = canCoderMagOffset; turningEncoder.getConfigurator().apply(turningEncoderConfiguration); driveEncoder = driveMotor.getEncoder(); driveEncoder.setPosition(0); rotController = new PIDController(ModuleConstants.kPRotationController, ModuleConstants.kIRotationController, ModuleConstants.kDRotationController); // 定義 PID 的最大最小輸入 rotController.enableContinuousInput(-180.0, 180.0); configDriveMotor(); configTurningMotor(); resetAllEncoder(); } ``` 由於 Swerve 的 Turning Motor 紀錄圓的方式為下圖此種形式，因此 PID 的輸入值需要將最大最小值訂為 -180 ~ 180 度 ![image](https://hackmd.io/_uploads/HyZfNcEjC.png) ``` public void configDriveMotor() { // 設定馬達在不運轉及運轉時的最大電流容許值，單位為安培 driveMotor.setSmartCurrentLimit(10, 80); // 設定馬達的 ClosedLoopRampRate 的時間長度 driveMotor.setClosedLoopRampRate(ModuleConstants.kDriveClosedLoopRampRate); driveMotor.setIdleMode(IdleMode.kBrake); // 設定電壓補償大小 driveMotor.enableVoltageCompensation(ModuleConstants.kMaxModuleDriveVoltage); driveMotor.burnFlash(); } public void configTurningMotor() { turningMotor.setSmartCurrentLimit(20); turningMotor.setClosedLoopRampRate(ModuleConstants.kTurningClosedLoopRampRate); turningMotor.setIdleMode(IdleMode.kBrake); turningMotor.enableVoltageCompensation(ModuleConstants.kMaxModuleTurningVoltage); turningMotor.burnFlash(); } public void resetAllEncoder() { driveEncoder.setPosition(0); } ``` 由於 Drive Motor 提供整台的動力，怕 Drive Motor 在不動時，怕負載過大以至於燒毀，就直接設定其停機最大電流，當達到最大設定的電流時，馬達控制器就強制斷電，讓馬達處於無通電的狀態，其電流限制可以依據使用需求及安全範圍內自由制定，Turning Motor 的電流限制也是 ClosedLoopRampRate 是一種保護裝置，他可以將輸入進馬達的值限制其最大變化率，當設定其值為 0.1，就是說他會在 0.1 秒才從速度 0 ~ 最大轉速 VoltageCompensation 為一種保護裝置的設定，如果電壓輸入量值大於電壓補償值，強制將輸入量值改為電壓補償值 #### Get Module State ``` public SwerveModuleState getState() { return new SwerveModuleState( getDriveRate(), new Rotation2d(Math.toRadians(getRotation()))); } // to get the drive distance public double getDriveDistance() { // 需要將從 driveEncoder 得到的圈數轉換成實際距離 return driveEncoder.getPosition() / ModuleConstants.kWheelGearRate * 2.0 * Math.PI * ModuleConstants.kWheelRadius; } // calculate the rate of the drive public double getDriveRate() { // 需要將從 driveEncoder 得到的每分鐘圈數轉換成實際速度 return driveEncoder.getVelocity() / 60.0 / ModuleConstants.kWheelGearRate * 2.0 * Math.PI * ModuleConstants.kWheelRadius; } // to get rotation of turning motor public double getRotation() { // 需要將從 Turning Encoder 得到的圈數轉換成角度 return turningEncoder.getAbsolutePosition().getValueAsDouble() * 360.0; } ``` 這邊所有的計算還是會依著使用的 Encoder 而有所不同，所以還是要依著自己使用的 Encoder 來自行推算計算式 推算方法： 1. 了解 encoder 轉一圈是幾個單元 ( encoderPulse ) 2. 了解輪子轉動一圈與 encoder 轉動圈數的相對關係 ( conversionFactor ) 3. 了解輪子轉動一圈是多長距離 ( unitDistance 4. 將 encoder 數值 / encoder * conversionFactor * unitDistance，及得到所要數值 ex. Drive Distance Drive Encoder 轉一圈是一單元，此 Encoder 是反應實際馬達轉動圈數， 因此要將轉動圈數 / 馬達對輪子的齒輪比，這樣可以得知輪子的實際轉動圈數， 我們可以知道輪子的半徑R ( 單位公尺 )， 再來將輪子轉動圈數 * 2 * R * π，即為 Drive Distance #### Set Module State ``` public void setDesiredState(SwerveModuleState desiredState) { if (Math.abs(desiredState.speedMetersPerSecond) < DrivebaseConstants.kMinSpeed) { stopModule(); } else { // 將從外面輸入進來的 ModuleState 轉換成 Voltage 驅動馬達 var moduleState = optimizeOutputVoltage(desiredState, getRotation()); driveMotor.setVoltage(moduleState[0]); turningMotor.setVoltage(moduleState[1]); } } ``` 怕 Gamepad 會有殘值，因此需要制定一個最小值，如果輸入的速度小於最小值，就直接將速度歸零 ``` public double[] optimizeOutputVoltage(SwerveModuleState goalState, double currentTurningDegree) { // 優化 State 的轉動角度及其速度，使之轉動角度小於 45 度 goalState = SwerveModuleState.optimize(goalState, Rotation2d.fromDegrees(currentTurningDegree)); // 使用 Feedforward Control 來控制 drive Motor double driveMotorVoltage = ModuleConstants.kDesireSpeedtoMotorVoltage * goalState.speedMetersPerSecond; // 使用 PID Control 來控制 Turning Motor double turningMotorVoltage = rotController.calculate(currentTurningDegree, goalState.angle.getDegrees()); double[] moduleState = { driveMotorVoltage, turningMotorVoltage }; return moduleState; } ``` ##### How to Optimize State ![image](https://hackmd.io/_uploads/Byg6HOA9R.png) 如上圖，當 Drive Motor 正轉時，這時如果要轉到目標角度，就需要逆時針轉 135 度，但其實可以更改成 Drive Motor 反轉，順時針轉 45 度 因此只要將需要轉動 >90 度的度數 -180 度，並將其 Drive Motor 反轉 反之 <-90 度時，將其 +180 度，並使之 Drive Motor 反轉 但這前提是須要將轉動角度絕對值要小於180度 為了讓此前提成立，遇到大於 180 度的角度，直接 -360 度，或是遇到小於 180 度的角度，直接 +360 度 ex 轉動角度為 193 度，需變成 -167 度 ### Swerve Drive #### Construct all things ``` private final Translation2d frontLeftLocation; private final Translation2d frontRightLocation; private final Translation2d backLeftLocation; private final Translation2d backRightLocation; private final SwerveModule frontLeft; private final SwerveModule frontRight; private final SwerveModule backLeft; private final SwerveModule backRight; private final SwerveDriveKinematics kinematics; private final SwerveDriveOdometry odometry; private final Pigeon2 gyro; private SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; ``` #### Construction of Swerve Drive ``` public Drivebase() { // 定義四隻腳的位置，單位為公尺 frontLeftLocation = new Translation2d(DrivebaseConstants.kRobotLength / 2.0, DrivebaseConstants.kRobotWidth / 2.0); frontRightLocation = new Translation2d(DrivebaseConstants.kRobotLength / 2.0, -DrivebaseConstants.kRobotWidth / 2.0); backLeftLocation = new Translation2d(-DrivebaseConstants.kRobotLength / 2.0, DrivebaseConstants.kRobotWidth / 2.0); backRightLocation = new Translation2d(-DrivebaseConstants.kRobotLength / 2.0, -DrivebaseConstants.kRobotWidth / 2.0); frontLeft = new SwerveModule(DrivebaseConstants.kFrontLeftDriveMotorChannel, DrivebaseConstants.kFrontLeftTurningMotorChannel, DrivebaseConstants.kFrontLeftTurningEncoderChannel, DrivebaseConstants.kFrontLeftDriveMotorInverted, DrivebaseConstants.kFrontLeftCanCoderMagOffset, "frontLeft"); frontRight = new SwerveModule(DrivebaseConstants.kFrontRightDriveMotorChannel, DrivebaseConstants.kFrontRightTurningMotorChannel, DrivebaseConstants.kFrontRightTurningEncoderChannel, DrivebaseConstants.kFrontRightDriveMotorInverted, DrivebaseConstants.kFrontRightCanCoderMagOffset, "frontRight"); backLeft = new SwerveModule(DrivebaseConstants.kBackLeftDriveMotorChannel, DrivebaseConstants.kBackLeftTurningMotorChannel, DrivebaseConstants.kBackLeftTurningEncoderChannel, DrivebaseConstants.kBackLeftDriveMotorInverted, DrivebaseConstants.kBackLeftCanCoderMagOffset, "backLeft"); backRight = new SwerveModule(DrivebaseConstants.kBackRightDriveMotorChannel, DrivebaseConstants.kBackRightTurningMotorChannel, DrivebaseConstants.kBackRightTurningEncoderChannel, DrivebaseConstants.kBackRightDriveMotorInverted, DrivebaseConstants.kBackRightCanCoderMagOffset, "backRight"); gyro = new Pigeon2(DrivebaseConstants.kGyroChannel); // 定義 Swerve Drive 的 Kinematics 去將整體的速度算成各腳角度及速度 kinematics = new SwerveDriveKinematics( frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation); // reset the gyro resetGyro(); } ``` ##### Why need the position of module 當在控制 Swerve 時需要了解其自身長寬，才可以計算出合適的轉動角度，使機器人轉動精準無誤 ![swerve drive dirction](https://hackmd.io/_uploads/ryeKDLGsR.png) 1、2、3、4 為填入 kinematics 的次序 #### Control Swerve Drive ``` public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { swerveModuleStates = kinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, getRotation2d()) : new ChassisSpeeds(xSpeed, ySpeed, rot)); SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DrivebaseConstants.kMaxSpeed); frontLeft.setDesiredState(swerveModuleStates[0]); frontRight.setDesiredState(swerveModuleStates[1]); backLeft.setDesiredState(swerveModuleStates[2]); backRight.setDesiredState(swerveModuleStates[3]); } ``` ``` public Rotation2d getRotation2d() { return Rotation2d.fromDegrees(DrivebaseConstants.kGyroOffSet + ((DrivebaseConstants.kGyroInverted) ? (360.0 - gyro.getRotation2d().getDegrees()) : gyro.getRotation2d().getDegrees())); } ``` ##### How to invert Gyro ![image](https://hackmd.io/_uploads/ryM0-YEs0.png) 因為一般 Gyro 為正面朝上放置，以順時針為正 但為了符合設計要求，Gyro 可能會倒過來放置，當 Gyro 為倒立放置，以逆時針為正，因此要使 Gyro 以順時針為正，只需要把從 Gyro 得到的值 -360 度就等同於 Gyro 順時針轉 ### Constants #### ModuleConstants ``` // 定義輪子的半徑，單位是公尺 public static final double kWheelRadius = 0.046; // 定義輪子的 driveMotor & turningMotor 最大輸出電壓 public static final double kMaxModuleDriveVoltage = 12.0; public static final double kMaxModuleTuringVoltage = 10.0; // 設定 Motor 的 closedLoopRampRate 之時距 public static final double kDriveClosedLoopRampRate = 0.1; // 1 second 1 unit public static final double kTurningClosedLoopRampRate = 0.1; // 目前使用方式為直接將輸入速度轉換成電壓，並沒有考慮輪子是否有達到目標轉速 public static final double kDesireSpeedtoMotorVoltage = kMaxModuleDriveVoltage / DrivebaseConstants.kMaxSpeed; // 設定 turningMotor 轉動到目標角度的速度比例，當此值越大轉動速度越慢 public static final double kMaxSpeedTurningDegree = 180.0; // 設定 rotPID 的參數 public static final double kPRotationController = kMaxModuleTuringVoltage / kMaxSpeedTurningDegree; public static final double kIRotationController = 0.0; public static final double kDRotationController = 0.0004; public static final boolean kTurningMotorInverted = true; ``` 一般馬達正反轉為左前左後一組，右前右後一組，但實際還是要經過測試 #### DrivebaseConstants ##### Motor ``` // driveMotor channel public static final int kFrontLeftDriveMotorChannel = 11; public static final int kFrontRightDriveMotorChannel = 15; public static final int kBackLeftDriveMotorChannel = 13; public static final int kBackRightDriveMotorChannel = 17; // turningMotor channel public static final int kFrontLeftTurningMotorChannel = 12; public static final int kFrontRightTurningMotorChannel = 16; public static final int kBackLeftTurningMotorChannel = 14; public static final int kBackRightTurningMotorChannel = 18; // 定義 driveMotor 的正反轉 public static final boolean kFrontLeftDriveMotorInverted = false; public static final boolean kFrontRightDriveMotorInverted = true; public static final boolean kBackLeftDriveMotorInverted = false; public static final boolean kBackRightDriveMotorInverted = true; ``` ##### Turning Encoder ``` // turnning encoder channel public static final int kFrontLeftTurningEncoderChannel = 31; public static final int kFrontRightTurningEncoderChannel = 32; public static final int kBackLeftTurningEncoderChannel = 33; public static final int kBackRightTurningEncoderChannel = 34; // turning encoder magnet offset value public static final double kFrontLeftCanCoderMagOffset = 0.003174; public static final double kFrontRightCanCoderMagOffset = 0.117676 ; public static final double kBackLeftCanCoderMagOffset = 0.347656; public static final double kBackRightCanCoderMagOffset = -0.178955; ``` ##### Gyro ``` public static final int kGyroChannel = 30; // whether gyro is under the robot public static final boolean kGyroInverted = false; public static final double kGyroOffSet = 0.0; ``` ##### Drive Control ``` // 機器人的大小規格 public static final double kRobotWidth = 0.6; public static final double kRobotLength = 0.6; public static final double kRobotDiagonal = Math.sqrt(Math.pow(kRobotLength, 2.0) + Math.pow(kRobotWidth, 2.0)); // 最大轉速需要實際測試看看 public static final double kMaxSpeed = 4.0; // 設定最小轉速，以避免 Gamepad 胡亂輸送訊號以至於機器人不受控制 public static final double kMinSpeed = 0.2; // 最大角速度 public static final double kMaxAngularSpeed = kMaxSpeed / (kRobotDiagonal / 2.0); // rad/s ``` ##### Constants of Gamepad's input ``` // make the input from Gamepad more smooth public static final double kXLimiterRateLimit = 5.0; public static final double kYLimiterRateLimit = 5.0; public static final double kRotLimiterRateLimit = 5.0; ``` ### Manual Drive 這邊會建立一個由 Gamepad 當作輸入源的 Command 程式 #### Construct all things ``` private final Drivebase drivebase; private final CommandXboxController main; private final SlewRateLimiter xLimiter; private final SlewRateLimiter yLimiter; private final SlewRateLimiter rotLimiter; private final double drivebaseMaxSpeed = DrivebaseConstants.kMaxSpeed; private final double drivebaseMaxAngularSpeed = DrivebaseConstants.kMaxAngularSpeed; private double xSpeed, ySpeed, rotSpeed; ``` #### Construction of this command ``` public SwerveJoystickCmd(Drivebase drivebase, CommandXboxController main) { // Use addRequirements() here to declare subsystem dependencies. this.drivebase = drivebase; this.main = main; xLimiter = new SlewRateLimiter(DrivebaseConstants.xLimiterRateLimit); yLimiter = new SlewRateLimiter(DrivebaseConstants.yLimiterRateLimit); rotLimiter = new SlewRateLimiter(DrivebaseConstants.rotLimiterRateLimit); addRequirements(this.drivebase); } ``` addRequirements 這個 function 是 FRC 程式用來確認此 Command 是使用何種 Subsystem 用的，避免多重 Commands 一起操縱同一個 Subsystem 以至於出現衝突問題 #### Excution of this command ``` @Override public void execute() { xSpeed = -xLimiter.calculate(main.getLeftY()) * drivebaseMaxSpeed;//forward ySpeed = -yLimiter.calculate(main.getLeftX()) * drivebaseMaxSpeed;//side rotSpeed = -rotLimiter.calculate(main.getRightX()) * drivebaseMaxAngularSpeed;//turn drivebase.drive(xSpeed, ySpeed, rotSpeed, true); } ``` 由於從 Gamepad 傳入的值為百分比，需要將其轉換為實際數值 再者因為 Swerve 為了方便計算，其使用的方向與我們平常在數學上使用的方向完全相反，因此將 Gamepad 輸出的數值乘負號，好讓機器人能在自動走路徑及手動時，駕駛不須熟悉新的方向模式，可以用其習慣的方向 ## 參考資料 [6083 swerve code](https://github.com/Team6083/2024SwerveDrive-example) [How To Build a Swerve-Drive Robot](https://www.freshconsulting.com/insights/blog/how-to-build-a-swerve-drive-robot/) [WPI Swerve](https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/index.html)