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 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 oneaction(): datalen = len(data['poses']) servo.enable() for i in range(0, datalen): srvo[i].set(data['poses'][i]['position']['z']) time.sleep(0.2) 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_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 start(self): servo.enable() for i in self.clocks: i.start() mpu9250.initialize(enable_dmp=True, dmp_sample_rate=100, enable_fusion=True, enable_magnetometer=True)
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 reset(): with open('open_1.json', 'r') as f: data = json.load(f) #print('open start') servo.enable() datalen = len(data['poses']) for i in range(0, datalen): print(i, data['poses'][i]['position']['x'], data['poses'][i]['position']['z']) srvo[i].set(data['poses'][i]['position']['z']) #print('open finish') time.sleep(1) 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 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): 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 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 runPWM(number): global number1, number2, pi # Raspberry Pi direct PWM output if os.uname()[1] == "raspberrypi": gpio = 0 if number == 1: gpio = 17 n = number1 elif number == 2: gpio = 27 n = number2 value = int(1000 + (n + 1) * 500) value = min(max(value, 1000), 2000) #print( gpio, value ) pi.set_servo_pulsewidth(gpio, value) if False: # Stop pulsing pi.set_PWM_dutycycle(17, 0) if False: # Start pulsing pi.set_PWM_dutycycle(17, 50) # If Beagle Bone Blue robotic cape support if rcpy: try: servo.enable() if number == 1: esc1.run() elif number == 2: servo2.run() except: pass elif pi is None: # Else output to Betaflight style motor controller if number == 1: # Only send if the first motor is pulsed, as we send for all motors in one message sendMotorCommands([0, 100.0 * number1, 100.0 * number2], False)
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()
#print('open start') servo.enable() datalen = len(data['poses']) for i in range(0, datalen): print(i, data['poses'][i]['position']['x'], data['poses'][i]['position']['z']) srvo[i].set(data['poses'][i]['position']['z']) #print('open finish') time.sleep(1) 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)
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) # THE SECTION BELOW WILL ONLY RUN IF L1_SERVO.PY IS CALLED DIRECTLY if __name__ == "__main__": while True:
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
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): srvo2.set(angle) # while rcpy.get_state() != rcpy.EXITING: # keep running # srvo.set(duty) # Set servo duty
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!")
f_m_minus = int(bins[m - 1]) # left f_m = int(bins[m]) # center f_m_plus = int(bins[m + 1]) # right fbank[m - 1, :f_m_minus] = 0 fbank[m - 1, f_m_plus:] = 0 for k in range(f_m_minus, f_m): fbank[m - 1, k] = (k - f_m_minus) / (f_m - f_m_minus) for k in range(f_m, f_m_plus): fbank[m - 1, k] = (f_m_plus - k) / (f_m_plus - f_m) #%% hitme_melcep = np.load('hitme_melcep.npy') left_melcep = np.load('left_melcep.npy') right_melcep = np.load('right_melcep.npy') up_melcep = np.load('up_melcep.npy') down_melcep = np.load('down_melcep.npy') garbo_melcep = np.load('garbo_melcep.npy') Nsamps = hitme_melcep.shape[1] Nsampsgarbo = garbo_melcep.shape[1] melceps = (hitme_melcep, left_melcep, right_melcep, up_melcep, down_melcep, garbo_melcep) servo.enable() #allow electrons to flow to the Mbed command_predictor = train(1, melceps, Nsamps, Nsampsgarbo) #train on the recorded samples time.sleep(2) #%% cont_voice_recog(command_predictor) #start listening for voice commands
def setDuty(duty): srvo.set(duty) servo.enable() clck = clock.Clock(srvo, period) clck.start()
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()