#init

2026-04-17 10:32:57 +08:00
parent 572ad1205f
commit 17c52c978c
691 changed files with 72342 additions and 0 deletions
--- a/maibu-web-middleware/src/main/java/com/maibu/control/DiffSteer.java
+++ b/maibu-web-middleware/src/main/java/com/maibu/control/DiffSteer.java
@@ -0,0 +1,92 @@
+package com.maibu.control;
+
+public class DiffSteer {
+
+    public static final int Polarity_High = 0;  //0
+    public static final int Polarity_Low = 0;  //1
+
+    private float turnSpeedScale;
+    private float forwardSpeedScale;
+    private int speedAmplLimit;
+    private int leftWheelSpeedDir;
+    private int rightWheelSpeedDir;
+    private int xDir;
+    private int yDir;
+    private int leftWheelSpeed;
+    private int rightWheelSpeed;
+
+    public int getLeftWheelSpeed() {
+        return leftWheelSpeed;
+    }
+
+    public void setLeftWheelSpeed(int leftWheelSpeed) {
+        this.leftWheelSpeed = leftWheelSpeed;
+    }
+
+    public int getRightWheelSpeed() {
+        return rightWheelSpeed;
+    }
+
+    public void setRightWheelSpeed(int rightWheelSpeed) {
+        this.rightWheelSpeed = rightWheelSpeed;
+    }
+
+    // 初始化函数
+    public void init(int turnSpeedMax, int forwardSpeedMax, int xMax, int yMax, int speedAmplLimit) {
+        this.turnSpeedScale = (float) turnSpeedMax / (float) xMax;
+        this.forwardSpeedScale = (float) forwardSpeedMax / (float) yMax;
+        this.speedAmplLimit = speedAmplLimit;
+        this.leftWheelSpeedDir = Polarity_High;
+        this.rightWheelSpeedDir = Polarity_High;
+        this.xDir = Polarity_High;
+        this.yDir = Polarity_High;
+    }
+
+    // 设置方向极性
+    public void setDirPolarity(int leftWheelSpeedDir, int rightWheelSpeedDir, int xDir, int yDir) {
+        this.leftWheelSpeedDir = leftWheelSpeedDir;
+        this.rightWheelSpeedDir = rightWheelSpeedDir;
+        this.xDir = xDir;
+        this.yDir = yDir;
+    }
+
+    // 计算左右轮速度
+    public void calcWheelSpeed(int x, int y) {
+        int turnSpeed = signOperator((int) (x * turnSpeedScale), xDir);
+        int forwardSpeed = signOperator((int) (y * forwardSpeedScale), yDir);
+
+        turnSpeed = (forwardSpeed >= 0 ? turnSpeed : -turnSpeed);
+        leftWheelSpeed = signOperator(forwardSpeed + turnSpeed, leftWheelSpeedDir);
+        rightWheelSpeed = signOperator(forwardSpeed - turnSpeed, rightWheelSpeedDir);
+
+        leftWheelSpeed = clamp(leftWheelSpeed, -speedAmplLimit, speedAmplLimit);
+        rightWheelSpeed = clamp(rightWheelSpeed, -speedAmplLimit, speedAmplLimit);
+    }
+
+    // 设置前向速比
+    public void setTurnSpeedScale(int turnSpeedMax, int xMax) {
+        this.turnSpeedScale = (float) turnSpeedMax / (float) xMax;
+    }
+
+    // 设置转向速比
+    public void setForwardSpeedScale(int forwardSpeedMax, int yMax) {
+        this.forwardSpeedScale = (float) forwardSpeedMax / (float) yMax;
+    }
+
+    // 设置电机转速的最大速度限制
+    public void setSpeedAmplLimit(int speedAmplLimit) {
+        this.speedAmplLimit = speedAmplLimit;
+    }
+
+    // 辅助函数：符号操作
+    private int signOperator(int num, int polarity) {
+        return polarity != 0 ? -num : num;
+    }
+
+    // 辅助函数：限制值在范围内
+    private int clamp(int val, int min, int max) {
+        return (val < min) ? min : ((val > max) ? max : val);
+    }
+
+
+}