def __init__(self, pin_list, group=4, db='/opt/ezblock/config'):
self.pin_list = []
for i in range(0, len(pin_list), group):
_pin_list = pin_list[i:i + group]
for pin in _pin_list:
pwm = PWM(self.PINS[pin])
servo = Servo(pwm)
servo.angle(0)
self.pin_list.append(servo)
time.sleep(0.2)
self.pin_num = len(pin_list)
self.origin_positions = self.new_list(0)
self.db = fileDB(db=db)
temp = self.db.get('servo_offset_list',
default_value=str(self.new_list(0)))
temp = [float(i.strip()) for i in temp.strip("[]").split(",")]
self.offset = temp
self.servo_positions = self.new_list(0)
self.calibrate_position = self.new_list(0)
self.direction = self.new_list(1)
# import picar_4wd as fc
import sys
import tty
import termios
import asyncio
from ezblock import Servo,PWM,Pin
import threading
import time
import numpy as np
power_val = 50
key = 'status'
p1 = PWM('P12')
p2 = PWM('P13')
dir_Servo = Servo(PWM("P0"))
cam_Servo_x = Servo(PWM("P1"))
cam_Servo_y = Servo(PWM("P2"))
cam_Servo_x.angle(0)
cam_Servo_y.angle(0)
dir_Servo.angle(0)
cam_key_x = 0
cam_key_y = 0
car_dir_angle = 0
key = 'e'
left_rear_dir_pin = Pin("D4")
right_rear_dir_pin = Pin("D5")
def motion_control():
from ezblock import Servo, PWM, Pin, ADC
import numpy as np
from vilib import Vilib
import time
PERIOD = 4095
PRESCALER = 10
TIMEOUT = 0.02
dir_servo_pin = Servo(PWM('P2'))
camera_servo_pin1 = Servo(PWM('P0'))
camera_servo_pin2 = Servo(PWM('P1'))
left_rear_pwm_pin = PWM("P13")
right_rear_pwm_pin = PWM("P12")
left_rear_dir_pin = Pin("D4")
right_rear_dir_pin = Pin("D5")
S0 = ADC('A0')
S1 = ADC('A1')
S2 = ADC('A2')
Servo_dir_flag = 1
dir_cal_value = 0
cam_cal_value_1 = 0
cam_cal_value_2 = 0
motor_direction_pins = [left_rear_dir_pin, right_rear_dir_pin]
motor_speed_pins = [left_rear_pwm_pin, right_rear_pwm_pin]
cali_dir_value = [1, -1]
cali_speed_value = [0, 0]
#初始化PWM引脚
for pin in motor_speed_pins:
class Arm(Robot):
A = 80
B = 80
def __init__(self, pin_list):
super().__init__(pin_list, group=3)
self.speed = 50
def set_speed(self, speed):
self.speed = speed
def bucket_init(self, pin):
self.bucket = Servo(pin)
def hanging_clip_init(self, pin):
self.hanging_clip = Servo(pin)
def electromagnet_init(self, pin):
self.elecma = pin
def set_angle(self, angle):
self.servo_move(angle, self.speed)
def coord2polar(self, coord):
x, y, z = coord
y = max(0, y)
u = math.sqrt(math.pow(z, 2) + math.pow(y, 2) + math.pow(x, 2))
# print(u)
if u == 0:
u = 0.1
#u = min(160, u)
if u > 160: # 坐标超出范围，等比换算成最大球面位置
temp = 160 / u
x = temp * x
y = temp * y
z = temp * z
u = 160
if u < 30: # 坐标超出范围，等比换算成最大球面位置
temp = 30 / u
x = temp * x
y = temp * y
z = temp * z
u = 30
angle1 = math.acos((self.A**2 + u**2 - self.B**2) / (2 * self.A * u))
angle2 = math.asin(z / u)
angle3 = math.acos(
(self.A**2 + self.B**2 - u**2) / (2 * self.A * self.B))
angle4 = math.atan2(x, y)
alpha = 90 - (angle1 + angle2) / math.pi * 180
beta = -180 + (angle1 + angle2 + angle3) / math.pi * 180
gamma = -angle4 / math.pi * 180
# print(alpha, beta, gamma)
# self.set_angle([alpha, beta, gamma], speed)
return [alpha, beta, gamma]
def do_by_coord(self, coord):
temp = self.coord2polar(coord)
# self.set_speed(speed)
self.set_angle(temp)
def set_bucket(self, angle):
self.bucket.angle(angle)
def set_hanging_clip(self, angle):
self.hanging_clip.angle(angle)
def set_electromagnet(self, status):
if status == "on":
self.elecma.pulse_width_percent(100)
else:
self.elecma.pulse_width_percent(0)
def hanging_clip_init(self, pin):
self.hanging_clip = Servo(pin)
def bucket_init(self, pin):
self.bucket = Servo(pin)
from ezblock import Servo, PWM
dir_servo_pin = Servo(PWM('P2'))
camera_servo_pin1 = Servo(PWM('P0'))
camera_servo_pin2 = Servo(PWM('P1'))
dir_servo_pin.angle(0)
camera_servo_pin1.angle(0)
camera_servo_pin2.angle(0)