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 closeAll(self): #GPIO.cleanup() self.clk = clock.Clock(self.srvo, self.period) # stop clock self.clk.stop() # disable servos servo.disable()
def main():
global steering_servo, throttle_esc, running
# Starting MQTT mosquitto
client_steering = mqtt.Client("BB_steering_client")
client_steering.connect(BROKER, PORT)
client_steering.on_connect = on_connect
client_steering.on_log = on_log
client_steering.on_message = on_message_steering
client_steering.on_disconnect = on_disconnect_steering
client_steering.loop_start()
client_steering.subscribe(STEERING_TOPIC)
client_throttle = mqtt.Client("BB_throttle_client")
client_throttle.connect(BROKER, PORT)
client_throttle.on_connect = on_connect
client_throttle.on_log = on_log
client_throttle.on_message = on_message_throttle
client_steering.on_disconnect = on_disconnect_steering
client_throttle.loop_start()
client_throttle.subscribe(THROTTLE_TOPIC)
while running:
try:
time.sleep(0.01)
except KeyboardInterrupt:
client_steering.loop_stop()
client_steering.disconnect()
client_throttle.loop_stop()
client_throttle.disconnect()
servo.disable()
running = False
print("F*****g off...")
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 shutdown(self, quiet=False):
self.print_stdout("shutdown(quiet={})".format(quiet), quiet)
self.running = False
# stop clock
for i in range(0, 8):
self.clcks[i].stop()
# disable servos
servo.disable()
rcpy.set_state(rcpy.EXITING)
GPIO.cleanup()
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):
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 main():
global s_command, t_command, new_command
servo.enable()
steering_servo.start(CONFIG["PWM_PERIOD"])
esc8.start(CONFIG["PWM_PERIOD"])
while True:
try:
if new_command:
apply_steering_command(steering_servo, s_command)
apply_throttle_command(t_command)
new_command = False
time.sleep(0.01)
except KeyboardInterrupt:
client.loop_stop()
client.disconnect()
servo.disable()
print("car_client, F*****g off...")
break
def run(self, direction):
if self.duty != 0:
if not self.sweep:
print('Setting servo {} to {} duty'.format(self.channel, self.duty))
self.internal.set(self.duty)
else:
print('Sweeping servo {} to {} duty'.format(self.channel, self.duty))
else:
self.sweep = False
try:
# enable servos
servo.enable()
# start clock
self.clck.start()
# sweep
if self.sweep:
self.servo_sweep(direction)
else:
# keep running
while rcpy.get_state() != rcpy.EXITING:
time.sleep(1)
except KeyboardInterrupt:
# stop clock
self.clck.stop()
# disable servos
servo.disable()
finally:
# stop clock
self.clck.stop()
# disable servos
servo.disable()
# say bye
print("\nBye Beaglebone!")
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 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 servosOff():
try:
servo.disable()
except:
pass
def stop(self):
for i in self.clocks:
i.stop()
servo.disable()
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()
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
initial()
while (True):
servo.enable()
print("Receiving...\n")
rcpy.set_state(rcpy.RUNNING)
result = ws.recv()
print("\n" + result + "\n")
mm = json.loads(result)
print(mm['msg'])
print(mm['msg']['goal']['order'])
revnum = mm['msg']['goal']['order']
for i in range(3):
object = ['button', 'bottle', 'phone']
print(object[revnum])
jsname = object[revnum]
jsfile = str(jsname) + ".json"
print(jsfile)
time.sleep(1)
action()
time.sleep(2)
reset()
time.sleep(1)
servo.disable()
print('Over')
ws.close()
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!")
