往期回顾
第一问： 需要将一颗黑棋，放入棋盘中的五号位置。 理想思路：依据摄像头，依据机械臂及其传感器。建立机械臂的逆运动学方程。然后完成精准定位，考虑到手搓机械臂的不稳定性。以及摄像头的精度。 所以本团队不使用上述思路。 实施思路：将一个黑色棋子的位置，与5号棋子的位置进行绑定。也就是说，确定三个舵机的角度即可。
下面附上我们团队的代码：
import RPi.GPIO as GPIO
import time
import cv2
import numpy as np
import copy
class ArmControl:
def __init__(self):
GPIO.setwarnings(False)
# 初始化舵机引脚
self.servos = {
'base': 16,
'shoulder': 20,
'elbow': 21
}
# 设定每个九宫格位置对应的舵机角度
self.position_angles = {
0: (55, 100, 80), # 初始位置
1: (91, 107, 40), # 位置 1: 基座 97°，肩部 14°，肘部 0°
2: (86, 106, 38), # 位置 2: 基座 90°，肩部 14°，肘部 0°
3: (81, 106, 36), # 位置 3: 基座 82°，肩部 14°，肘部 0°
4: (90, 102, 68), # 位置 4: 基座 97°，肩部 5°，肘部 3°
5: (86, 102, 68), # 位置 5: 基座 90°，肩部 0°，肘部 78°
6: (78, 102, 68), # 位置 6: 基座 82°，肩部 5°，肘部 3°
7: (89, 101, 88), # 位置 7: 基座 99°，肩部 0°，肘部 45°
8: (84, 102, 88), # 位置 8: 基座 90°，肩部 0°，肘部 45°
9: (78, 101, 88), # 位置 9: 基座 80°，肩部 0°，肘部 46°
'A1': (99, 108, 49), # 取棋子位置 1: 基座 97°，肩部 14°，肘部 0°
'B1': (101, 104, 71), # 取棋子位置 1: 基座 97°，肩部 14°，肘部 0°
'C1': (100, 104, 87), # 取棋子位置 1: 基座 97°，肩部 14°，肘部 0°
'D1': (103, 103, 100), # 取棋子位置 1: 基座 97°，肩部 14°，肘部 0°
'A2': (74, 107, 47), # 取棋子位置 2: 基座 97°，肩部 14°，肘部 0°
'B2': (70, 104, 73), # 取棋子位置 2: 基座 97°，肩部 14°，肘部 0°
'C2': (68, 104, 90), # 取棋子位置 2: 基座 97°，肩部 14°，肘部 0°
'D2': (63, 103, 104), # 取棋子位置 2: 基座 97°，肩部 14°，肘部 0°
}
# 设置GPIO模式
GPIO.setmode(GPIO.BCM)
# 设置舵机引脚
self.pwm = {}
self.last_time = 0 # 上一次设置时间
self.debounce_time = 0.5 # 防抖时间（秒）
for servo in self.servos.values():
GPIO.setup(servo, GPIO.OUT)
self.pwm[servo] = GPIO.PWM(servo, 50) # 50Hz
self.pwm[servo].start(0)
def set_servo_angle(self, pwm, target_angle, speed):
"""将舵机平滑旋转到指定角度，并实现防抖逻辑"""
current_time = time.time() # 获取当前时间
# 防抖逻辑
if current_time - self.last_time < self.debounce_time:
return # 如果距离上次设置时间小于防抖时间，则不执行
# 计算占空比
duty = self.angle_to_duty_cycle(target_angle)
# 确保占空比在合理范围内
if duty < 0:
duty = 0
elif duty > 12: # 假设最大是180度
duty = 12
pwm.ChangeDutyCycle(duty)
time.sleep(speed) # 按速度控制时间延迟
pwm.ChangeDutyCycle(0) # 停止PWM信号
self.last_time = current_time # 更新最后设置时间
def move_servo(self, servo_name, angle):
"""指定舵机移动到指定角度"""
self.set_servo_angle(self.pwm[self.servos[servo_name]], angle, 0.5) # 设置速度为0.5秒
def move_to_position(self, position):
"""移动机械臂到指定棋盘位置（1-9）"""
if position in self.position_angles:
angles = self.position_angles[position] # 获取角度元组
self.move_servo('base', angles[0])
time.sleep(0.5)# 移动基座
self.move_servo('elbow', angles[2])
time.sleep(0.5)
self.move_servo('shoulder', angles[1]) # 移动肩部
# 移动肘部
else:
print("无效的位置编号！")
def remove_to_position(self, position):
"""移动机械臂到指定棋盘位置（1-9）"""
if position in self.position_angles:
angles = self.position_angles[position] # 获取角度元组
self.move_servo('shoulder', angles[1])# 移动基座
time.sleep(0.5)
self.move_servo('elbow', angles[2])
time.sleep(1)
self.move_servo('base', angles[0])
else:
print("无效的位置编号！")
def angle_to_duty_cycle(self, angle):
"""将角度转换为PWM占空比（0到100之间）"""
return (angle / 18) + 2 # 示例转换，具体根据舵机类型调整
def cleanup(self):
"""清理GPIO设置"""
for pwm in self.pwm.values():
pwm.stop()
GPIO.cleanup()
class RelayController: # 电磁铁控制
def __init__(self, relay_pin,led_pin, debounce_time=0.5):
"""
初始化继电器控制类
:param relay_pin: 继电器连接的 GPIO 引脚
:param debounce_time: 控制继电器的状态保持时间（秒）
"""
self.relay_pin = relay_pin
self.led_pin=led_pin
self.debounce_time = debounce_time
# 设置 GPIO 模式为 BCM
GPIO.setmode(GPIO.BCM)
# 设置引脚为输出模式
GPIO.setup(self.relay_pin, GPIO.OUT)
GPIO.setup(self.led_pin, GPIO.OUT)
def turn_on(self):
"""开启继电器"""
print("继电器开启")
GPIO.output(self.relay_pin, GPIO.HIGH) # 继电器开启
def turn_off(self):
"""关闭继电器"""
print("继电器关闭")
GPIO.output(self.relay_pin, GPIO.LOW) # 继电器关闭
# 点亮LED灯1秒
GPIO.output(self.led_pin, GPIO.HIGH) # 点亮LED
time.sleep(1) # 等待1秒
GPIO.output(self.led_pin, GPIO.LOW) # 关闭LED
def run(self):
"""控制继电器的状态"""
try:
while True:
self.turn_on()
time.sleep(self.debounce_time) # 保持状态
self.turn_off()
time.sleep(self.debounce_time) # 保持状态
except KeyboardInterrupt:
print("程序终止，清理 GPIO 设置")
finally:
self.cleanup()
def cleanup(self):
"""清理 GPIO 设置"""
GPIO.cleanup()
def main1():
# 初始化舵机控制
arm_control = ArmControl()
relay_controller = RelayController(relay_pin=17,led_pin=12) # 假设继电器连接在 GPIO 17 引脚
try:
# 初始位置（0）
print("移动到初始位置")
arm_control.move_to_position('A1')
time.sleep(10) # 延时2秒，确保机械臂到达初始位置
arm_control.remove_to_position(0)
time.sleep(2)
arm_control.move_to_position('B1')
time.sleep(10) # 延时2秒，确保机械臂到达初始位置
arm_control.remove_to_position(0)
time.sleep(2)
arm_control.move_to_position('C1')
time.sleep(10) # 延时2秒，确保机械臂到达初始位置
arm_control.remove_to_position(0)
time.sleep(2)
arm_control.move_to_position('D1')
time.sleep(10) # 延时2秒，确保机械臂到达初始位置
arm_control.remove_to_position(0)
time.sleep(2)
arm_control.move_to_position('A2')
time.sleep(10) # 延时2秒，确保机械臂到达初始位置
arm_control.remove_to_position(0)
time.sleep(2)
arm_control.move_to_position('B2')
time.sleep(10) # 延时2秒，确保机械臂到达初始位置
arm_control.remove_to_position(0)
time.sleep(2)
arm_control.move_to_position('C2')
time.sleep(10) # 延时2秒，确保机械臂到达初始位置
arm_control.remove_to_position(0)
time.sleep(2)
arm_control.move_to_position('D2')
time.sleep(10) # 延时2秒，确保机械臂到达初始位置
arm_control.remove_to_position(0)
except KeyboardInterrupt:
print("操作中断")
finally:
# 清理GPIO设置
arm_control.cleanup()
relay_controller.cleanup()
if __name__ == "__main__":
main1()
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
