def scissors():
servo.enable()
print('scissors')
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(1.5)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(-1.5)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(1.5)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(-1.5)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.01)
clck.start()
srvo5.set(0.2)
print('scissors')
time.sleep(0.5)
servo.disable()
return
def first():
print('first')
servo.enable()
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(-1.5)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(-1.5)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(-0.8)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(-0.8)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.02)
clck.start()
srvo5.set(0)
time.sleep(3)
print('second')
srvo1.set(-1.5)
srvo2.set(1.2)
srvo3.set(1.5)
srvo4.set(-0.5)
srvo5.set(-0.5)
time.sleep(3);
print('third')
srvo1.set(0.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(-0.3)
srvo5.set(-0.8)
time.sleep(3)
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(1.5)
srvo5.set(1)
time.sleep(1000)
servo.disable()
return
def __init__(self):
# Hips are servos 1 - 4 clockwise from the front right
# Ankles are servos 5 -8 clockwise from the front right
rcpy.set_state(rcpy.RUNNING)
self.servos = [servo.Servo(srv) for srv in range(1, 9)]
self.clocks = [clock.Clock(srv, .02) for srv in self.servos]
self.reset()
def test1():
class MyAction(clock.Action):
def __init__(self):
self.count = 0
def run(self):
self.count += 1
action = MyAction()
obj = clock.Clock(action)
obj.start()
assert action.count == 0
time.sleep(1.1*obj.period)
assert action.count == 1
time.sleep(1.1*obj.period)
assert action.count == 2
obj.toggle()
time.sleep(1.1*obj.period)
assert action.count == 2
obj.toggle()
time.sleep(1.1*obj.period)
assert action.count == 3
obj.stop()
def closeAll(self): #GPIO.cleanup() self.clk = clock.Clock(self.srvo, self.period) # stop clock self.clk.stop() # disable servos servo.disable()
def main(duty):
# # exit if no options
# if len(sys.argv) < 2:
# usage()
# sys.exit(2)
# # Parse command line
# try:
# opts, args = getopt.getopt(sys.argv[1:], "hst:d:c:", ["help"])
# except getopt.GetoptError as err:
# # print help information and exit:
# print('rcpy_test_servos: illegal option {}'.format(sys.argv[1:]))
# usage()
# sys.exit(2)
# defaults
# duty = 1.5
period = 0.02
channel = 0
sweep = False
brk = False
free = False
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(channel)
srvo.set(duty)
clck = clock.Clock(srvo, period)
servo.enable()
clck.start()
def rock():
for i in range(0, 4):
servo.enable()
print(data['poses'][i]['position']['x'])
srvo[i] = servo.Servo(data['poses'][i]['position']['x'])
clck = clock.Clock(srvo[i], data['poses'][i]['position']['y'])
print(srvo)
clck.start()
srvo[i].set(data['poses'][i]['position']['z'])
time.sleep(0.5)
return
def handshake():
servo.enable()
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(-1.2)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(-1.2)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(1.2)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(1.2)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.01)
clck.start()
srvo5.set(1.2)
time.sleep(3)
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(1.5)
srvo5.set(1.4)
time.sleep(0.5)
servo.disable()
return
def close():
for i in range(0, 6):
servo.enable()
print(i, data['poses'][i]['position']['x'])
srvo[i] = servo.Servo(data['poses'][i]['position']['x'])
clck = clock.Clock(srvo[i], data['poses'][i]['position']['y'])
clck.start()
srvo[i].set(data['poses'][i]['position']['z'])
srvo[4].set(data['poses'][10]['position']['z'])
time.sleep(0.5)
print('close')
return
def sweep(self, angle):
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(self.channel)
duty = self.angle_to_duty(angle)
clck = clock.Clock(srvo, self.period)
try:
servo.enable()
clck.start()
d = 0
direction = 1
duty = math.fabs(duty)
delta = duty / 100
while rcpy.get_state() != rcpy.EXITING:
# increment duty
d = d + direction * delta
# end of range?
if d > duty or d < -duty:
direction = direction * -1
if d > duty:
d = duty
else:
d = -duty
srvo.set(d)
self.duty = d
msg = servolcm()
msg.duty = self.duty
msg.angle = (self.duty)
msg.timestamp = datetime.strftime(datetime.now(),
datetime_format)
self.lc.publish("servo_data", msg.encode())
time.sleep(.02)
# handle ctrl c exit
except KeyboardInterrupt:
pass
finally:
clck.stop()
servo.disable()
def __init__(self):
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# front esc
self.front_right = servo.ESC(esc_const.EscPwmPins.FRONT_RIGHT)
self.front_left = servo.ESC(esc_const.EscPwmPins.FRONT_LEFT)
# back esc
self.back_right = servo.ESC(esc_const.EscPwmPins.BACK_RIGHT)
self.back_left = servo.ESC(esc_const.EscPwmPins.BACK_LEFT)
self.escs = [
self.front_right, self.back_left, self.front_left, self.back_right
]
self.logger = logging.getLogger('MotorController')
self.logger.debug('__init__')
# create clock to do periodic updates
self.fr_clock = clock.Clock(self.front_right, self.UPDATE_PERIOD)
self.fl_clock = clock.Clock(self.front_left, self.UPDATE_PERIOD)
self.br_clock = clock.Clock(self.back_right, self.UPDATE_PERIOD)
self.bl_clock = clock.Clock(self.back_left, self.UPDATE_PERIOD)
# set all to zero
self.front_right.set(0)
self.front_left.set(0)
self.back_right.set(0)
self.back_left.set(0)
# start all the clocks
self.fr_clock.start()
self.fl_clock.start()
self.br_clock.start()
self.bl_clock.start()
def initial():
with open('open.json', 'r') as f:
data = json.load(f)
#print(data)
datalen = len(data['poses']) #6
servo.enable()
for i in range(0, datalen):
#print(data['poses'][i]['position']['x'],data['poses'][i]['position']['z'])
srvo[i] = servo.Servo(data['poses'][i]['position']['x'])
clck = clock.Clock(srvo[i], data['poses'][i]['position']['y'])
clck.start()
srvo[i].set(data['poses'][i]['position']['z'])
#print(i)
time.sleep(1)
return
def __init__(self):
servo.disable() # turn off 6v servo power rail
self.throttle = [None, None, None, None]
self.throttle_max = [0.0, 0.0, 0.0, 0.0]
self.throttle_min = [999.9, 999.9, 999.9, 999.9]
self.escs = [
servo.ESC(ESC_FORE + 1),
servo.ESC(ESC_STARBOARD + 1),
servo.ESC(ESC_AFT + 1),
servo.ESC(ESC_PORT + 1)
]
self.clks = [
clock.Clock(esc, PULSE_FREQUENCY_SECS) for esc in self.escs
]
self.armed = False
def __init__(self):
super().__init__()
self.running = False
self.reload()
period = self.period
self.bbb_servos = [
servo.Servo(1),
servo.Servo(2),
servo.Servo(3),
servo.Servo(4),
servo.Servo(5),
servo.Servo(6),
servo.Servo(7),
servo.Servo(8)
]
self.clcks = [
clock.Clock(self.bbb_servos[0], period),
clock.Clock(self.bbb_servos[1], period),
clock.Clock(self.bbb_servos[2], period),
clock.Clock(self.bbb_servos[3], period),
clock.Clock(self.bbb_servos[4], period),
clock.Clock(self.bbb_servos[5], period),
clock.Clock(self.bbb_servos[6], period),
clock.Clock(self.bbb_servos[7], period)
]
self.motors = [
motor.Motor(1),
motor.Motor(2),
motor.Motor(3),
motor.Motor(4)
]
# Boot
GPIO.cleanup()
rcpy.set_state(rcpy.RUNNING)
# disable servos
servo.enable()
# start clock
for i in range(0, 8):
self.clcks[i].start()
self.tts_engine = pyttsx3.init()
self.tts_engine.setProperty('rate', 150)
self.running = True
def __init__(self, rotations, frequency, channel, sweep=False, brk=False, free=False, update_interval=100):
self.duty = rotations*3/8 - 1.5
self.period = 1/frequency
self.channel = channel
self.sweep = sweep
self.brk = brk
self.free = free
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# set servo duty (only one option at a time)
self.internal = servo.Servo(self.channel)
self.clck = clock.Clock(self.internal, self.period)
self.delta = self.duty/100
self.timer = threading.Timer(update_interval)
def set_angle(self, angle):
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(self.channel)
duty = self.angle_to_duty(angle)
print(duty)
srvo.set(duty)
clck = clock.Clock(srvo, self.period)
servo.enable()
clck.start()
clck.stop()
servo.disable()
self.duty = duty
msg = servolcm()
msg.duty = self.duty
msg.timestamp = datetime.strftime(datetime.now(), datetime_format)
self.lc.publish("servo_data", msg.encode())
def setup(self):
# # Set the two LEDs as outputs:
# GPIO.setup(self.inputpin, GPIO.OUT)
# GPIO.setup(self.inputpin, GPIO.OUT)
#
# # Start one high and one low:
# GPIO.output(self.inputpin, GPIO.HIGH)
#GPIO.output("SERVO_PWR", GPIO.HIGH)
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
self.rcpy_state = rcpy.RUNNING
# set servo duty (only one option at a time)
#Enable Servos
servo.enable()
self.srvo = servo.Servo(self.num)
if self.duty != 0:
print('Setting servo {} to {} duty'.format(self.num, self.duty))
self.srvo.set(self.duty)
self.currentState = True
self.clk = clock.Clock(self.srvo, self.period)
# start clock
self.clk.start()
import time, math
import getopt, sys
import rcpy # This automatically initizalizes the robotics cape
import rcpy.servo as servo
import rcpy.clock as clock # For PWM period for servos
# INITIALIZE DEFAULT VARS
duty = 1.5 # Duty cycle (-1.5,1.5 is the typical range)
period = 0.02 # recommended servo period: 20ms (this is the interval of commands)
ch1 = 1 # select channel (1 thru 8 are available)
ch2 = 2 # selection of 0 performs output on all channels
rcpy.set_state(rcpy.RUNNING) # set state to rcpy.RUNNING
srvo1 = servo.Servo(ch1) # Create servo object
srvo2 = servo.Servo(ch2)
clck1 = clock.Clock(srvo1, period) # Set PWM period for servos
clck2 = clock.Clock(srvo2, period)
servo.enable() # Enables 6v rail on beaglebone blue
clck1.start() # Starts PWM
clck2.start()
def move1(angle):
srvo1.set(angle)
def move2(angle):
srvo2.set(angle)
def count():
print('one')
servo.enable()
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(1)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(1)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(1.5)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(-1)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.018)
clck.start()
srvo5.set(0.7)
time.sleep(2)
print('two')
srvo1.set(1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(-1.5)
srvo5.set(0.7)
time.sleep(2)
print('three')
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(-1.5)
srvo4.set(-1.5)
srvo5.set(0.7)
time.sleep(2)
print('four')
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(-1.5)
srvo5.set(0.7)
time.sleep(2)
print('five')
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(1.5)
srvo5.set(1)
time.sleep(1000)
servo.disable()
return
sample_rate = 100
imu = mpu9250.IMU(enable_dmp=True, dmp_sample_rate=sample_rate)
# Set RCPY State to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
base_srvo = servo.Servo(base_channel)
shoulder_srvo = servo.Servo(shoulder_channel)
elbow_srvo = servo.Servo(elbow_channel)
pitch_srvo = servo.Servo(pitch_channel)
roll_srvo = servo.Servo(roll_channel)
grabber_srvo = servo.Servo(grabber_channel)
base_clck = clock.Clock(base_srvo, period)
shoulder_clck = clock.Clock(shoulder_srvo, period)
elbow_clck = clock.Clock(elbow_srvo, period)
pitch_clck = clock.Clock(pitch_srvo, period)
roll_clck = clock.Clock(roll_srvo, period)
grabber_clck = clock.Clock(grabber_srvo, period)
def dock_arm():
base_srvo.set(base_dock)
shoulder_srvo.set(shoulder_dock)
elbow_srvo.set(elbow_dock)
pitch_srvo.set(pitch_dock)
roll_srvo.set(roll_dock)
grabber_srvo.set(grabber_dock)
def start(self, period):
thread = clock.Clock(self, period)
thread.start()
return thread
# This automatically initizalizes the robotics cape
import rcpy
import rcpy.servo as servo
import rcpy.clock as clock # For PWM period for servos
# defaults
duty = 1.5 # Duty cycle (-1.5,1.5)
period = 0.02 # Set servo period to 20ms
channel = 0 # Which channel (1-8), 0 outputs to all channels
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(channel) # Create servo object
clck = clock.Clock(srvo, period) # Set PWM period for servos
try:
# enable servos
servo.enable() # Enables 6v rail
# start clock
clck.start() # Starts PWM
# keep running
while rcpy.get_state() != rcpy.EXITING:
srvo.set(duty) # Set servo duty
except KeyboardInterrupt:
def main():
# exit if no options
if len(sys.argv) < 2:
usage()
sys.exit(2)
# Parse command line
try:
opts, args = getopt.getopt(sys.argv[1:], "hst:d:c:", ["help"])
except getopt.GetoptError as err:
# print help information and exit:
print('rcpy_test_servos: illegal option {}'.format(sys.argv[1:]))
usage()
sys.exit(2)
# defaults
duty = 1.5
period = 0.02
channel = 0
sweep = False
brk = False
free = False
for o, a in opts:
if o in ("-h", "--help"):
usage()
sys.exit()
elif o in "-d":
duty = float(a)
elif o in "-t":
period = float(a)
elif o in "-c":
channel = int(a)
elif o == "-s":
sweep = True
else:
assert False, "Unhandled option"
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# set servo duty (only one option at a time)
srvo = servo.Servo(channel)
if duty != 0:
if not sweep:
print('Setting servo {} to {} duty'.format(channel, duty))
srvo.set(duty)
else:
print('Sweeping servo {} to {} duty'.format(channel, duty))
else:
sweep = False
# message
print("Press Ctrl-C to exit")
clck = clock.Clock(srvo, period)
try:
# enable servos
servo.enable()
# start clock
clck.start()
# sweep
if sweep:
d = 0
direction = 1
duty = math.fabs(duty)
delta = duty / 100
# keep running
while rcpy.get_state() != rcpy.EXITING:
# increment duty
d = d + direction * delta
# end of range?
if d > duty or d < -duty:
direction = direction * -1
if d > duty:
d = duty
else:
d = -duty
srvo.set(d)
# sleep some
time.sleep(.02)
# or do nothing
else:
# keep running
while rcpy.get_state() != rcpy.EXITING:
# sleep some
time.sleep(1)
except KeyboardInterrupt:
# handle what to do when Ctrl-C was pressed
pass
finally:
# stop clock
clck.stop()
# disable servos
servo.disable()
# say bye
print("\nBye Beaglebone!")
def mySet(serv, duty, lastDuty):
timeToWait = 0.2 * abs(duty - lastDuty)
serv.set(duty)
serv.run()
#time.sleep(timeToWait)
COMPASS_FIX = -90
COMPASS_ERR = 5
imu = mpu9250.IMU(enable_dmp=True, dmp_sample_rate=4, enable_magnetometer=True)
serv = servo.Servo(1)
servo.enable()
clck = clock.Clock(serv, 0.01)
clck.start()
duty = 0
lastDuty = duty
mySet(serv, duty, duty)
dataArr = []
prev_heading = 0
change = 0
servo_move = 0.2
servo_min = -1.5
servo_max = 1.5
while True:
data = imu.read()
def setDuty(duty):
srvo.set(duty)
servo.enable()
clck = clock.Clock(srvo, period)
clck.start()
M1 = 3 # Stepper Coil 1 Motor Output
M2 = 4 # Stepper Coil 2 Motor Output
position = 0
direction = 1
print("initializing rcpy...")
rcpy.set_state(rcpy.RUNNING)
print("finished initializing rcpy.")
# set servo servo_duty (only one option at a time)
srvo = servo.Servo(servo_channel)
srvo.set(servo_duty)
clck = clock.Clock(srvo, period)
GPIO.setup(crane_bottom_switch, GPIO.IN)
# enable servos
servo.enable()
# start clock
clck.start()
# while GPIO.input(crane_bottom_switch) == 0:
#
# step(1)
#
# while GPIO.input(crane_bottom_switch) == 1:
#
period = 0.02 # recommended servo period: 20ms (this is the interval of commands) ch1 = 1 # select channel (1 thru 8 are available) ch2 = 2 # selection of 0 performs output on all channels ch3 = 3 ch4 = 4 ch5 = 5 ch6 = 6 rcpy.set_state(rcpy.RUNNING) # set state to rcpy.RUNNING srvo1 = servo.Servo(ch1) # Create servo object srvo2 = servo.Servo(ch2) srvo3 = servo.Servo(ch3) srvo4 = servo.Servo(ch4) srvo5 = servo.Servo(ch5) srvo6 = servo.Servo(ch6) clck1 = clock.Clock(srvo1, period) # Set PWM period for servos clck2 = clock.Clock(srvo2, period) clck3 = clock.Clock(srvo3, period) clck4 = clock.Clock(srvo4, period) clck5 = clock.Clock(srvo5, period) clck6 = clock.Clock(srvo6, period) servo.enable() # Enables 6v rail on beaglebone blue clck1.start() # Starts PWM clck2.start() clck3.start() clck4.start() clck5.start() clck6.start() def move1(angle):
def bye():
servo.enable()
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(-1.5)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(-1.5)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(1.5)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(1.5)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.02)
clck.start()
srvo5.set(1.4)
time.sleep(0.5)
srvo1.set(0.5)
srvo2.set(0.5)
srvo3.set(-0.5)
srvo4.set(-0.5)
srvo5.set(1.4)
time.sleep(0.5)
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(-0.5)
srvo5.set(1.4)
time.sleep(0.5)
srvo1.set(0.5)
srvo2.set(0.5)
srvo3.set(-0.5)
srvo4.set(-0.5)
srvo5.set(1.4)
time.sleep(0.5)
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(1.5)
srvo5.set(1.4)
time.sleep(.5)
servo.disable()
# This automatically initizalizes the robotics cape
import rcpy
import rcpy.mpu9250 as mpu9250
import rcpy.servo as servo
import rcpy.clock as clock
print("1")
rcpy.set_state(rcpy.RUNNING)
mpu9250.initialize(enable_dmp=True,
dmp_sample_rate=100,
enable_fusion=True,
enable_magnetometer=True)
srvo = servo.Servo(1)
clck = clock.Clock(srvo, 0.02)
servo.enable()
clck.start()
srvo.set(-1.0)
theta_max = math.pi / 2.5
theta_min = -math.pi / 2.5
theta_dot_max = 5
theta_dot_min = -5
throttle_max = 20
throttle_min = 0
theta_tuple = (theta_min, theta_max)
theta_dot_tuple = (theta_dot_min, theta_dot_max)
throttle_tuple = (throttle_min, throttle_max)
ctrl = c.QTblController(theta_tuple, theta_dot_tuple, throttle_tuple)
ctrl.load_table()
