为子系统编写代码

添加代码以创建实际工作子系统非常简单。对于不使用反馈的简单子系统,结果证明非常简单。在本节中,我们将看一个 Claw 子系统的示例。 Claw 子系统还有一个限位开关来确定物体是否在抓地力中。

爪子系统的RobotBuilder表示

../../../../../_images/writing-subsystem-code-1.png

机械臂末端的爪子是一个由单个 VictorSPX 电机控制器控制的子系统。我们希望电机做三件事,打开,关闭和停止移动。这是子系统的责任。打开和关闭的时间将在本教程后面的指令处讨论到。我们还将定义一个方法来判断爪子是否正在抓取物体。

添加子系统功能

11// ROBOTBUILDER TYPE: Subsystem.
12
13package frc.robot.subsystems;
14
15
16import frc.robot.commands.*;
17import edu.wpi.first.wpilibj.livewindow.LiveWindow;
18import edu.wpi.first.wpilibj2.command.SubsystemBase;
19
20// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
21import edu.wpi.first.wpilibj.DigitalInput;
22import edu.wpi.first.wpilibj.motorcontrol.MotorController;
23import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
24
25    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
26
27
28/**
29 *
30 */
31public class Claw extends SubsystemBase {
32    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
33public static final double PlaceDistance = 0.1;
34public static final double BackAwayDistance = 0.6;
35
36    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
37
38    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
39private PWMVictorSPX motor;
40private DigitalInput limitswitch;
41
42    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
43
44    /**
45    *
46    */
47    public Claw() {
48        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
49motor = new PWMVictorSPX(4);
50 addChild("motor",motor);
51 motor.setInverted(false);
52
53limitswitch = new DigitalInput(4);
54 addChild("limit switch", limitswitch);
55
56
57
58    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
59    }
60
61    @Override
62    public void periodic() {
63        // This method will be called once per scheduler run
64
65    }
66
67    @Override
68    public void simulationPeriodic() {
69        // This method will be called once per scheduler run when in simulation
70
71    }
72
73    public void open() {
74        motor.set(1.0);
75    }
76
77    public void close() {
78        motor.set(-1.0);
79    }
80
81    public void stop() {
82        motor.set(0.0);
83    }
84
85    public boolean isGripping() {
86        return limitswitch.get();
87    }
88
89}
11// ROBOTBUILDER TYPE: Subsystem.
12
13// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
14#include "subsystems/Claw.h"
15#include <frc/smartdashboard/SmartDashboard.h>
16
17// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
18
19Claw::Claw(){
20    SetName("Claw");
21    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
22    SetSubsystem("Claw");
23
24 AddChild("limit switch", &m_limitswitch);
25
26
27 AddChild("motor", &m_motor);
28 m_motor.SetInverted(false);
29
30    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
31}
32
33void Claw::Periodic() {
34    // Put code here to be run every loop
35
36}
37
38void Claw::SimulationPeriodic() {
39    // This method will be called once per scheduler run when in simulation
40
41}
42
43// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS
44
45// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS
46
47
48void Claw::Open() {
49    m_motor.Set(1.0);
50}
51
52void Claw::Close() {
53    m_motor.Set(-1.0);
54}
55
56void Claw::Stop() {
57    m_motor.Set(0.0);
58}
59
60bool Claw::IsGripping() {
61    return m_limitswitch.Get();
62}

将方法添加到 claw.javaclaw.cpp 将打开,关闭和停止爪子移动并获取爪子限位开关。这些将由实际操作爪子的命令使用。

备注

注释已从此文件中删除,以便更轻松地查看此文档的更改。

请注意,名为“motor”和“limitswitch”的成员变量是由 RobotBuilder 创建的,因此可以在整个子系统中使用。每个拖入的调色板项目都有一个成员变量,其名称在 RobotBuilder 中给出。

将方法声明添加到头文件(仅C ++)

11// ROBOTBUILDER TYPE: Subsystem.
12#pragma once
13
14// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
15#include <frc2/command/SubsystemBase.h>
16#include <frc/DigitalInput.h>
17#include <frc/motorcontrol/PWMVictorSPX.h>
18
19// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
20
21/**
22 *
23 *
24 * @author ExampleAuthor
25 */
26class Claw: public frc2::SubsystemBase {
27private:
28    // It's desirable that everything possible is private except
29    // for methods that implement subsystem capabilities
30    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
31frc::DigitalInput m_limitswitch{4};
32frc::PWMVictorSPX m_motor{4};
33
34    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
35public:
36Claw();
37
38    void Periodic() override;
39    void SimulationPeriodic() override;
40    void Open();
41    void Close();
42    void Stop();
43    bool IsGripping();
44    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS
45
46    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS
47    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
48static constexpr const double PlaceDistance = 0.1;
49static constexpr const double BackAwayDistance = 0.6;
50
51    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
52
53
54};

除了将方法添加到类实现文件“Claw.cpp”之外,还需要将方法的声明添加到头文件“Claw.h”中。必须添加的那些声明显示在此处。

要将行为添加到爪子子系统以处理打开和关闭,您需要 define commands