邢善泽的工程笔记#

9.21#

编程组的人都是猪头 🐷 完善了我们的小车方案，并且将编程组最终的人选确认了一下。安装 AndroidSTUDIO 搞了一节课，诶，下次找时间给他们恶补知识。

10.1#

学习调优 roadrunner 并完整调优一辆车。看了一个小时的视频，调整参数 KA KV KS 以及微调参数，学习使用 dashboard5.0。让后给新手李铭泽学习 Java 基础语法。就这么简单。

10.2#

今天超级编程，学习如何编写颜色传感器，但是有点困难，我不是美术生，我也不知道啊？编写了基础的测试用的程序，包括磁力限位传感器，舵机和颜色传感器。了解看一些高阶 Java 语法（构造函数）也算是复习了，终于是回归正轨了。

10.3#

学习使用 camera 识别 AirTag，找到了一个新的 dashboard 叫 panel，可以无线设置 limilight3A 太强了，但是它其它功能要用.kt 文件，放在主机里直接闪退用不了，所以还是不用其他功能了。

10.6#

研究抛物线公式

判定式：

方程（化简后）

所以计算出来有两个解对应高抛和低抛

所以谁能告诉我速度和角度该咋去写？

1
package org.firstinspires.ftc.teamcode.FTC_27650_new;
2

3

4
import com.acmerobotics.dashboard.FtcDashboard;
5
import com.acmerobotics.dashboard.config.Config;
6
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
7
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
8
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
9

10
@TeleOp(name="parabola_test(抛物线)", group="Linear OpMode")
11
//@Disabled
12
@Config
13

14
public class parabola_test extends LinearOpMode {
15

16
    double power = 1;
17
    public static double h1 = 0.2;//车高unit:m
18
    public static double h2 = 1.1;//塔高unit:m
19
    public static double k = 15;//系数
20
    double v = (power * k);//速度（电机功率*系数k）unit:(m/s)
21
    double g = 9.8;//重力加速度unit:(m/s*s)
22
    public static double d = 1;//水平距离（h1与h2）unit:m
23

24

25
    public void runOpMode() {
26
        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
27

28
        waitForStart();
29

30
        while (opModeIsActive()) {
31

32
            double discriminant = (v * v * v * v) - g * d * (2 * v * v * (h2 - h1) + g * d); // 判别式
33

34
            if (discriminant >= 0) {
35
                double sqrtDiscriminant = Math.sqrt(discriminant);
36
                double tanTheta1 = ((v * v) + sqrtDiscriminant) / (g * d);//高抛角
37
                double tanTheta2 = ((v * v) - sqrtDiscriminant) / (g * d);//低抛角
38
                double theta1 = Math.atan(tanTheta1); // 弧度
39
                double theta2 = Math.atan(tanTheta2); // 弧度
40

41
                telemetry.addData("theta1 (弧度)", "%4.4f", Math.atan(theta1));
42
                telemetry.addData("theta2 (弧度)", "%4.4f", Math.atan(theta2));
43
                telemetry.addData("theta1 (角度,高抛角)", "%4.4f", Math.toDegrees(theta1));
44
                telemetry.addData("theta1 (角度,低抛角)", "%4.4f", Math.toDegrees(theta2));
45
                telemetry.addData("discriminant(判定式)", "%4.4f", discriminant);
46

47

48
            } else {
49
                telemetry.addData("discriminant(判定式无解)", "%4.4f", discriminant);
50
            }
51
            telemetry.update();
52

53

54
        }
55

56
    }
57
}

学习使用 LIMELIGHT 识别与 AirTag 的距离

如图，我们可以得知计算公式为

所以谁能告诉我相机角度和高度是多少呢?

程序：

1
/*
2
Copyright (c) 2024 Limelight Vision
3

4
All rights reserved.
5

6
Redistribution and use in source and binary forms, with or without modification,
7
are permitted (subject to the limitations in the disclaimer below) provided that
8
the following conditions are met:
9

10
Redistributions of source code must retain the above copyright notice, this list
11
of conditions and the following disclaimer.
12

13
Redistributions in binary form must reproduce the above copyright notice, this
14
list of conditions and the following disclaimer in the documentation and/or
15
other materials provided with the distribution.
16

17
Neither the name of FIRST nor the names of its contributors may be used to
18
endorse or promote products derived from this software without specific prior
19
written permission.
20

21
NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
22
LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
24
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
26
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
30
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
31
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32
*/
33
package org.firstinspires.ftc.teamcode.FTC_27650_new;
34

35
import com.acmerobotics.dashboard.FtcDashboard;
36
import com.acmerobotics.dashboard.config.Config;
37
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
38
import com.qualcomm.hardware.limelightvision.LLResult;
39
import com.qualcomm.hardware.limelightvision.LLResultTypes;
40
import com.qualcomm.hardware.limelightvision.LLStatus;
41
import com.qualcomm.hardware.limelightvision.Limelight3A;
42
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
43
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
44
import com.qualcomm.robotcore.hardware.DcMotorEx;
45

46
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
47

48
import java.util.List;
49

50
/**
51
 * This OpMode illustrates how to use the Limelight3A Vision Sensor.
52
 *
53
 * @see <a href="https://limelightvision.io/">Limelight</a>
54
 *
55
 * Notes on configuration:
56
 *
57
 *   The device presents itself, when plugged into a USB port on a Control Hub as an ethernet
58
 *   interface.  A DHCP server running on the Limelight automatically assigns the Control Hub an
59
 *   ip address for the new ethernet interface.
60
 *
61
 *   Since the Limelight is plugged into a USB port, it will be listed on the top level configuration
62
 *   activity along with the Control Hub Portal and other USB devices such as webcams.  Typically
63
 *   serial numbers are displayed below the device's names.  In the case of the Limelight device, the
64
 *   Control Hub's assigned ip address for that ethernet interface is used as the "serial number".
65
 *
66
 *   Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight
67
 *   and specify the Limelight's ip address.  Users should take care not to confuse the ip address of
68
 *   the Limelight itself, which can be configured through the Limelight settings page via a web browser,
69
 *   and the ip address the Limelight device assigned the Control Hub and which is displayed in small text
70
 *   below the name of the Limelight on the top level configuration screen.
71
 */
