机器人硬件组织(RobotHardware)¶
约 715 个字 164 行代码 预计阅读时间 4 分钟
RobotHardware类是一个提供机器人硬件抽象和管理的实用工具类,旨在将硬件初始化和控制逻辑从操作模式中分离出来。
功能描述¶
该类提供了:
-
机器人硬件资源的中央管理点
-
硬件设备的初始化和配置
-
基本的机器人移动和控制功能
-
标准化的硬件访问接口
-
可以被多个操作模式重用的硬件管理类
类关系¶
-
与LinearOpMode紧密协作,需要在构造时传入操作模式实例
-
使用DcMotor类控制驱动和机械臂电机
-
使用Servo类控制手爪舵机
-
与Range类配合限制舵机位置值
-
通常与ConceptExternalHardwareClass示例一起使用
重要成员变量¶
私有硬件引用¶
| 变量名 | 类型 | 描述 |
|---|---|---|
| myOpMode | LinearOpMode | 对调用此类的操作模式的引用 |
| leftDrive | DcMotor | 左侧驱动电机 |
| rightDrive | DcMotor | 右侧驱动电机 |
| armMotor | DcMotor | 机械臂电机 |
| leftHand | Servo | 左爪舵机 |
| rightHand | Servo | 右爪舵机 |
公共常量¶
| 常量名 | 类型 | 描述 |
|---|---|---|
| MID_SERVO | static final double | 舵机中间位置(0.5) |
| HAND_SPEED | static final double | 舵机移动速率(0.02) |
| ARM_UP_POWER | static final double | 机械臂向上功率(0.45) |
| ARM_DOWN_POWER | static final double | 机械臂向下功率(-0.45) |
主要方法¶
/**
* 构造函数 - 接受一个操作模式的引用
* @param opmode 调用此硬件类的LinearOpMode实例
*/
public RobotHardware(LinearOpMode opmode)
/**
* 初始化机器人的所有硬件
* 在操作模式初始化时必须调用此方法一次
*/
public void init()
/**
* 计算实现请求的机器人运动所需的左/右电机功率
* @param Drive 前进/后退驱动功率(-1.0到1.0)正值为前进
* @param Turn 右/左转向功率(-1.0到1.0)正值为顺时针
*/
public void driveRobot(double Drive, double Turn)
/**
* 将请求的车轮电机功率传递给适当的硬件驱动电机
* @param leftWheel 左轮前进/后退驱动功率(-1.0到1.0)正值为前进
* @param rightWheel 右轮前进/后退驱动功率(-1.0到1.0)正值为前进
*/
public void setDrivePower(double leftWheel, double rightWheel)
/**
* 将请求的机械臂功率传递给适当的硬件驱动电机
* @param power 驱动功率(-1.0到1.0)
*/
public void setArmPower(double power)
/**
* 根据传递的偏移量,将两个手部舵机发送到相反(镜像)位置
* @param offset 舵机范围中点(0.5)的偏移量。正值将打开爪子,负值将关闭爪子。
*/
public void setHandPositions(double offset)
方法详解¶
构造函数¶
创建 RobotHardware 实例,需要传入 LinearOpMode 引用以访问硬件映射和遥测功能。
init¶
public void init() {
// 初始化电机
leftDrive = myOpMode.hardwareMap.get(DcMotor.class, "left_drive");
rightDrive = myOpMode.hardwareMap.get(DcMotor.class, "right_drive");
armMotor = myOpMode.hardwareMap.get(DcMotor.class, "arm");
// 设置电机方向
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
// 初始化舵机
leftHand = myOpMode.hardwareMap.get(Servo.class, "left_hand");
rightHand = myOpMode.hardwareMap.get(Servo.class, "right_hand");
leftHand.setPosition(MID_SERVO);
rightHand.setPosition(MID_SERVO);
// 显示初始化完成信息
myOpMode.telemetry.addData(">", "Hardware Initialized");
myOpMode.telemetry.update();
}
初始化所有硬件组件,配置默认设置,并向驾驶站报告完成状态。
driveRobot¶
public void driveRobot(double Drive, double Turn) {
// 计算左右电机功率
double left = Drive + Turn;
double right = Drive - Turn;
// 缩放功率值,确保不超过±1.0
double max = Math.max(Math.abs(left), Math.abs(right));
if (max > 1.0) {
left /= max;
right /= max;
}
// 设置电机功率
setDrivePower(left, right);
}
提供高级运动控制,结合直线驱动和转向功能。
setDrivePower¶
public void setDrivePower(double leftWheel, double rightWheel) {
leftDrive.setPower(leftWheel);
rightDrive.setPower(rightWheel);
}
直接设置驱动电机功率,用于更精细的控制。
setArmPower¶
控制机械臂电机功率。
setHandPositions¶
public void setHandPositions(double offset) {
offset = Range.clip(offset, -0.5, 0.5);
leftHand.setPosition(MID_SERVO + offset);
rightHand.setPosition(MID_SERVO - offset);
}
根据偏移量控制机器人爪子,实现镜像运动。
使用示例¶
基本用法¶
@TeleOp(name="Hardware Demo", group="Examples")
public class HardwareDemoOpMode extends LinearOpMode {
// 创建硬件对象
RobotHardware robot = new RobotHardware(this);
@Override
public void runOpMode() {
// 初始化硬件
robot.init();
telemetry.addData(">", "按下开始按钮运行");
telemetry.update();
// 等待开始
waitForStart();
// 运行循环
while (opModeIsActive()) {
// 获取控制器输入
double drive = -gamepad1.left_stick_y;
double turn = gamepad1.right_stick_x;
double armPower = gamepad2.left_stick_y;
double handOffset = gamepad2.right_stick_x / 2; // 将右摇杆X值缩小一半,控制爪子
// 控制机器人
robot.driveRobot(drive, turn);
robot.setArmPower(armPower);
robot.setHandPositions(handOffset);
// 更新遥测数据
telemetry.addData("驱动指令", "驱动(%.2f), 转向(%.2f)", drive, turn);
telemetry.addData("机械臂", "功率(%.2f)", armPower);
telemetry.addData("爪子", "偏移(%.2f)", handOffset);
telemetry.update();
// 控制频率
sleep(50);
}
}
}
更高级的使用案例¶
@Autonomous(name="Auto with Hardware Class", group="Examples")
public class AutoWithHardwareClass extends LinearOpMode {
RobotHardware robot = new RobotHardware(this);
@Override
public void runOpMode() {
// 初始化硬件
robot.init();
telemetry.addData(">", "机器人就绪");
telemetry.update();
waitForStart();
// 自主运行序列
// 前进1秒
robot.driveRobot(0.5, 0);
sleep(1000);
// 停止并打开爪子
robot.driveRobot(0, 0);
robot.setHandPositions(0.25);
sleep(500);
// 提起机械臂
robot.setArmPower(robot.ARM_UP_POWER);
sleep(1000);
robot.setArmPower(0);
// 右转90度
robot.driveRobot(0, 0.5);
sleep(500);
robot.driveRobot(0, 0);
// 完成
telemetry.addData(">", "路径完成");
telemetry.update();
}
}
设计理念¶
RobotHardware类遵循以下设计原则:
-
抽象硬件访问- 隐藏硬件细节,提供简单的接口
-
代码重用- 允许多个操作模式重用相同的硬件配置代码
-
集中管理- 在一个地方管理所有硬件配置和默认设置
-
分离关注点- 将硬件管理与机器人行为分离
注意事项¶
-
使用前确保在机器人配置中定义了所有必要的硬件(电机、舵机等)
-
在使用此类之前必须调用init()方法
-
需要根据实际机器人硬件配置修改类中的硬件名称
-
根据机器人驱动系统的构造,可能需要调整电机方向设置
-
如果需要添加更多硬件设备,扩展此类并添加相应的方法
-
此类设计用于基本的坦克式驱动机器人,其他驱动系统可能需要修改