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!")