72
@TeleOp(name = "Sensor: Limelight3A", group = "Sensor")
73
//@Disabled
74
@Config
75
public class SensorLimelight3A extends LinearOpMode {
76
    private static Limelight3A limelight;
77
    static LLResult result = limelight.getLatestResult();
78
    public static double targetOffsetAngle_Vertical = result.getTy();
79
    // 您的limelight从完全垂直方向向后旋转了多少度？
80
    public static double limelightMountAngleDegrees = 0;
81
    // Limelight镜头中心到地面的距离
82
    public static double limelightLensHeightInches = 0;//unit:in
83
    // 目标到地面的距离
84
    public static double goalHeightInches = 24.0;//unit:in
85

86

87
    @Override
88
    public void runOpMode() {
89
        limelight = hardwareMap.get(Limelight3A.class, "limelight");
90
        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
91

92

93
        telemetry.setMsTransmissionInterval(11);
94

95
        limelight.pipelineSwitch(0);
96

97
        /*
98
         * Starts polling for data.  If you neglect to call start(), getLatestResult() will return null.
99
         */
100
        limelight.start();
101

102
        telemetry.addData(">", "Robot Ready.  Press Play.");
103
        telemetry.update();
104
        DcMotorEx m0 = null;
105
        m0 = hardwareMap.get(DcMotorEx.class, "m0"); // c0
106

107

108
        waitForStart();
109

110
        while (opModeIsActive()) {
111
            if (gamepad1.a) {
112
                m0.setPower(1);
113
            }
114

115
            LLStatus status = limelight.getStatus();
116

117

118
            telemetry.addData("Name", "%s",
119
                    status.getName());
120
            telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d",
121
                    status.getTemp(), status.getCpu(), (int) status.getFps());
122
            telemetry.addData("Pipeline", "Index: %d, Type: %s",
123
                    status.getPipelineIndex(), status.getPipelineType());
124

125

126
            if (result.isValid()) {
127
                // Access general information
128
                Pose3D botpose = result.getBotpose();
129
                double captureLatency = result.getCaptureLatency();
130
                double targetingLatency = result.getTargetingLatency();
131
                double parseLatency = result.getParseLatency();
132
                //telemetry.addData("LL Latency", captureLatency + targetingLatency);
133
                //telemetry.addData("Parse Latency", parseLatency);
134
                //telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput()));
135

136
                double angleToGoalDegrees = limelightMountAngleDegrees + targetOffsetAngle_Vertical;
137
                double angleToGoalRadians = angleToGoalDegrees * (3.14 / 180.0);
138
                //计算距离
139
                double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches) / Math.tan(angleToGoalRadians);
140
                double Distance = (distanceFromLimelightToGoalInches * 0.0254);
141
                telemetry.addData("Distance (m)", Distance);
142
                telemetry.addData("tx", result.getTx());
143
                telemetry.addData("txnc", result.getTxNC());
144
                telemetry.addData("ty", result.getTy());
145
                telemetry.addData("tync", result.getTyNC());
146

147
                telemetry.addData("Botpose", botpose.toString());
148

149
                // Access barcode results
150
                List<LLResultTypes.BarcodeResult> barcodeResults = result.getBarcodeResults();
151
                for (LLResultTypes.BarcodeResult br : barcodeResults) {
152
                    telemetry.addData("Barcode", "Data: %s", br.getData());
153
                }
154

155
                // Access classifier results
156
                List<LLResultTypes.ClassifierResult> classifierResults = result.getClassifierResults();
157
                for (LLResultTypes.ClassifierResult cr : classifierResults) {
158
                    telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence());
159
                }
160

161
                // Access detector results
162
                List<LLResultTypes.DetectorResult> detectorResults = result.getDetectorResults();
163
                for (LLResultTypes.DetectorResult dr : detectorResults) {
164
                    telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea());
165
                }
166

167
                // Access fiducial results
168
                List<LLResultTypes.FiducialResult> fiducialResults = result.getFiducialResults();
169
                for (LLResultTypes.FiducialResult fr : fiducialResults) {
170
                    telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees());
171
                }
172

173
                // Access color results
174
                List<LLResultTypes.ColorResult> colorResults = result.getColorResults();
175
                for (LLResultTypes.ColorResult cr : colorResults) {
176
                    telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees());
177
                }
178
            } else {
179
                telemetry.addData("Limelight", "No data available");
180
            }
181

182
            telemetry.update();
183
            // 计算距离
184

185

186
        }
187
        limelight.stop();
188
    }
189
}

学习如何删除李铭泽的工程笔记

qwq（飞书竟然可以写公式！！！）

\Delta_{\text{判}} = v_0^4 - g \cdot D \cdot \left( 2 \cdot \Delta h \cdot v_0^2 + g \cdot D^2 \right) \geq 0

\tan\theta = \frac{v_0^2 \pm \sqrt{\Delta_{\text{判}}}}{g \cdot D}

还可以上传 Java 文件！！！

10.7#

今日内容，研究了一上午为什么主机一下程序就无限重启的问题，下午测试了一些飞轮的方案加上把转轮的驱动器换成了电机，编了程。

10.8#

左轮结构的程序进一步优化算法，旋转驱动器由舵机转换为电机。运用编码器确定位置编程。

