def __init__(self):
self.i2c = busio.I2C(SCL, SDA)
self.pca = PCA9685(self.i2c)
self.pca.frequency = 50
self.br_motor = servo.Servo(self.pca.channels[0],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.fr_motor = servo.Servo(self.pca.channels[1],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.fl_motor = servo.Servo(self.pca.channels[2],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.bl_motor = servo.Servo(self.pca.channels[3],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.rc = Roboclaw("/dev/ttyS0", 115200)
i = self.rc.Open()
self.d1 = 7.254 #C/L to corner wheels
self.d2 = 10.5 #mid axle to fwd axle
self.d3 = 10.5 #mid axle to rear axle
self.d4 = 10.073 #C/L to mid wheels
self.fl_motor.angle = bias
self.fr_motor.angle = bias
self.bl_motor.angle = bias
self.br_motor.angle = bias
def __init__(self, hat, channel_x, channel_y, location, host, port):
# self.channel_x = channel_x
# self.channel_y = channel_y
self.host = host
self.port = port
self.location = "/dev/video" + str(location)
self.servo_x = servo.Servo(hat.channels[channel_x])
self.servo_y = servo.Servo(hat.channels[channel_y])
def __init__(self, container):
i2c = busio.I2C(SCL, SDA)
self.pca = PCA9685(i2c)
self.pca.frequency = 50
self.left = servo.Servo(self.pca.channels[1])
self.mid = servo.Servo(self.pca.channels[0],
min_pulse=500,
max_pulse=2000)
self.right = servo.Servo(self.pca.channels[2])
self.arduino = container.get('arduinoStepper')
def Servo_move_test(Direction):
servox = servo.Servo(pca.channels[0])
servoy = servo.Servo(pca.channels[1])
try:
Direction = eval(Direction)
#print (Direction[0])
#print (type(Direction[0]))
servox.angle = int(Direction[0]['x_angle'])
print(Direction[0]['x_angle'])
servoy.angle = int(Direction[0]['y_angle'])
print(Direction[0]['y_angle'])
except ValueError:
print("Angle out of range")
def __init__(self):
print("Initializing Servos")
self._i2c_bus0 = (busio.I2C(board.SCL_1, board.SDA_1))
print("Initializing ServoKit")
self._pca_1 = PCA9685(self._i2c_bus0, address=0x40)
self._pca_1.frequency = 60
self._pca_2 = PCA9685(self._i2c_bus0, address=0x41)
self._pca_2.frequency = 60
self._servos = list()
for i in range(0, 12):
if i < 6:
self._servos.append(
servo.Servo(self._pca_1.channels[i],
min_pulse=771,
max_pulse=2740))
else:
self._servos.append(
servo.Servo(self._pca_2.channels[i],
min_pulse=771,
max_pulse=2740))
# servos.append(servo.Servo(pca[int(i/6)].channels[int(i%6)], min_pulse=460, max_pulse=2440)) CL motor
# servos.append(servo.Servo(pca[int(i/6)].channels[int(i%6)], min_pulse=771, max_pulse=2740)) MG996R
print("Done initializing")
#1 by 12 array
self.MOTOR_LEG_FRONT = 0
self.MOTOR_LEG_BACK = 0
self.MOTOR_LEG_LEFT = 0
self.MOTOR_LEG_RIGHT = 0
self.MOTOR_LEG_SHOULDER = 0
self.MOTOR_LEG_UPPER = 0
self.MOTOR_LEG_LOWER = 0
#4 by 3 matrix
self.SIM_LEG_FRONT = 0
self.SIM_LEG_BACK = 6
self.SIM_LEG_LEFT = 0
self.SIM_LEG_RIGHT = 1
self.SIM_LEG_THETA1 = 0
self.SIM_LEG_THETA2 = 1
self.SIM_LEG_THETA3 = 2
# [0]~[2] : 왼쪽 앞 다리 // [3]~[5] : 오른쪽 앞 다리 // [6]~[8] : 왼쪽 뒷 다리 // [9]~[11] : 오른쪽 뒷 다리
# centered position perpendicular to the ground
self._servo_offsets = [180, 90, 90, 1, 90, 90, 180, 90, 90, 1, 90, 90]
self._val_list = np.zeros(12) #[ x for x in range(12) ]
# All Angles for Leg 3 * 4 = 12 length
self._thetas = []
def __init__(self, smokeyGpio, exit_condition):
self.__keyboardController = KeyboardController(exit_condition)
self.__smokeyGpio = smokeyGpio
self.__greenArm = SmokeyArm(self.__pca, self.__kit.motor3, 12,
servo.Servo(self.__pca.channels[2]),
servo.Servo(self.__pca.channels[5]), 30, 0,
False, 6)
self.__redArm = SmokeyArm(self.__pca, self.__kit.motor2, 22,
servo.Servo(self.__pca.channels[1]), None,
30, 0, False, 10)
self.__blueArm = SmokeyArm(self.__pca, self.__kit.motor1, 21,
servo.Servo(self.__pca.channels[0]),
servo.Servo(self.__pca.channels[4]), 30, 0,
True)
def __init__(self):
# init i2c
i2c = busio.I2C(SCL, SDA)
# init PCA
self.pca = PCA9685(i2c)
self.pca.frequency = 50
self.servo_steer = servo.Servo(self.pca.channels[0])
self.esc = servo.ContinuousServo(self.pca.channels[1])
# init model
model = neural_network.Net()
self.model = model.eval()
self.model.load_state_dict(torch.load('model/autopilot.pt'))
self.device = torch.device('cuda')
self.model.to(self.device)
# init vars
self.temp = 0
mean = 255.0 * np.array([0.485, 0.456, 0.406])
stdev = 255.0 * np.array([0.229, 0.224, 0.225])
self.normalize = torchvision.transforms.Normalize(mean, stdev)
self.angle_out = 0
# init Camera
self.cam = camera.Camera()
# initial content
with open('control_data.csv', 'w') as f:
f.write('date,steering,speed\n')
def set_servo(x):
"""
:param i: fuction to define a servo with the parameter of board pin
:return: a servo
"""
pwm = pulseio.PWMOut(x, duty_cycle=2**15, frequency=50)
return servo.Servo(pwm, min_pulse=620, max_pulse=2320)
def send(self, value):
# Control as a Servo
print("Controlling servo [" + format(hex(self.cardAddr)) + ":" +
format(self.channelNr) + "] with angle = " + str(value))
myCardAddr = hex(self.cardAddr)
if (boards.get(myCardAddr)):
#print("Board found ++++++++++++++")
#specific pulse range ---
try:
# initiate servo variable
theServo = servo.Servo(
boards[myCardAddr].channels[self.channelNr],
min_pulse=50,
max_pulse=2500)
newAngle = (value / 270.0) * 180
stepAngle = 0.25
cmdAngle = theServo.angle
# Check out of range values
if (cmdAngle < 0):
cmdAngle = 0
if (cmdAngle > 180):
cmdAngle = 180
#print("floatValue= {0}".format(value), " Angle desired= {0}".format(newAngle), " Actual angle= {0}".format(cmdAngle))
if newAngle > cmdAngle:
# Increase
while cmdAngle < newAngle:
#print("passing command of angle {0}".format(cmdAngle), " Servo Angle={0}".format(theServo.angle), " Step is {0}".format(stepAngle))
theServo.angle = cmdAngle
cmdAngle += stepAngle
elif newAngle < cmdAngle:
# Decrease
while cmdAngle > newAngle:
#print("passing command of angle {0}".format(cmdAngle), " Servo Angle={0}".format(theServo.angle), " Step is {0}".format(stepAngle))
theServo.angle = cmdAngle
cmdAngle -= stepAngle
else:
theServo.angle = newAngle
print("no command of angle {0}".format(cmdAngle),
" Servo Angle={0}".format(theServo.angle),
" Step is {0}".format(stepAngle))
theServo.angle = newAngle
except Exception as e:
print(format(e))
except:
pass
else:
print("No board matching --------")
def __init__(self):
self.steering_angle_max = 30.0
self.servo_center = 90.0
self.servoPIN = 0
self.motorPIN = 1
self.motorMax = 0.1
print ( " Initializing motors " )
# Startup the i2c interface
i2c = busio.I2C(SCL, SDA)
# Create a simple PCA9685 class instance
self.pca = PCA9685(i2c)
self.pca.frequency = 50
# Start the PWM for the motor at 0
self.motor = self.pca.channels[self.motorPIN]
self.motor.duty_cycle = 0
# Setup the steering servo and set to center
self.steering = servo.Servo(self.pca.channels[self.servoPIN])
self.steering.angle = self.servo_center
print ( " Motors initialized " )
def __init__(self, servo_id: int, debug_log: bool = False):
self._servo_id = servo_id
self._angle = 0
self._debug_log = debug_log
self._driver = servo.Servo(Servo.pca.channels[self._servo_id],
min_pulse=Servo.min_pulse,
max_pulse=Servo.max_pulse)
def __init__(self):
i2c_bus = busio.I2C(SCL, SDA)
self.pca = PCA9685(i2c_bus)
self.pca.frequency = 150
self.maxPulse = 2500
self.minPulse = 600
self.angleInit = [5, 90, 50, 90]
self.angleCurrent = [0, 0, 0, 0]
self.angleMin = [5, 0, 50, 60]
self.angleMax = [65, 180, 150, 110]
self.angleStep = [5, 5, 5, 4]
self.servo0 = servo.Servo(self.pca.channels[0], min_pulse=600, max_pulse=2500)
self.servo1 = servo.Servo(self.pca.channels[2], min_pulse=600, max_pulse=2500)
self.servo2 = servo.Servo(self.pca.channels[3], min_pulse=600, max_pulse=2500)
self.servo3 = servo.Servo(self.pca.channels[4], min_pulse=600, max_pulse=2500)
self.servos =[self.servo0, self.servo1, self.servo2, self.servo3]
def extGate():
ext_srv = servo.Servo(pca.channels[3])
for i in range(90):
ext_srv.angle = i
while (GPIO.input(ext_ir) != 1):
time.sleep(0.5)
time.sleep(2.0)
for i in range(90):
ext_srv.angle = 90 - i
def setUpClass(cls):
i2c = busio.I2C(SCL, SDA)
# Create a simple PCA9685 class instance.
cls.pca = PCA9685(i2c)
cls.pca.frequency = 50
cls.servo = servo.Servo(cls.pca.channels[0],
min_pulse=500,
max_pulse=2500)
def __init__(self, servo_locs, frequency=50):
if not simpleMode:
self.pca = PCA9685(i2c)
self.pca.frequency = frequency
self.servos = [None] * 16
for i, (loc, s_type) in enumerate(servo_locs):
self.servos.append((servo.Servo(
pca.channels[loc],
settings["servo_settings"][s_type]["min_pulse"],
settings["servo_settings"][s_type]["max_pulse"]), s_type))
def entGate():
ent_srv = servo.Servo(pca.channels[7])
for i in range(90):
ent_srv.angle = i
#gate is open at this point, updating database:
db.child("GateStatus").set(noEntry)
while (GPIO.input(ent_ir) != 1):
time.sleep(0.5)
time.sleep(2.0)
for i in range(90):
ent_srv.angle = 90 - i
def __init__(self,
channel,
initial_angle=180,
min_pulse=1000,
max_pulse=2000):
self.servo = servo.Servo(pca.channels[channel],
min_pulse=min_pulse,
max_pulse=max_pulse)
self.min_pulse = min_pulse
self.max_pulse = max_pulse
self.angle = initial_angle
def __init__(self):
self.i2c_bus = busio.I2C(SCL, SDA)
self.pca = PCA9685(self.i2c_bus)
self.pca.frequency = 60
self.steering = servo.Servo(self.pca.channels[0])
self.streering_angle = 90
rospy.init_node('rccar_interface', anonymous=True)
rospy.Subscriber('move_cmd', MoveCommand, self.message_callback)
rospy.spin()
def __init__(self, container):
self.serial = serial.Serial("/dev/ttyAMA0", 115200)
self.websocket = container.get('websocket')
self.positionWatcher = container.get('positionWatcher')
self.logger = self.container.get('logger').get('Lidar')
i2c = busio.I2C(SCL, SDA)
self.pca = PCA9685(i2c)
self.pca.frequency = 50
self.servo = servo.Servo(self.pca.channels[8])
self.servo.set_pulse_width_range(400, 2600)
def Servo_move(Direction):
servox = servo.Servo(pca.channels[0])
servoy = servo.Servo(pca.channels[1])
try:
if Direction == "L":
# 90度置中點
for i in range(0, 90):
servox.angle = 90 - i
if Direction == "R":
for i in range(90, 180):
servox.angle = i
if Direction == "D":
for i in range(0, 90):
servoy.angle = 90 - i
if Direction == "U":
for i in range(90, 180):
servoy.angle = i
except ValueError:
print("Angle out of range")
finally:
pca.deinit()
def __init__(self):
global GPIO, PWM
from periphery import GPIO, PWM, GPIOError
from adafruit_motor import servo
import pulseio
# create a PWMOut object on Pin PWM3.
pwm1 = pulseio.PWMOut(board.PWM1, duty_cycle=2 ** 15, frequency=50)
pwm2 = pulseio.PWMOut(board.PWM2, duty_cycle=2 ** 15, frequency=50)
pwm3 = pulseio.PWMOut(board.PWM3, duty_cycle=2 ** 15, frequency=50)
# Create a servo object, my_servo.
self.my_servo1 = servo.Servo(pwm1)
self.my_servo2 = servo.Servo(pwm2)
self.my_servo3 = servo.Servo(pwm3)
def initPWM(pin):
pwm = PWM(pin, 0)
pwm.frequency = 1e3
pwm.duty_cycle = 0
pwm.enable()
return pwm
try:
self._LEDs = [GPIO(86, "out"),
GPIO(77,"out"),
initPWM(0),
GPIO(140, "out"),
GPIO(73,"out"),
]
self._buttons = [GPIO(141, "in"),
GPIO(8, "in"),
GPIO(7, "in"),
GPIO(138, "in"),
GPIO(6, "in"),
]
except GPIOError as e:
print("Unable to access GPIO pins. Did you run with sudo ?")
sys.exit(1)
super(UI_EdgeTpuDevBoard, self).__init__()
def Servo_move_Y(Id, Direction):
servo = servo.Servo(pca.channels[Id])
try:
if Direction == "D":
# 90度置中點
for i in range(0, 90):
servo.angle = 90 - i
if Direction == "U":
for i in range(90, 180):
servo.angle = i
except ValueError:
print("Angle out of range")
finally:
pca.deinit()
def __init__(self):
# init i2c bus
self.i2c = busio.I2C(SCL, SDA)
# init PWM controller
self.pwm = PCA9685(self.i2c)
self.pwm.frequency = 50
# init drive motors on pins listed in constants.py
self.frontLeft = self.pwm.PWMChannel(self.pwm, LF_PORT)
self.centerLeft = self.pwm.PWMChannel(self.pwm, LC_PORT)
self.rearLeft = self.pwm.PWMChannel(self.pwm, LR_PORT)
self.frontRight = self.pwm.PWMChannel(self.pwm, RF_PORT)
self.centerRight = self.pwm.PWMChannel(self.pwm, RC_PORT)
self.rearRight = self.pwm.PWMChannel(self.pwm, RC_PORT)
# init cornersteer servos on pins listed in constants.py
# TODO: Verify and set servo pulse limits
self.servoFrontLeft = servo.Servo(self.pwm.channels[CS_LF_PORT])
self.servoRearLeft = servo.Servo(self.pwm.channels[CS_LR_PORT])
self.servoFrontRight = servo.Servo(self.pwm.channels[CS_RF_PORT])
self.servoRearRight = servo.Servo(self.pwm.channels[CS_RR_PORT])
def __init__(
self,
i2c,
num_motor=8,
motor_enable=(1, 1, 1, 1, 1, 1, 1, 1),
motor_min_angle=(0, 0, 50, 0, 0, 0, 0, 0),
motor_max_angle=(150, 150, 140, 60, 60, 0, 0, 0),
motor_default_angle=(100, 100, 90, 30, 30, 0, 0, 0),
):
# 0. arguments
self.i2c = i2c
self.num_motor = num_motor
self.motor_enable = list(motor_enable)
self.min_angle_limit = list(motor_min_angle)
self.max_angle_limit = list(motor_max_angle)
self.motor_default_angle = list(motor_default_angle)
self.motor_direction = [-1, 1, 1, 1, -1, 0, 0, 0]
# 1. Static Variables
self.pwm_min = 580 # min pwm, sg-90 : -90,
self.pwm_max = 2480 # max pwm, sg-90 : +90,
self.min_trj_time = 0.3
self.max_trj_time = 5
self.control_time = 0.02
# 2. variables
self.servos = []
self.cur_angles = self.motor_default_angle
self.force_stop_flag = False
self.sleep_mode_register = None
self.operation_mode_register = 33
# 3. Objects
# 4. Init Functions
self.pca = PCA9685(self.i2c)
self.sleep_mode_register = (self.operation_mode_register & 0x7F) | 0x10
self.pca.frequency = 50
self.servos = []
for i in range(8):
self.servos.append(
servo.Servo(
self.pca.channels[i],
min_pulse=self.pwm_min,
max_pulse=self.pwm_max,
))
self.sleep()
def servo_init():
i2c = busio.I2C(SCL, SDA)
pca = PCA9685(i2c)
pca.frequency = 50
a = 800 #Pulso minimo
b = 2700 #Pulso maximo
pca_channels = [0, 8, 2, 3, 4, 5, 15]
#home_degree = [140,30,140,50,150,60]
global servos
servos = []
for i in range(7):
servos.append(
servo.Servo(pca.channels[pca_channels[i]],
min_pulse=a,
max_pulse=b))
def __init__(self, name, num, speed, min, max, initial_position):
dev = '/dev/serial0'
self.servo = servo.Servo(pca.channels[num], min_pulse=min, max_pulse=max)
self.debug = False
self.speed = speed
self.name = name
self.num = num
self.min = min
self.max = max
print("Servo %s setup with min: %s and max: %s" % (num, min, max))
self.target_position = None
self.current_position = None
self.last_time = None
all_motors.update({name: self})
self.setToPosition(initial_position, fastest=True)
def on_message(client, userdata, message):
channel = int(message.topic.split("/")[2])
name = str(message.topic.split("/")[3])
value = int(message.payload.decode("utf-8"))
index = int(channel / 16)
channel = channel % 16
print(f"Device:{index} Channel:{channel} {name}:{value}")
if (name == 'duty_cycle'):
c = devices[index].channels[channel]
# c = pca.channels[channel]
c.duty_cycle = value
elif (name == 'angle'):
s = servo.Servo(devices[index].channels[channel],
min_pulse=600,
max_pulse=2600)
s.angle = value
def __init__(self, addr, N_servos):
# I don't want to deal with passing hex shit, so just pass addr
# as an int.
self.addr = addr
self.N_servos = N_servos
assert addr in [40, 41], 'Invalid I2C addr! : {}'.format(addr)
assert (N_servos >= 1) and (N_servos <= 16), (
'N_servos out of bds in DriverBoard.__init__!' +
'Val is {}, must be >=1, <={}'.format(N_servos, N_servos))
'''if addr == 40:
self.dev = Device(0x40)
if addr == 41:
self.dev = Device(0x41)
self.dev.set_pwm_frequency(50)'''
i2c = busio.I2C(SCL, SDA)
# Create a simple PCA9685 class instance.
self.dev = PCA9685(i2c, address=65)
self.dev.frequency = 50
self.servos = [servo.Servo(self.dev.channels[i]) for i in range(16)]
def __init__(self, container):
i2c = busio.I2C(SCL, SDA)
self.pca = PCA9685(i2c)
self.pca.frequency = 50
self.servo = servo.Servo(self.pca.channels[8])
import time
import board
import pulseio
import touchio
import neopixel
from adafruit_motor import servo
touch_1 = board.A0
#sets A0 to touch one
touch_2 = board.A1
#sets A1 to touch 2
pwm = pulseio.PWMOut(board.A2, duty_cycle=2**15, frequency=50)
#sets the frequency and sets up duty cicle to go out of A2 with pulsie io and PWM
dot = neopixel.NeoPixel(board.NEOPIXEL, 1)
my_servo = servo.Servo(pwm)
touch1 = touchio.TouchIn(touch_1)
touch2 = touchio.TouchIn(touch_2)
angle = 0
while True:
if touch1.value:
print("wow!")
#for angle in range(0, 180, 5):
if angle < 180:
angle += 10
my_servo.angle = angle
time.sleep(0.05)
if touch2.value:
print("spicy!")
if angle > 0:
angle -= 10
