这里对应的是使用舵机拿球放球的代码，此处功能还需要继续进行调参：
def __init__(self):
# 旋转舵机
self.servo_rotate = ServoBus(2)
self.servo_rotate.set_angle(90, 0)
# 放球舵机
self.servo_ball = ServoBus(3)
self.servo_ball.set_angle(80, -110)
time.sleep(0.3)
self.servo_ball.set_rotate(0)
self.servo_rotate.set_rotate(0)
self.servo_high = ServoPwm(2)
self.block_cfg = {"arm": {"grap": [-0.2, 0.02], "release": [-0.2, 0.02]},
"target":[-0.2, 0.02]}
self.block_mid_pos = [-0.12, 0.04]
# 机械臂
self.angle_ball = [-75, -95]
# self.cylinder_height = 0.14
# 机械臂
self.arm = ArmBase()舵机初始化
-
旋转舵机：
self.servo_rotate = ServoBus(2)：创建一个连接到总线位置2的旋转舵机实例。self.servo_rotate.set_angle(90, 0)：将旋转舵机设置到90度角，其中0可能表示操作的立即执行或是某种特定的速度/力度设置。
-
放球舵机：
self.servo_ball = ServoBus(3)：创建一个连接到总线位置3的放球舵机实例。self.servo_ball.set_angle(80, -110)：将放球舵机设置到80度角，-110可能表示这个动作的速度或者是某种特殊的力度设置。time.sleep(0.3)：在执行下一步动作前等待0.3秒，确保舵机动作完成。self.servo_ball.set_rotate(0)和self.servo_rotate.set_rotate(0)：可能是用来停止舵机的进一步旋转，将舵机的旋转速度设置为0。
高度调节舵机
self.servo_high = ServoPwm(2)：创建一个PWM控制的舵机实例，用于高度调节，连接到PWM通道2。
机械臂配置
self.block_cfg和self.block_mid_pos：设置了一些预设的坐标和配置，可能用于机械臂操作过程中的物体抓取和放置位置。"arm": {"grap": [-0.2, 0.02], "release": [-0.2, 0.02]}：定义了抓取和释放动作的坐标。"target":[-0.2, 0.02]：定义了目标放置位置的坐标。self.block_mid_pos = [-0.12, 0.04]：表示机械臂在执行任务时的中间位置坐标。
def save_ball(self):
pass
# self.servo_ball.set_angle(100, -50)
# time.sleep(0.3)
self.servo_ball.set_angle(100, -30)
# time.sleep(0.6)
def reset(self):
self.arm.reset()
self.arm.switch_side(-1)
def put_down_self_block(self):
target_height = 0.05
target_horiz = -0.12
arm_angle = 41
grab_angle = 130
# 平台达到指定角度，放平
self.servo_ball.set_angle(80, -80)
# 按照给定速度到达给定位置
self.arm.set(target_horiz, target_height+0.03)
# 到达指定角度
self.arm.set_arm_angle(arm_angle)
time.sleep(0.5)
# 下移0.03m
self.arm.set_offset(0, -0.02, 0.5)
# 指定角度
self.arm.set_grap_angle(grab_angle)
time.sleep(0.4)
# 上移0.05m
self.arm.set_offset(0, 0.03, 0.8)
# 返回初始角度
self.arm.set_arm_dir(-1)
time.sleep(0.5)
self.arm.set_offset(0, -0.1, 1.5)save_ball 方法
此方法用于将一个球体移动到特定的角度。
- 注释代码：
- 原始代码中有注释的部分显示先前可能设置过其他角度并伴有延时，这些被注释可能是为了调试或改进流程。
self.servo_ball.set_angle(100, -30)：调整放球舵机到100度的位置，-30可能代表速度或特定的执行参数。
reset 方法
此方法用于重置机械臂到一个基础的或安全的状态。
- 执行流程：
self.arm.reset()：重置机械臂到初始状态或默认位置。self.arm.switch_side(-1)：可能是设置机械臂操作的方向或切换到特定的配置侧。
put_down_self_block 方法
此方法涉及一系列复杂的动作，用于将一个物块放置到指定的位置。
- 执行步骤：
self.servo_ball.set_angle(80, -80)：首先调整放球舵机到80度，-80可能指的是运动速度或力度。self.arm.set(target_horiz, target_height+0.03)：移动机械臂到指定的水平和垂直位置。self.arm.set_arm_angle(arm_angle)：设置机械臂的角度。time.sleep(0.5)：等待0.5秒以稳定机械臂位置。self.arm.set_offset(0, -0.02, 0.5)：微调机械臂的位置，下移0.02米。self.arm.set_grap_angle(grab_angle)：调整抓取物块的角度。time.sleep(0.4)：等待0.4秒以完成抓取动作。self.arm.set_offset(0, 0.03, 0.8)：机械臂上移0.03米，以抬起物块。self.arm.set_arm_dir(-1)：可能是重置机械臂的方向或返回到初始角度。time.sleep(0.5)：等待0.5秒以完成动作。self.arm.set_offset(0, -0.1, 1.5)：最后将机械臂移动回起始位置或更低位置以放下物块。
def elevation_pole(self, arm_set=False):
pt_tar = []
if arm_set:
self.servo_rotate.set_angle(90, 0)
time.sleep(0.3)
self.servo_rotate.set_rotate(0)
return pt_tar
self.servo_rotate.set_rotate(100)
time.sleep(5)
self.servo_rotate.set_rotate(0)
self.servo_rotate.set_angle(90, 0)
time.sleep(0.3)
self.servo_rotate.set_rotate(0)
def pick_up_block(self, arm_set=False):
block_pt = [0, 0, "block", 0, -0.03, -0.28, 0.57, 0.8]
# 转换到右侧
self.arm.switch_side(-1)
tar_horiz = -0.20
tar_height = 0.03
angle_grab_ball = 100
# 抓手打开指定角度稍微大角度, 到达指定位置上方
self.arm.set_grap_angle(angle_grab_ball+25)
self.arm.set(tar_horiz+0.1, tar_height)
if arm_set:
return block_pt
self.arm.set(tar_horiz+0.05, tar_height)
# 右移0.05
self.arm.set_offset(-0.05, 0, 0.6)
# 抓取
self.arm.set_grap_angle(angle_grab_ball)
time.sleep(0.5)
# 上升
self.arm.set_offset(0, 0.02, 0.6)
self.arm.set_offset(0.06, 0, 1)
def put_down_block(self):
tar_height = 0
tar_horiz = -0.22
angle_grab_ball = 135
self.arm.set(tar_horiz+0.10, tar_height+0.05)
# 右移0.1
self.arm.set_offset(-0.13, 0, 1.5)
# 下降
self.arm.set_offset(0, -0.06, 0.8)
# 松开
self.arm.set_grap_angle(angle_grab_ball)
time.sleep(0.5)
# 上升
self.arm.set_offset(0, 0.05, 0.8)
# 左移0.1
self.arm.set_offset(0.1, 0)
def pick_up_ball(self, arm_set=False):
# 转换到左侧
tar = [4, 0, 'blue_ball', 0.6, -0.05, 0.15, 0.47, 0.62]
self.arm.switch_side(1)
tar_height = 0.09
angle_grab_ball = 95
tar_horiz = -0.12
self.arm.set(tar_horiz, tar_height)
if arm_set:
self.servo_ball.set_angle(80, -110)
time.sleep(0.3)
self.servo_ball.set_rotate(0)
return tar
self.arm.set_grap_angle(angle_grab_ball+25)
self.arm.set_offset(0.06, 0, 0.6)
self.arm.set_offset(0.05, 0, 0.6)
self.arm.set_grap_angle(angle_grab_ball)
time.sleep(0.5)
self.arm.set_offset(0, 0.03, 0.2)
self.arm.set_offset(-0.11, 0, 1)
def put_down_self_ball(self):
tar_height = 0.10
tar_horiz = -0.12
arm_angle = -52
grap_angle = 130
self.arm.set(tar_horiz, tar_height)
self.arm.set_arm_angle(arm_angle)
time.sleep(0.8)
# self.arm.set(-0.1, 0.105, 0.3)
self.arm.set_grap_angle(grap_angle)
time.sleep(0.4)
# self.arm.set(-0.1, 0.105, 0.3)
self.arm.set_arm_dir(1)
time.sleep(0.5)elevation_pole 方法
- 功能：调整旋转舵机的角度来定位或复位。
- 流程：
- 如果
arm_set为真，则旋转舵机设置到90度，并停止旋转，返回一个空列表。 - 如果
arm_set为假，则旋转舵机先以速度100旋转5秒，然后停止并设置到90度，最后再次停止旋转。
- 如果
pick_up_block 方法
- 功能：从指定位置抓取一个方块。
- 流程：
- 调整机械臂到抓取方块所需的初始角度和位置。
- 如果
arm_set为真，直接返回方块的位置参数。 - 否则，继续调整位置和角度，完成抓取动作并进行必要的位置校正。
put_down_block 方法
- 功能：在指定位置放下一个方块。
- 流程：
- 移动机械臂到指定的水平和高度位置。
- 执行一系列动作来精确放置方块，包括下降、松开抓手和上升。
pick_up_ball 方法
- 功能：从指定位置抓取一个球体。
- 流程：
- 调整机械臂到适合抓取球体的角度和位置。
- 如果
arm_set为真，则调整放球舵机的角度并等待，然后停止其旋转。 - 否则，继续调整抓取角度和位置，完成抓取动作并进行必要的位置校正。
put_down_self_ball 方法
- 功能：在指定位置放下一个球体。
- 流程：
- 调整机械臂到指定的角度和位置。
- 执行放下球体所需的动作序列，包括调整角度、等待和重新调整方向。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