1
while(opModeIsActive()){
2
    rotateMotorCurrentPosition = robot.rotateMotor.getCurrentPosition();
3
    if(gamepad2.b){
4
        while(gamepad2.b){
5
            if(!gamepad2.b){
6
                break;
7
            }
8
        }
9
        rotateMotorTargetPosition+=96;//(288/3);
10
    }
11
    rotateMotorPower =(rotateMotorTargetPosition-rotateMotorCurrentPosition)*0.01;
12
    if(0<rotateMotorPower && rotateMotorPower<=rotateMotorMinPower){
13
        rotateMotorPower = rotateMotorMinPower;
14
    }
15
    if(rotateMotorPower<0 && rotateMotorPower>=-rotateMotorMinPower){
16
        rotateMotorPower =- rotateMotorMinPower;
17
    }
18
    if(Math.abs(rotateMotorTargetPosition-rotateMotorCurrentPosition)<=5){
19
        rotateMotorPower = 0;
20
    }
21
    robot.rotateMotor.setPower(rotateMotorPower);
22
    telemetry.addData("旋转功率", "%4.2f",rotateMotorPower);
23
    telemetry.addData("旋转位置", "%7d",rotateMotorCurrentPosition);
24
    telemetry.addData("目标位置", "%7d",rotateMotorTargetPosition);
25
    telemetry.update();

首先，while(opModeIsActive()) 是一个主循环，确保代码在操作模式激活时持续运行。每次循环开始时，代码通过 robot.rotateMotor.getCurrentPosition() 获取当前电机的位置，并存储在 rotateMotorCurrentPosition 中。

接着，代码检查 gamepad2.b 按钮是否被按下。如果按下，进入一个嵌套的 while(gamepad2.b) 循环，确保在按钮释放之前不会继续执行其他逻辑。按钮释放后，目标位置 rotateMotorTargetPosition 增加 96（即 rotateMotorTargetPosition += 96），这可能对应于电机旋转的一个固定步长。

然后，代码计算目标位置与当前电机位置之间的差值，并乘以一个系数 0.01 来确定电机的功率：

1
rotateMotorPower = (rotateMotorTargetPosition - rotateMotorCurrentPosition) * 0.01;

为了避免功率过小导致电机无法启动，代码设置了一个最小功率限制：

如果功率为正且小于等于 rotateMotorMinPower，将功率设置为 rotateMotorMinPower。

如果功率为负且绝对值小于等于 rotateMotorMinPower，将功率设置为 -rotateMotorMinPower。

此外，当目标位置与当前电机位置的差值小于或等于 5 时，功率被设置为 0，表示电机停止：

1
if (Math.abs(rotateMotorTargetPosition - rotateMotorCurrentPosition) <= 5) {
2
    rotateMotorPower = 0;
3
}

最后，代码通过 robot.rotateMotor.setPower(rotateMotorPower) 将计算出的功率应用到电机，并通过 telemetry 输出当前功率、位置和目标位置的信息到遥控器屏幕，方便实时监控。

最后把他写成线程合并到主程序里

1
package org.firstinspires.ftc.teamcode.FTC_27650_TeleOp;
2

3
import android.graphics.Color;
4

5
import com.acmerobotics.dashboard.config.Config;
6
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
7
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
8
import com.qualcomm.robotcore.hardware.DcMotor;
9
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
10
import com.qualcomm.robotcore.util.ElapsedTime;
11

12

13
@TeleOp(name = "手动27650")
14
@Config
15

16
public class MecanumWheel extends LinearOpMode {
17
    public static double strikerServoSpeed = 0.0001;
18
    public static double greenMin = 152, greenMax = 185;
19
    public static double purpleMin = 225, purpleMax = 250;
20
    public static double rotatePowerStart = 0.2;
21
    public static double rotateMotorMinPower = 0.6;
22
    final float[] hsvValuesLeft = new float[3]; //左边色调
23
    final float[] hsvValuesRight = new float[3];//右边色调
24
    private final ElapsedTime runtime = new ElapsedTime();
25
    public String colorLeft = "无";
26
    public String colorRight = "无";
27
    MyRobotHardware_27650_TeleOp robot = new MyRobotHardware_27650_TeleOp(this);
28
    Mecanum_1Thread Mecanum_1Thread = new Mecanum_1Thread();
29
    inBallPrepareThread inBallPrepareThread = new inBallPrepareThread();
30
    outBallPrepareThread outBallPrepareThread = new outBallPrepareThread();
31
    //public static double rotateServoPosition = 0.5;//旋转模式舵机 0.5停止    0正着转   1反着转
32
    //public static double rotateServoSpeed = 0.5; //  0 - 0.5 舵机旋转变快
33
    double strikerServoPosition = 0.6;//角度舵机    0.5代表转到中间
34
    boolean servoUsing = true;
35
    float gain = 2;//颜色传感器增益值，要>=1
36
    boolean magnetic_out_bool = false;
37
    boolean magnetic_in_bool = false;
38

39
    int rotateMotorTargetPosition = 0;
40
    double rotateMotorPower = 0;
41
    int a = 1;
42
    int b = 1;
43
    double rotateMotorCurrentPosition = 0;
44
    final int step = 96;
45

46
    @Override
47
    public void runOpMode() {
48

49
        robot.init();
50
        rotateMotorCurrentPosition =robot.rotateMotor.getCurrentPosition();
51
        robot.strikerServo.setPosition(strikerServoPosition);
52
        sleep(200);
53

54
        while (!robot.magnetic_in.isPressed()) {
55
            robot.rotateMotor.setPower(rotatePowerStart);
56
        }
57
        robot.rotateMotor.setPower(0);
58

59
        robot.rotateMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
60
        robot.rotateMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
61

62
        waitForStart();
63
        Mecanum_1Thread.start();
64
        //inBallPrepareThread.start();
65
        while (opModeIsActive()) {
66
            rotateMotorCurrentPosition = robot.rotateMotor.getCurrentPosition();
67

68

69
            if (gamepad2.left_bumper) {
70
                while (gamepad2.left_bumper) {
71
                    if (!gamepad2.left_bumper) {
72
                        break;
73
                    }
74
                }
75
                inBallPrepareThread.start();
76

77

78
            }
79
            if (gamepad2.right_bumper) {
80
                while (gamepad2.right_bumper) {
81
                    if (!gamepad2.right_bumper) {
82
                        break;
83
                    }
84
                }
85
                outBallPrepareThread.start();
86

87
            }
88
            if (servoUsing) servoControl();
89
            colorSensor();
90
            show();
91
        }
92
    }
93

94
    public void show() {
95
        magnetic_out_bool = robot.magnetic_out.isPressed();
96
        magnetic_in_bool = robot.magnetic_in.isPressed();
97

98
        //telemetry.addData("前轮 左/右","%4.2f, %4.2f", robot.fl.getPower(),robot.fr.getPower());
99
        //telemetry.addData("后轮 左/右","%4.2f, %4.2f", robot.bl.getPower(),robot.br.getPower());
100
        //telemetry.addData("旋转舵机","%4.2f", rotateServoPosition);
101
        telemetry.addData("向上抬球", "%4.2f", strikerServoPosition);
102
        telemetry.addData("Gain", gain);
103
        telemetry.addData("色调 左/右", "%.3f, %.3f", hsvValuesLeft[0], hsvValuesRight[0]);
104
        telemetry.addData("球颜色 左/右", "%s, %s", colorLeft, colorRight);
105
        telemetry.addData("磁性限位开关 out/in", "%b, %b", magnetic_out_bool, magnetic_in_bool);
106
        telemetry.addData("旋转功率", "%4.2f", rotateMotorPower);
107
        telemetry.addData("旋转位置", "%7d", rotateMotorCurrentPosition);
108
        telemetry.addData("目标位置", "%7d", rotateMotorTargetPosition);
109
        telemetry.update();
110
    }
111

112
    public void servoControl() {
113
        if (gamepad2.dpad_up)
114
            strikerServoPosition = Math.min(strikerServoPosition + strikerServoSpeed, 1);
115
        if (gamepad2.dpad_down)
116
            strikerServoPosition = Math.max(strikerServoPosition - strikerServoSpeed, 0);
117

118
        if (gamepad2.y) strikerServoPosition = 0.3;  //一键抬升
119
        if (gamepad2.a) strikerServoPosition = 0.6;  //一键下降
120
/*
121
        if(strikerServoPosition == 0.6){
122
            if(gamepad2.x) rotateServoPosition = 0.5 - rotateServoSpeed;
123
            if(gamepad2.b) rotateServoPosition = 0.5 + rotateServoSpeed;
124
            if(!gamepad2.x && !gamepad2.b) rotateServoPosition = 0.5;
125
        }else rotateServoPosition = 0.5;
126

127
 */
128
        //robot.rotateServo.setPosition(rotateServoPosition);
129
        robot.strikerServo.setPosition(strikerServoPosition);
130
    }
131

132
    public void colorSensor() {
133
        if (gamepad1.a) gain += 0.005;
134
        else if (gamepad1.b && gain > 1) gain -= 0.005;
135
        robot.colorSensorLeft.setGain(gain);
136
        robot.colorSensorRight.setGain(gain);
137

138
        NormalizedRGBA colorsLeft = robot.colorSensorLeft.getNormalizedColors();
139
        NormalizedRGBA colorsRight = robot.colorSensorRight.getNormalizedColors();
140

141
        Color.colorToHSV(colorsLeft.toColor(), hsvValuesLeft);
142
        Color.colorToHSV(colorsRight.toColor(), hsvValuesRight);
143

144
        if (greenMin <= hsvValuesLeft[0] && hsvValuesLeft[0] <= greenMax) colorLeft = "green";
145
        else if (purpleMin <= hsvValuesLeft[0] && hsvValuesLeft[0] <= purpleMax)
146
            colorLeft = "purple";
147
        else colorLeft = "无";
148
        if (greenMin <= hsvValuesRight[0] && hsvValuesRight[0] <= greenMax) colorRight = "green";
149
        else if (purpleMin <= hsvValuesRight[0] && hsvValuesRight[0] <= purpleMax)
150
            colorRight = "purple";
151
        else colorRight = "无";
152
    }
153

154
    public class Mecanum_1Thread extends Thread {
155
        public void run() {
156
            while (opModeIsActive()) {
157
                double y = -gamepad1.left_stick_y;
158
                double x = gamepad1.left_stick_x;
159
                double rx = gamepad1.right_stick_x;
160

161
                double flPower = y + x + rx;
162
                double frPower = y - x - rx;
163
                double brPower = y + x - rx;
164
                double blPower = y - x + rx;
165

166
                robot.fl.setPower(flPower);
167
                robot.fr.setPower(frPower);
168
                robot.br.setPower(brPower);
169
                robot.bl.setPower(blPower);
170
            }
171
        }
172
    }
173

174
    public class inBallPrepareThread extends Thread {
175

176
        public void run() {
177

178
            strikerServoPosition = 0.6;  //一键下降
179
            robot.strikerServo.setPosition(strikerServoPosition);
180
            try {
181
                sleep(200);
182
            } catch (InterruptedException e) {
183
                throw new RuntimeException(e);
184
            }
185

186
            if (a == 1) {
187
                rotateMotorTargetPosition += step;//(288/3);
188
            }
189

190

191
            while (opModeIsActive()) {
192
                rotateMotorPower = (rotateMotorTargetPosition - rotateMotorCurrentPosition) * 0.01;
193

194
                if (0 < rotateMotorPower && rotateMotorPower <= rotateMotorMinPower) {
195
                    rotateMotorPower = rotateMotorMinPower;
196
                }
197
                if (rotateMotorPower < 0 && rotateMotorPower >= -rotateMotorMinPower) {
198
                    rotateMotorPower = -rotateMotorMinPower;
199
                }
200
                if (Math.abs(rotateMotorTargetPosition - rotateMotorCurrentPosition) <= 5) {
201

202
                    break;
203
                }
204

205
                robot.rotateMotor.setPower(rotateMotorPower);
206
            }
207
            rotateMotorPower = 0;
208

209
            robot.rotateMotor.setPower(rotateMotorPower);
210
            a = 1;
211
        }
212
    }
213

214
    public class outBallPrepareThread extends Thread {
215
        public void run() {
216

217
            strikerServoPosition = 0.6;  //一键下降
218
            robot.strikerServo.setPosition(strikerServoPosition);
219
            try {
220
                sleep(200);
221
            } catch (InterruptedException e) {
222
                throw new RuntimeException(e);
223
            }
224

225
            if (b == 1) {
226
                rotateMotorTargetPosition += step+20;//(288/3);
227
            }
228

229

230
            while (opModeIsActive()) {
231
                rotateMotorPower = (rotateMotorTargetPosition - rotateMotorCurrentPosition) * 0.01;
232

233
                if (0 < rotateMotorPower && rotateMotorPower <= rotateMotorMinPower) {
234
                    rotateMotorPower = rotateMotorMinPower;
235
                }
236
                if (rotateMotorPower < 0 && rotateMotorPower >= -rotateMotorMinPower) {
237
                    rotateMotorPower = -rotateMotorMinPower;
238
                }
239
                if (Math.abs(rotateMotorTargetPosition - rotateMotorCurrentPosition) <= 5) {
240

241
                    break;
242
                }
243

244
                robot.rotateMotor.setPower(rotateMotorPower);
245
            }
246
            rotateMotorPower = 0;
247

248
            robot.rotateMotor.setPower(rotateMotorPower);
249
            b = 1;
250
        }
251

252
    }
253

254
}

1
package org.firstinspires.ftc.teamcode.FTC_27650_TeleOp;
2

3
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
4
import com.qualcomm.robotcore.hardware.DcMotor;
5
import com.qualcomm.robotcore.hardware.DcMotorEx;
6
import com.qualcomm.robotcore.hardware.DcMotorSimple;
7
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
8
import com.qualcomm.robotcore.hardware.Servo;
9
import com.qualcomm.robotcore.hardware.TouchSensor;
10

11
public class MyRobotHardware_27650_TeleOp {
12

13
    public DcMotorEx fl = null;
14
    public DcMotorEx fr = null;
15
    public DcMotorEx br = null;
16
    public DcMotorEx bl = null;
17

18
    public DcMotorEx rotateMotor = null;
19

20
    //public Servo rotateServo = null;
21
    public Servo strikerServo = null;
22

23
    public NormalizedColorSensor colorSensorLeft;
24
    public NormalizedColorSensor colorSensorRight;
25

26
    public TouchSensor magnetic_out;
27
    public TouchSensor magnetic_in;
28

29
    private LinearOpMode myOpMode = null;
30
    public MyRobotHardware_27650_TeleOp (LinearOpMode opmode) {
31
       myOpMode = opmode;
32
    }
33

34
    public void init(){
35
        //端口配置
36
        fl = myOpMode.hardwareMap.get(DcMotorEx.class,"fl");//c0
37
        fr = myOpMode.hardwareMap.get(DcMotorEx.class,"fr");//c1
38
        br = myOpMode.hardwareMap.get(DcMotorEx.class,"br");//c2
39
        bl = myOpMode.hardwareMap.get(DcMotorEx.class,"bl");//c3
40

41
        rotateMotor = myOpMode.hardwareMap.get(DcMotorEx.class,"rm");//e0
42

43
        //rotateServo = myOpMode.hardwareMap.get(Servo.class,"rs");//cs0
44
        strikerServo = myOpMode.hardwareMap.get(Servo.class,"ss");//cs1
45

46
        colorSensorLeft = myOpMode.hardwareMap.get(NormalizedColorSensor.class, "csl");//cI1
47
        colorSensorRight = myOpMode.hardwareMap.get(NormalizedColorSensor.class, "csr");//cI2
48

49
        magnetic_out = myOpMode.hardwareMap.get(TouchSensor.class,"mo");//cd0
50
        magnetic_in = myOpMode.hardwareMap.get(TouchSensor.class,"mi");//cd2
51

52
        //制动
53
        fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
54
        fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
55
        br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
56
        bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
57

58
        rotateMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
59
        rotateMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
60
        rotateMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
61

62
        //马达方向
63
        fl.setDirection(DcMotorSimple.Direction.FORWARD);
64
        bl.setDirection(DcMotorSimple.Direction.FORWARD);
65
        fr.setDirection(DcMotorSimple.Direction.REVERSE);
66
        br.setDirection(DcMotorSimple.Direction.REVERSE);
67

68
        rotateMotor.setDirection(DcMotorSimple.Direction.REVERSE);
69

70
        //((SwitchableLight)colorSensorLeft).enableLight(true);
71
        //((SwitchableLight)colorSensorRight).enableLight(true);
72

73
    }
74

75
}

10.12#

[王泽国的日志]第四点，测试各种飞轮发射装置，编写以转速为条件的伺服电机控制程序(Dc motor EX)。得出结论：单电机单飞轮装置的动力不足以将标本投入框中，双飞轮发射器的动力足够，角度合适。运用基于 iPhone 的角度测量仪精准测量角度，建议实装时用一个

代码

1
package org.firstinspires.ftc.teamcode.ftc27650_test;
2

3
import com.acmerobotics.dashboard.FtcDashboard;
4
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
5
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
6
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
7
import com.qualcomm.robotcore.hardware.DcMotor;
8
import com.qualcomm.robotcore.hardware.DcMotorEx;
9

10
@TeleOp(name = "motor_test(电机测试)")
11
public class motor_test extends LinearOpMode {
12

13
    public DcMotorEx m0 = null;
14
    public DcMotorEx m1 = null;
15

16
    public void runOpMode() {
17
        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
18
        m0 = hardwareMap.get(DcMotorEx.class, "m0");
19
        m1 = hardwareMap.get(DcMotorEx.class, "m1");
20
        m0.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
21
        m1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
22
        m0.setDirection(DcMotorEx.Direction.REVERSE);
23
        m1.setDirection(DcMotorEx.Direction.FORWARD);
24
        m0.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
25
        m1.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
26
        m0.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
27
        m1.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
28

29

30
        waitForStart();
31

32
        while (opModeIsActive()) {
33
            m0.setVelocity(5000);
34
            m1.setVelocity(5000);
35
            telemetry.addData("转速m0", m0.getVelocity());
36
            telemetry.addData("转速m1", m1.getVelocity());
37
            telemetry.update();
38
        }
39
    }
40
}

10.19#

做了个死轮，花费一节课的时间，太浪费时间了。终于算是做好了

其实 Linux 安装 android studio 很好用，现在就在用

确定了侧板的材质，决定使用铝型材 cnc 切割侧板，任务交给了高天狗，复习了一下如何在 DDS 的装配体里画草图。

10.26#

今天铝型材料，陶瓷轴承和 gobuilda 的死轮到了，安装花费一节课。Tips 的麦克纳姆轮滚轴只需要拆掉三个就可以了。

下次都不用带电脑来了 qwq。写个工笔还要凑字数，实在是今天写不到 100 字。

11.2#

终于写上程序了…今天写了除左轮以及发射装置以外的所有程序（发射装置暂时只是一个马达速度，等于没写），今天一大早打开电脑发现 Android Studio 更新了，然后导入项目的时候报错 Cause: index == 7，以为是程序问题，最后观察日志发现是新版的**Android Studio 代理设置对话框 UI 渲染 Bug。**解决方案核心是「避免触发该对话框」或「修复对话框渲染环境」。也就是关闭代理。搞了我 15 分钟，我服了。

最新的代码如下：

1
package org.firstinspires.ftc.teamcode.FTC_27650_TeleOp;
2

3
import android.graphics.Color;
4

5
import com.acmerobotics.dashboard.config.Config;
6
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
7
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
8
import com.qualcomm.robotcore.hardware.DcMotor;
9
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
10
import com.qualcomm.robotcore.util.ElapsedTime;
11

12

13
@TeleOp(name = "手动27650")
14
@Config
15

16
public class MecanumWheel extends LinearOpMode {
17
    public static double strikerServoSpeed = 0.0001;
18
    public static double greenMin = 152, greenMax = 185;
19
    public static double purpleMin = 225, purpleMax = 250;
20
    public static double rotatePowerStart = 0.2;
21
    public static double rotateMotorMinPower = 0.6;
22
    public static double shooterMotorPower = 0;
23
    final float[] hsvValuesLeft = new float[3]; //左边色调
24
    final float[] hsvValuesRight = new float[3];//右边色调
25
    private final ElapsedTime runtime = new ElapsedTime();
26
    public String colorLeft = "无";
27
    public String colorRight = "无";
28
    MyRobotHardware_27650_TeleOp robot = new MyRobotHardware_27650_TeleOp(this);
29
    Mecanum_1Thread Mecanum_1Thread = new Mecanum_1Thread();
30
    inBallPrepareThread inBallPrepareThread = new inBallPrepareThread();
31
    outBallPrepareThread outBallPrepareThread = new outBallPrepareThread();
32
    shooterThread shooterThread = new shooterThread();
33
    //public static double rotateServoPosition = 0.5;//旋转模式舵机 0.5停止    0正着转   1反着转
34
    //public static double rotateServoSpeed = 0.5; //  0 - 0.5 舵机旋转变快
35
    double strikerServoPosition = 0.6;//角度舵机    0.5代表转到中间
36
    boolean servoUsing = true;
37
    float gain = 2;//颜色传感器增益值，要>=1
38
    boolean magnetic_out_bool = false;
39
    boolean magnetic_in_bool = false;
40

41
    int rotateMotorTargetPosition = 0;
42
    double rotateMotorPower = 0;
43
    double collectMotorPower = 0;
44
    int a = 1;
45
    int b = 1;
46
    double rotateMotorCurrentPosition = 0;
47
    final int step = 96;
48

49
    @Override
50
    public void runOpMode() {
51

52
        robot.init();
53
        rotateMotorCurrentPosition = robot.rotateMotor.getCurrentPosition();
54
        robot.strikerServo.setPosition(strikerServoPosition);
55
        sleep(200);
56

57
        while (!robot.magnetic_in.isPressed()) {
58
            robot.rotateMotor.setPower(rotatePowerStart);
59
        }
60
        robot.rotateMotor.setPower(0);
61

62
        robot.rotateMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
63
        robot.rotateMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
64

65
        waitForStart();
66
        Mecanum_1Thread.start();
67
        //inBallPrepareThread.start();
68
        while (opModeIsActive()) {
69
            rotateMotorCurrentPosition = robot.rotateMotor.getCurrentPosition();
70

71

72
            if (gamepad2.left_bumper) {
73
                while (gamepad2.left_bumper) {
74
                    if (!gamepad2.left_bumper) {
75
                        break;
76
                    }
77
                }
78
                inBallPrepareThread.start();
79

80

81
            }
82
            if (gamepad2.right_bumper) {
83
                while (gamepad2.right_bumper) {
84
                    if (!gamepad2.right_bumper) {
85
                        break;
86
                    }
87
                }
88
                outBallPrepareThread.start();
89

90
            }
91
            if (gamepad2.right_trigger >= 0.2) {
92
                shooterThread.start();
93
            }
94
            if (servoUsing) servoControl();
95
            colorSensor();
96
            motorControl();
97
            show();
98
        }
99
    }
100

101
    public void show() {
102
        magnetic_out_bool = robot.magnetic_out.isPressed();
103
        magnetic_in_bool = robot.magnetic_in.isPressed();
104

105
        //telemetry.addData("前轮 左/右","%4.2f, %4.2f", robot.fl.getPower(),robot.fr.getPower());
106
        //telemetry.addData("后轮 左/右","%4.2f, %4.2f", robot.bl.getPower(),robot.br.getPower());
107
        //telemetry.addData("旋转舵机","%4.2f", rotateServoPosition);
108
        telemetry.addData("向上抬球", "%4.2f", strikerServoPosition);
109
        telemetry.addData("Gain", gain);
110
        telemetry.addData("色调 左/右", "%.3f, %.3f", hsvValuesLeft[0], hsvValuesRight[0]);
111
        telemetry.addData("球颜色 左/右", "%s, %s", colorLeft, colorRight);
112
        telemetry.addData("磁性限位开关 out/in", "%b, %b", magnetic_out_bool, magnetic_in_bool);
113
        telemetry.addData("旋转功率", "%4.2f", rotateMotorPower);
114
        telemetry.addData("旋转位置", "%7d", rotateMotorCurrentPosition);
115
        telemetry.addData("目标位置", "%7d", rotateMotorTargetPosition);
116
        telemetry.update();
117
    }
118

119
    public void servoControl() {
120
        if (gamepad2.dpad_up)
121
            strikerServoPosition = Math.min(strikerServoPosition + strikerServoSpeed, 1);
122
        if (gamepad2.dpad_down)
123
            strikerServoPosition = Math.max(strikerServoPosition - strikerServoSpeed, 0);
124

125
        if (gamepad2.y) strikerServoPosition = 0.3;  //一键抬升
126
        if (gamepad2.a) strikerServoPosition = 0.6;  //一键下降
127
        robot.strikerServo.setPosition(strikerServoPosition);
128
    }
129

130
    public void colorSensor() {
131
        if (gamepad1.a) gain += 0.005F;
132
        else if (gamepad1.b && gain > 1) gain -= 0.005F;
133
        robot.colorSensorLeft.setGain(gain);
134
        robot.colorSensorRight.setGain(gain);
135

136
        NormalizedRGBA colorsLeft = robot.colorSensorLeft.getNormalizedColors();
137
        NormalizedRGBA colorsRight = robot.colorSensorRight.getNormalizedColors();
138

139
        Color.colorToHSV(colorsLeft.toColor(), hsvValuesLeft);
140
        Color.colorToHSV(colorsRight.toColor(), hsvValuesRight);
141

142
        if (greenMin <= hsvValuesLeft[0] && hsvValuesLeft[0] <= greenMax) colorLeft = "green";
143
        else if (purpleMin <= hsvValuesLeft[0] && hsvValuesLeft[0] <= purpleMax)
144
            colorLeft = "purple";
145
        else colorLeft = "无";
146
        if (greenMin <= hsvValuesRight[0] && hsvValuesRight[0] <= greenMax) colorRight = "green";
147
        else if (purpleMin <= hsvValuesRight[0] && hsvValuesRight[0] <= purpleMax)
148
            colorRight = "purple";
149
        else colorRight = "无";
150
    }
151
    public void motorControl() {
152
        if(!gamepad2.left_bumper && !gamepad2.right_bumper){
153
            collectMotorPower = 0;
154
        }
155
        if(gamepad2.left_bumper){
156
            collectMotorPower = 1;
157
        }
158
        if(gamepad2.right_bumper){
159
            collectMotorPower = -1;
160
        }
161
        robot.collectMotor.setPower(collectMotorPower);
162
        robot.shooterMotor1.setPower(shooterMotorPower);
163
        robot.shooterMotor2.setPower(shooterMotorPower);
164

165
    }
166

167

168
    public class Mecanum_1Thread extends Thread {
169
        public void run() {
170
            while (opModeIsActive()) {
171
                double y = -gamepad1.left_stick_y;
172
                double x = gamepad1.left_stick_x;
173
                double rx = gamepad1.right_stick_x;
174

175
                double flPower = y + x + rx;
176
                double frPower = y - x - rx;
177
                double brPower = y + x - rx;
178
                double blPower = y - x + rx;
179

180
                robot.fl.setPower(flPower);
181
                robot.fr.setPower(frPower);
182
                robot.br.setPower(brPower);
183
                robot.bl.setPower(blPower);
184
            }
185
        }
186
    }
187

188
    public class inBallPrepareThread extends Thread {
189

190
        public void run() {
191

192
            strikerServoPosition = 0.6;  //一键下降
193
            robot.strikerServo.setPosition(strikerServoPosition);
194
            try {
195
                sleep(200);
196
            } catch (InterruptedException e) {
197
                throw new RuntimeException(e);
198
            }
199

200
            if (a == 1) {
201
                rotateMotorTargetPosition += step;//(288/3);
202
            }
203

204

205
            while (opModeIsActive()) {
206
                rotateMotorPower = (rotateMotorTargetPosition - rotateMotorCurrentPosition) * 0.01;
207

208
                if (0 < rotateMotorPower && rotateMotorPower <= rotateMotorMinPower) {
209
                    rotateMotorPower = rotateMotorMinPower;
210
                }
211
                if (rotateMotorPower < 0 && rotateMotorPower >= -rotateMotorMinPower) {
212
                    rotateMotorPower = -rotateMotorMinPower;
213
                }
214
                if (Math.abs(rotateMotorTargetPosition - rotateMotorCurrentPosition) <= 5) {
215

216
                    break;
217
                }
218

219
                robot.rotateMotor.setPower(rotateMotorPower);
220
            }
221
            rotateMotorPower = 0;
222

223
            robot.rotateMotor.setPower(rotateMotorPower);
224
            a = 1;
225
        }
226
    }
227

228
    public class outBallPrepareThread extends Thread {
229
        public void run() {
230

231
            strikerServoPosition = 0.6;  //一键下降
232
            robot.strikerServo.setPosition(strikerServoPosition);
233
            try {
234
                sleep(200);
235
            } catch (InterruptedException e) {
236
                throw new RuntimeException(e);
237
            }
238

239
            if (b == 1) {
240
                rotateMotorTargetPosition += step + 20;//(288/3);
241
            }
242

243

244
            while (opModeIsActive()) {
245
                rotateMotorPower = (rotateMotorTargetPosition - rotateMotorCurrentPosition) * 0.01;
246

247
                if (0 < rotateMotorPower && rotateMotorPower <= rotateMotorMinPower) {
248
                    rotateMotorPower = rotateMotorMinPower;
249
                }
250
                if (rotateMotorPower < 0 && rotateMotorPower >= -rotateMotorMinPower) {
251
                    rotateMotorPower = -rotateMotorMinPower;
252
                }
253
                if (Math.abs(rotateMotorTargetPosition - rotateMotorCurrentPosition) <= 5) {
254

255
                    break;
256
                }
257

258
                robot.rotateMotor.setPower(rotateMotorPower);
259
            }
260
            rotateMotorPower = 0;
261

262
            robot.rotateMotor.setPower(rotateMotorPower);
263
            b = 1;
264
        }
265

266
    }
267
    public class shooterThread extends Thread {
268
        public void run() {
269
            if (gamepad2.right_trigger >= 0.2) {
270
                shooterMotorPower = 1;
271
            } else {
272
                shooterMotorPower = 0;
273
            }
274
        }
275

276
    }
277
}

1
package org.firstinspires.ftc.teamcode.FTC_27650_TeleOp;
2

3
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
4
import com.qualcomm.robotcore.hardware.DcMotor;
5
import com.qualcomm.robotcore.hardware.DcMotorEx;
6
import com.qualcomm.robotcore.hardware.DcMotorSimple;
7
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
8
import com.qualcomm.robotcore.hardware.Servo;
9
import com.qualcomm.robotcore.hardware.TouchSensor;
10

11
public class MyRobotHardware_27650_TeleOp {
12

13
    public DcMotorEx fl = null;
14
    public DcMotorEx fr = null;
15
    public DcMotorEx br = null;
16
    public DcMotorEx bl = null;
17

18
    public DcMotorEx rotateMotor = null;
19
    public DcMotorEx collectMotor = null;
20
    public DcMotorEx shooterMotor1 = null;
21
    public DcMotorEx shooterMotor2 = null;
22

23
    public Servo strikerServo = null;
24

25
    public NormalizedColorSensor colorSensorLeft;
26
    public NormalizedColorSensor colorSensorRight;
27

28
    public TouchSensor magnetic_out;
29
    public TouchSensor magnetic_in;
30

31
    private LinearOpMode myOpMode = null;
32
    public MyRobotHardware_27650_TeleOp (LinearOpMode opmode) {
33
       myOpMode = opmode;
34
    }
35

36
    public void init(){
37
        //端口配置
38
        fl = myOpMode.hardwareMap.get(DcMotorEx.class,"fl");//c0
39
        fr = myOpMode.hardwareMap.get(DcMotorEx.class,"fr");//c1
40
        br = myOpMode.hardwareMap.get(DcMotorEx.class,"br");//c2
41
        bl = myOpMode.hardwareMap.get(DcMotorEx.class,"bl");//c3
42

43
        rotateMotor = myOpMode.hardwareMap.get(DcMotorEx.class,"rm");//e0
44
        collectMotor = myOpMode.hardwareMap.get(DcMotorEx.class,"cm");//e1
45
        shooterMotor1 = myOpMode.hardwareMap.get(DcMotorEx.class,"sm1");//e2
46
        shooterMotor2 = myOpMode.hardwareMap.get(DcMotorEx.class,"sm2");//e3
47

48

49

50
        strikerServo = myOpMode.hardwareMap.get(Servo.class,"ss");//cs1
51

52
        colorSensorLeft = myOpMode.hardwareMap.get(NormalizedColorSensor.class, "csl");//cI1
53
        colorSensorRight = myOpMode.hardwareMap.get(NormalizedColorSensor.class, "csr");//cI2
54

55
        magnetic_out = myOpMode.hardwareMap.get(TouchSensor.class,"mo");//cd0
56
        magnetic_in = myOpMode.hardwareMap.get(TouchSensor.class,"mi");//cd2
57

58
        //制动
59
        fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
60
        fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
61
        br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
62
        bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
63

64
        rotateMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
65
        collectMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
66
        shooterMotor1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
67
        shooterMotor2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
68
        rotateMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
69
        rotateMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
70

71
        //马达方向
72
        fl.setDirection(DcMotorSimple.Direction.FORWARD);
73
        bl.setDirection(DcMotorSimple.Direction.FORWARD);
74
        fr.setDirection(DcMotorSimple.Direction.REVERSE);
75
        br.setDirection(DcMotorSimple.Direction.REVERSE);
76

77
        rotateMotor.setDirection(DcMotorSimple.Direction.REVERSE);
78
        collectMotor.setDirection(DcMotorSimple.Direction.FORWARD);
79
        shooterMotor1.setDirection(DcMotorSimple.Direction.FORWARD);
80
        shooterMotor2.setDirection(DcMotorSimple.Direction.REVERSE);
81

82
        //((SwitchableLight)colorSensorLeft).enableLight(true);
83
        //((SwitchableLight)colorSensorRight).enableLight(true);
84

85
    }
86

87
}

车快做好了，预计下节课完成整车的接线安装，开始调优。

贴标签ing…

11.9#

编完了所有的手动的程序，自学如何使用 git。

仓库：

可以通过远程上传的方式同步项目。

左轮的程序终于搞完了，把逻辑搞清楚了

1
public class greenBallThread extends Thread {
2
    public void run() {
3
        try {
4
            servoUsing = false;
5
            strikerServoPosition = 0.49;  //一键下降
6
            robot.strikerServo.setPosition(strikerServoPosition);
7
            sleep(200);
8
            if (a == 2 && (colorLeft.equals("green") || colorRight.equals("green"))) {
9
                rotateMotorTargetPosition += 0;//(288/3);
10
            } else {
11
                if (a == 2) {
12
                    rotateMotorTargetPosition += step;//(288/3);
13
                }
14
                if (a == 1) {
15
                    rotateMotorTargetPosition += (step - 20);//(288/3);
16
                }
17
                while (opModeIsActive() && greenBallThreadUsing) {
18
                    rotateMotorCurrentPosition = robot.rotateMotor.getCurrentPosition();
19
                    rotateMotorPower = (rotateMotorTargetPosition - rotateMotorCurrentPosition) * 0.01;
20
                    if (0 < rotateMotorPower && rotateMotorPower <= rotateMotorMinPower) {
21
                        rotateMotorPower = rotateMotorMinPower;
22
                    }
23
                    if (rotateMotorPower < 0 && rotateMotorPower >= -rotateMotorMinPower) {
24
                        rotateMotorPower = -rotateMotorMinPower;
25
                    }
26

27
                    robot.rotateMotor.setPower(rotateMotorPower);
28

29
                    if (Math.abs(rotateMotorTargetPosition - rotateMotorCurrentPosition) <= 5 || rotateMotorCurrentPosition > rotateMotorTargetPosition - 5) {
30
                        rotateMotorPower = 0;
31
                        robot.rotateMotor.setPower(rotateMotorPower);
32
                        sleep(100);
33
                        if (colorLeft.equals("green") || colorRight.equals("green")) {
34
                            break;
35
                        } else {
36
                            rotateMotorTargetPosition += step;//(288/3);
37
                        }
38
                    }
39

40
                }
41
                rotateMotorPower = 0;
42
                robot.rotateMotor.setPower(rotateMotorPower);
43
            }
44
            a = 2;
45
            servoUsing = true;
46
        } catch (InterruptedException e) {
47
            throw new RuntimeException(e);
48
        }
49
    }
50
}

解释：

1.据变量 a 和当前左右颜色决定是否调整 rotateMotorTargetPosition（如果 a==2 且已有绿色则不移动，否则加上 step 或 step-20）。

2.进入循环 while (opModeIsActive() && greenBallThreadUsing)：

读取当前编码器 rotateMotorCurrentPosition；

计算 rotateMotorPower = (target - current) * 0.01，并强制最小绝对功率为 rotateMotorMinPower（避免死转力矩过小）；

将功率写入 robot.rotateMotor。

当靠近目标（差值 <= 5 或当前 > target - 5）时：把电机停下、sleep(100)，如果检测到左右任一为绿色则退出循环，否则把 rotateMotorTargetPosition += step 继续下一段搜索。

4.退出循环后确保电机停 (power=0)，把 a = 2，恢复 servoUsing = true。

开启逻辑：

1
if (gamepad2.left_bumper) {
2
    purpleBallThreadUsing = false;
3
    inBallPrepareThreadUsing = false;
4
    while (gamepad2.left_bumper) {
5
        if (!gamepad2.left_bumper) {
6
            break;
7
        }
8
    }
9
    greenBallThread.start();
10
    purpleBallThreadUsing = true;
11
    inBallPrepareThreadUsing = true;
12
}

解释：

检测到 gamepad2.left_bumper 被按下后，先把 purpleBallThreadUsing 和 inBallPrepareThreadUsing 设为 false，请求其它线程停止工作。

while (gamepad2.left_bumper) { … } 这段是等待按键释放的循环；它会在按键保持按下时一直空转（忙等待），直到释放才继续。

释放后调用 greenBallThread.start() 启动绿色球处理线程，随后再次将 purpleBallThreadUsing 和 inBallPrepareThreadUsing 设为 true（恢复标志）。

11.15-16#

我也不知道干啥了，总之车能跑了，应该是测试了飞轮的速度，利用转速控制。发现两个问题。一，在自己写的线程中直接.get 马达转速会导致线程堵塞，占用 CPU 和内存，导致手柄延迟上升。二，单个电机最高转速（实际）为 4300 刻度/分钟。有点慢，估计要两个马达。所以耗电量巨大，平均一场比赛下来就要换一次电。吐槽：内个左轮转盘幻想着有多好用，实际上就是一坨打的。

12.14#

今天上课！写了一点点程序就。找到了可以在手动阶段确定自己的位置的程序：

1
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
2
Pose2d currentPose = drive.localizer.getPose();
3

4
// 提取坐标和角度
5
double x = currentPose.position.x;        // X坐标（英寸）
6
double y = currentPose.position.y;        // Y坐标（英寸）
7
double heading = currentPose.heading.toDouble(); // 方位角（弧度）
8

9
// 转换为角度
10
double headingDegrees = Math.toDegrees(heading);
11
telemetry.addData("x", x);
12
telemetry.addData("y", y);
13
telemetry.addData("heading (deg)", headingDegrees);
14
telemetry.update();
15

16
Pose2d pose = drive.localizer.getPose();
17
TelemetryPacket packet = new TelemetryPacket();
18
packet.fieldOverlay().setStroke("#3F51B5");
19
Drawing.drawRobot(packet.fieldOverlay(), pose);
20
FtcDashboard.getInstance().sendTelemetryPacket(packet);

这个代码借助借助 org.firstinspires.ftc.teamcode.MecanumDrive;所以要 import 一下。

我看了一下 LED 灯的编写方法

1
package org.firstinspires.ftc.teamcode.FTC_27650_Test;
2
/*
3
        Copyright (c) 2021-24 Alan Smith
4

5
        All rights reserved.
6

7
        Redistribution and use in source and binary forms, with or without modification,
8
        are permitted (subject to the limitations in the disclaimer below) provided that
9
        the following conditions are met:
10

11
        Redistributions of source code must retain the above copyright notice, this list
12
        of conditions and the following disclaimer.
13

14
        Redistributions in binary form must reproduce the above copyright notice, this
15
        list of conditions and the following disclaimer in the documentation and/or
16
        other materials provided with the distribution.
17

18
        Neither the name of Alan Smith nor the names of its contributors may be used to
19
        endorse or promote products derived from this software without specific prior
20
        written permission.
21

22
        NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
23
        LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24
        "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
25
        THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE
26
        ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
27
        FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
28
        DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
29
        SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30
        CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
31
        TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
32
        THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33
*/
34

35
import android.graphics.Color;
36

37
import com.qualcomm.hardware.sparkfun.SparkFunLEDStick;
38
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
39
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
40
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
41
import com.qualcomm.robotcore.util.Range;
42

43
/*
44
 * This OpMode illustrates how to use the SparkFun QWIIC LED Strip
45
 *
46
 * This is a simple way to add a strip of 10 LEDs to your robot where you can set the color of each
47
 * LED or the whole strip.   This allows for driver feedback or even just fun ways to show your team
48
 * colors.
49
 *
50
 * Why?
51
 * Because more LEDs == more fun!!
52
 *
53
 * This OpMode assumes that the QWIIC LED Stick is attached to an I2C interface named "back_leds" in the robot configuration.
54
 *
55
 * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
56
 * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
57
 *
58
 * You can buy this product here:  https://www.sparkfun.com/products/18354
59
 * Don't forget to also buy this to make it easy to connect to your Control or Expansion Hub:
60
 * https://www.sparkfun.com/products/25596
61
 */
62
@TeleOp(name = "Concept: LED Stick", group = "Concept")
63

64
public class ConceptLEDStick extends OpMode {
65
    private boolean wasUp;
66
    private boolean wasDown;
67
    private int brightness = 5;  // this needs to be between 0 and 31
68
    private final static double END_GAME_TIME = 120 - 30;
69

70
    private SparkFunLEDStick ledStick;
71

72
    @Override
73
    public void init() {
74
        ledStick = hardwareMap.get(SparkFunLEDStick.class, "back_leds");
75
        ledStick.setBrightness(brightness);
76
        ledStick.setColor(Color.GREEN);
77
    }
78

79
    @Override
80
    public void start() {
81
        resetRuntime();
82
    }
83

84
    @Override
85
    public void loop() {
86
        telemetry.addLine("Hold the A button to turn blue");
87
        telemetry.addLine("Hold the B button to turn red");
88
        telemetry.addLine("Hold the left bumper to turn off");
89
        telemetry.addLine("Use DPAD Up/Down to change brightness");
90

91
        if (getRuntime() > END_GAME_TIME) {
92
            int[] ledColors = {Color.RED, Color.YELLOW, Color.RED, Color.YELLOW, Color.RED,
93
                    Color.YELLOW, Color.RED, Color.YELLOW, Color.RED, Color.YELLOW};
94
            ledStick.setColors(ledColors);
95
        } else if (gamepad1.a) {
96
            ledStick.setColor(Color.BLUE);
97
        } else if (gamepad1.b) {
98
            ledStick.setColor(Color.RED);
99
        } else if (gamepad1.left_bumper) {
100
            ledStick.turnAllOff();
101
        } else {
102
            ledStick.setColor(Color.GREEN);
103
        }
104

105
        /*
106
         * Use DPAD up and down to change brightness
107
         */
108
        int newBrightness = brightness;
109
        if (gamepad1.dpad_up && !wasUp) {
110
            newBrightness = brightness + 1;
111
        } else if (gamepad1.dpad_down && !wasDown) {
112
            newBrightness = brightness - 1;
113
        }
114
        if (newBrightness != brightness) {
115
            brightness = Range.clip(newBrightness, 0, 31);
116
            ledStick.setBrightness(brightness);
117
        }
118
        telemetry.addData("Brightness", brightness);
119

120
        wasDown = gamepad1.dpad_down;
121
        wasUp = gamepad1.dpad_up;
122
    }
123
}

对于我来说还是很简单的。

把车要改进的地方都搭好了，下节课就能跑了。

上节课的内容：使用 limelight

1
package org.firstinspires.ftc.teamcode.FTC_27650_Test;
2

3
import com.acmerobotics.dashboard.config.Config;
4
import com.qualcomm.hardware.limelightvision.LLResult;
5
import com.qualcomm.hardware.limelightvision.LLResultTypes;
6
import com.qualcomm.hardware.limelightvision.Limelight3A;
7
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
8
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
9

10
import java.util.List;
11

12
@TeleOp(name = "limelight", group = "limelight")
13
@Config
14
public class limelight extends LinearOpMode {
15
    public static double limelightMountAngleDegrees = 0; // 您的limelight从完全垂直方向向后旋转了多少度？
16
    public static double limelightLensHeightInches = 6.1; // Limelight镜头中心到地面的距离
17
    public static double goalHeightInches = 29.33071;// 目标到地面的距离
18

19
    @Override
20

21
    public void runOpMode() throws InterruptedException {
22
        Limelight3A limelight;
23
        limelight = hardwareMap.get(Limelight3A.class, "limelight");
24
        limelight.setPollRateHz(90); // 这设置了我们向Limelight请求数据的频率（每秒90次）
25
        limelight.pipelineSwitch(0); // 切换到管道编号0
26

27

28
        waitForStart();
29
        while (opModeIsActive()) {
30
            limelight.start(); // 这告诉Limelight开始观察！
31
            LLResult result = limelight.getLatestResult();
32
            if (result != null && result.isValid()) {
33
                double tx = result.getTx(); // 目标在左右方向上的偏移（度）
34
                double ty = result.getTy(); // 目标在上下方向上的偏移（度）
35
                double ta = result.getTa(); // 目标看起来有多大（图像的0%-100%）
36

37
                telemetry.addData("目标 X", tx);
38
                telemetry.addData("目标 Y", ty);
39
                telemetry.addData("目标大小", ta);
40
                List<LLResultTypes.FiducialResult> fiducialResults = result.getFiducialResults();
41
                for (LLResultTypes.FiducialResult fr : fiducialResults) {
42
                    telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees());
43
                }
44

45
                // Estimating_Distance（估算距离）方法的使用示例
46
                // 你需要提供实际的安装角度和高度
47

48
                double distance = Estimating_Distance(ty, limelightMountAngleDegrees, limelightLensHeightInches, goalHeightInches);
49
                telemetry.addData("Estimated Distance", distance);
50

51
            } else {
52
                telemetry.addData("Limelight", "没有目标");
53
            }
54
            telemetry.update();
55
        }
56
    }
57

58
    public double Estimating_Distance(double targetOffsetAngle_Vertical, double limelightMountAngleDegrees, double limelightLensHeightInches, double goalHeightInches) {
59
        // NetworkTables 代码已被移除，因为它是为 FRC 设计的，不适用于 FTC。
60
        // 我们直接使用传入的 targetOffsetAngle_Vertical (ty)。
61

62
        double angleToGoalDegrees = limelightMountAngleDegrees + targetOffsetAngle_Vertical;
63
        double angleToGoalRadians = angleToGoalDegrees * (Math.PI / 180.0);
64

65
        //计算距离
66

67
        // 必须返回计算出的值
68
        return (goalHeightInches - limelightLensHeightInches) / Math.tan(angleToGoalRadians);
69
    }
70

71
}