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 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 init_steering(init_data): global steering_servo # Setup steering servo rcpy.set_state(rcpy.RUNNING) servo.enable() steering_servo = servo.Servo(init_data["channel"]) steering_servo.start(init_data["pwm_period"]) print("Steering initialized. YOU LOVELY PERSON!")
def init_throttle(init_data): global throttle_esc # Setup throttle esc. 0.5 is the 'zero throttle' # command used for arming the esc throttle_esc = servo.Servo(init_data["channel"]) throttle_esc.set(init["pwm_zero"]) throttle_esc.start(init["pwm_period"]) print("Throttle initialized. NICE ONE COBBA!")
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): 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 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, 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 external libraries 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
# PROGRAM REQUIRES SUDO. IN PROGRESS AS OF 2019.08.09 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 # defaults duty = 1.5 # Duty cycle (-1.5,1.5) period = 0.02 # Set servo period to 20ms channel = 1 # Which channel (1-8), 0 outputs to all channels channel2 = 2 rcpy.set_state(rcpy.RUNNING) # set state to rcpy.RUNNING srvo = servo.Servo(channel) # Create servo object srvo2 = servo.Servo(channel2) clck = clock.Clock(srvo, period) # Set PWM period for servos clck2 = clock.Clock(srvo2, period) #try: servo.enable() # Enables 6v rail clck.start() # Starts PWM clck2.start() def move(angle): srvo.set(angle) def move2(angle):
motor_x = 3 motor_y = 4 # Initial Motor Duty Cycles duty_x = 0 duty_y = 0 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():
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!")
import math 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
time.sleep(0.005) # 10ms delay 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: #
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 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()
return val # Starting MQTT mosquitto print("Connecting to broker: {}:{}".format(BROKER, PORT)) print("Subscribe topic: '{}'".format(TOPIC_CONTROL)) client = mqtt.Client("car_client") client.on_message = on_message client.connect(BROKER, PORT) client.loop_start() client.subscribe(TOPIC_CONTROL) print("Connected") # Setup steering servo rcpy.set_state(rcpy.RUNNING) steering_servo = servo.Servo(CONFIG["STEERING_PIN"]) #clck = clock.Clock(steering_servo, CONFIG["PWM_PERIOD"]) def apply_steering_command(servo, s_command): s_command = clip_value(s_command, CONFIG["STEER_PWM_MIN"], CONFIG["STEER_PWM_MAX"]) servo.set(s_command) # Setup throttle esc. 0.5 is the 'zero throttle' # command used for arming the esc esc8.set(0.5) def apply_throttle_command(t_command):
# # print("jorge") # # # sleep some # # time.sleep(1) # # return # s = ScuttleServo() # s.setServoChannel() # s.move2(-1.5) duty = 1.5 period = 0.02 channel = 0 sweep = False brk = False free = False rcpy.set_state(rcpy.RUNNING) srvo = servo.Servo(channel) # def setServoChannel(channel=1): def setDuty(duty): srvo.set(duty) servo.enable() clck = clock.Clock(srvo, period) clck.start() # setServoChannel() setDuty(1.5) time.sleep(2) setDuty(-1.5) time.sleep(2)
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()
# import rcpy library # 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()