Intake

Posted on Edited on In

Intake车教程

1. 任务拆解

这个intake的手动程序涉及的硬件有两个舵机和一个电机，也就是说我们会接触到一个新的硬件servo。
在完成任务的过程中可能会遇到的主要问题就是servo的控制，所以本教程的重点就是控制servo。

2. servo的基本控制方式

我们在本次任务中使用的goBuilda servo有两种模式，普通servo模式和连续旋转servo(crservo)模式。
本程序中需要的模式是cr连续旋转模式。

两种servo模式对比
普通舵机模式：
在一个角度范围内 set to position，只能来回扫动无法连续旋转
连续旋转舵机模式：
也通过set to position 控制，但是可以连续旋转

2.0 如何把普通舵机模式转变成连续旋转舵机模式

这个步骤叫做烧录固件，需要用到programmer
(虽然使用的是goBuilda舵机,但是在这里我们用REV的programmer，这不会影响舵机的效果)
REV的官方文档中有详细的说明：https://docs.revrobotics.com/rev-crossover-products/servo/srs-programmer/switching-operating-modes
使用REVprogrammer烧录的程序如下：

  • 连接servo和programmer
    • 使用原本连接舵机和hub的杜邦线，注意插线的方向，白线也就是通信线要在左侧S标志的位置。
      \"ConnectProgrammer\"
  • 烧录固件
    • 把programmer的开关turn on，把模式开关滑到c(连续模式)。
      按下并释放 PROGRAM 按钮一次。
      PROGRAM LED 应闪烁，然后保持稳定，表示成功。
  • 测试
    • 在cr或servo模式下，按下并释放TEST可以循环两种测试模式
      第一次按下 - 自动扫描模式
      第二次按下 - 手动测试模式
      第三次按下 - 返回默认状态
      \"cr模式手动测试\"

2.1 servo控制的代码实现

就像前面所说的，我们用setPosition使舵机连续旋转

  • setPosition(1) - 舵机连续向前旋转
  • setPosition(0) - 舵机连续向后旋转
  • setPosition(0.5) - 舵机停止

2.1.0 一直连续旋转

1
2
intakeLeft.setPosition(1);
intakeRight.setPosition(1);

2.1.1 手柄控制旋转

1
2
3
4
5
6
7
8
9
10
11
12
13
if (gamepad1.right_bumper) {
    intakeMotor.setPower(motorInput);
    intakeLeft.setPosition(1);
    intakeRight.setPosition(1);
} else if (gamepad1.left_bumper) {
    intakeMotor.setPower(-motorInput);
    intakeLeft.setPosition(0);
    intakeRight.setPosition(0);
}else {
    intakeMotor.setVelocity(0);
    intakeLeft.setPosition(0.5);
    intakeRight.setPosition(0.5);
}

3. 完整代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;

@TeleOp(name = "IntakeTest")
public class IntakeTest extends LinearOpMode {
    private DcMotorEx intakeMotor;
    private Servo intakeLeft;
    private Servo intakeRight;
    @Override
    public void runOpMode() throws InterruptedException {

        DcMotorEx frontLeftMotor = hardwareMap.get(DcMotorEx.class,"leftFrontMotor");
        DcMotorEx backLeftMotor = hardwareMap.get(DcMotorEx.class,"leftBackMotor");
        DcMotorEx frontRightMotor = hardwareMap.get(DcMotorEx.class,"rightFrontMotor");
        DcMotorEx backRightMotor = hardwareMap.get(DcMotorEx.class,"rightBackMotor");

        frontLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
        backLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE); // only for this robot (Broken motor)

        double motorInput = 1.0;

        intakeMotor = hardwareMap.get(DcMotorEx.class,"intakeMotor");
        intakeLeft = hardwareMap.get(Servo.class,"intakeLeft");
        intakeRight = hardwareMap.get(Servo.class,"intakeRight");

        intakeRight.setDirection(Servo.Direction.REVERSE);
        intakeLeft.setDirection(Servo.Direction.FORWARD);

        intakeMotor.setDirection(DcMotorSimple.Direction.FORWARD);

        waitForStart();

        while(opModeIsActive()) {
            intakeMotor.setPower(motorInput);

            intakeLeft.setPosition(1);
            intakeRight.setPosition(1);
            

            double y = -gamepad1.left_stick_y;
            double x = gamepad1.left_stick_x * 1.1;
            double rx = gamepad1.right_stick_x;

            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
            double frontLeftPower = (y + x + rx) / denominator;
            double backLeftPower = (y - x + rx) / denominator;
            double frontRightPower = (y - x - rx) / denominator;
            double backRightPower = (y + x - rx) / denominator;

            double powerCoefficient = 1.0;

            frontLeftMotor.setPower(frontLeftPower * powerCoefficient);
            backLeftMotor.setPower(backLeftPower * powerCoefficient);
            frontRightMotor.setPower(frontRightPower * powerCoefficient);
            backRightMotor.setPower(backRightPower * powerCoefficient);
            }
        }
    }