def teleop(): atexit.register(set_normal_term) set_curses_term() # ノードの初期化。第1引数はノード名。第2引数はノード名がユニークになるように乱数をつける。 rospy.init_node('my_teleop', anonymous=True) # パブリッシャーの生成。第1引数はトピック名、第2引数は型、第3引数はメッセージバッファのサイズ。 pub = rospy.Publisher('/teleop', Int8, queue_size=1) # ループの周期。この場合は50Hz、1秒間に50回。 rate = rospy.Rate(50) print("w: forward, s: backward, d: right, a:left") while not rospy.is_shutdown(): if kbhit(): # 何かキーが押されるのを待つ key = getch() # 1文字取得 print(key) if key == 'a': pub.publish(1) elif key == 'd': pub.publish(-1) # linear.xは前後方向の並進速度(m/s)。前方向が正。 # angular.zは回転速度(rad/s)。反時計回りが正。 else: print("Input a,d") else: pub.publish(0) # 速度指令メッセージをパブリッシュ rate.sleep() # 指定した周期でループするよう寝て待つ
def myo_main(self): atexit.register(set_normal_term) set_curses_term() m = MyoRaw(None) m.add_emg_handler(self.proc_emg) m.connect() m.add_arm_handler(lambda arm, xdir: print('arm', arm, 'xdir', xdir)) m.add_pose_handler(lambda p: print('pose', p)) # [EMG0, EMG1, EMG2, EMG3, EMG4, EMG5, EMG6, EMG7, TIME, STATUS] dim_data = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0], dtype=float) data = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float) try: t_start = time.time() while True: m.run(1) #stop vibration ever m.write_attr(0x19, b'\x03\x01\x00') emg, self._time = m.plot_emg(t_start) if kbhit(): key = getch() if key == 'r': #print("ROCK") dim_data[-1:] = 1 elif key == 's': #print("SCISSOR") dim_data[-1:] = 2 elif key == 'p': #print("PAPET") dim_data[-1:] = 3 print("\r", end='') else: print("\r{0}".format(dim_data), end="") dim_data[-1:] = 0 continue #グラフは1次元 dim_data[:9] = np.append(emg, self._time) if self._time > 1.: if len(dim_data) == 10: dim2_data = np.expand_dims(dim_data, axis=0) data = np.append(data, dim2_data, axis=0) self.count += 1 except KeyboardInterrupt: pass finally: m.disconnect() if self.save_csv: self.save_data(self.saving_path + ".csv", data[1:]) if self.byn_np: np.save(self.saving_path, data[1:]) if self.plt_graph: self.data_plot(data) print("")
def main(): # kbhitのためのおまじない atexit.register(set_normal_term) set_curses_term() # Telloクラスを使って,droneというインスタンス(実体)を作る drone = tello.Tello('', 8889) current_time = time.time() # 現在時刻の保存変数 pre_time = current_time # 5秒ごとの'command'送信のための時刻変数 #Ctrl+cが押されるまでループ try: while True: if kbhit(): # 何かキーが押されるのを待つ key = getch() # 1文字取得 # キーに応じた処理 if key == 't': # 離陸 drone.takeoff() elif key == 'l': # 着陸 drone.land() elif key == 'w': # 前進 drone.move_forward(0.3) elif key == 's': # 後進 drone.move_backward(0.3) elif key == 'a': # 左移動 drone.move_left(0.3) elif key == 'd': # 右移動 drone.move_right(0.3) elif key == 'q': # 左旋回 drone.rotate_ccw(20) elif key == 'e': # 右旋回 drone.rotate_cw(20) elif key == 'r': # 上昇 drone.move_up(0.3) elif key == 'f': # 下降 drone.move_down(0.3) time.sleep(0.3) # 適度にウェイトを入れてCPU負荷を下げる # 5秒おきに'command'を送って、Telloが自動終了しないようにする current_time = time.time() # 現在時刻を取得 if current_time - pre_time > 5.0: # 前回時刻から5秒以上経過しているか? drone.send_command('command') # 'command'送信 pre_time = current_time # 前回時刻を更新 except (KeyboardInterrupt, SystemExit): # Ctrl+cが押されたら離脱 print("SIGINTを検知") # telloクラスを削除 del drone
# フライトコントローラ(FC)へ接続 print( "FCへ接続: %s" % (connection_string) ) # 接続設定文字列を表示 vehicle = connect(connection_string, wait_ready=True) # 接続 #==MQTTの初期化=========================================== client = mqtt.Client() # クラスのインスタンス(実体)の作成 client.on_connect = on_connect # 接続時のコールバック関数を登録 client.on_disconnect = on_disconnect # 切断時のコールバックを登録 client.on_publish = on_publish # メッセージ送信時のコールバック client.connect("localhost", 1883, 60) # 接続先は自分自身 client.loop_start() # 通信処理スタート #Ctrl+cが押されるまでループ try: while True: if kbhit(): # 何かキーが押されるのを待つ key = getch() # 1文字取得 # keyの中身に応じて分岐 if key=='g': # guided vehicle.mode = VehicleMode( 'GUIDED' ) elif key=='l': # land vehicle.mode = VehicleMode( 'LAND' ) elif key=='a': # arm vehicle.armed = True elif key=='d': # disarm vehicle.armed = False elif key=='t': # takeoff vehicle.simple_takeoff(alt=10) elif key=='1': # simple_goto # 柏の葉キャンパス交番上空30mへ
SpeedStepPlus = 0x05 SpeedStepMinus = 0x06 # Addr 0x40 for Adafuit board, 0x55 for XMC board <-<-<-<-<-<-<-<-<-<-I2C-Addresses #bus.write_byte_data(address_xmc,register,value) pwm = PWM(0x40, debug=True) pwm.setPWMFreq(60) # 1000 Hz for wheels, 60 for servos bus = smbus.SMBus(1) address_xmc = 0x55 setup() #Setup for Portexpander #-------------------------------------------------------------------- atexit.register(set_normal_term) set_curses_term() try: while (True): if kbhit(): Richtung = getch() #print "i2c Adresse: %02x" % (pwm.i2c.address) bus.write_byte_data(address_xmc,WaitStep, 0xa0) bus.write_byte_data(address_xmc,SpeedStepPlus, 0x02) bus.write_byte_data(address_xmc,SpeedStepMinus, 0x04) if Richtung=="w": #vorwaerts (MSD 0: 0x01..0x7F) bus.write_byte_data(address_xmc,SpeedLeftMotor, 0x28) bus.write_byte_data(address_xmc,SpeedRightMotor,0x28) elif Richtung=="s": #Stopp bus.write_byte_data(address_xmc,SpeedLeftMotor,0) bus.write_byte_data(address_xmc,SpeedRightMotor,0) elif Richtung=="x": #Rueckwaerts (MSD gesetzt: 0x81..0xFF) bus.write_byte_data(address_xmc,SpeedLeftMotor, 0xa8) bus.write_byte_data(address_xmc,SpeedRightMotor,0xa8) elif Richtung=="a": #links drehen
#kbhitを使う為のおまじない atexit.register(set_normal_term) set_curses_term() #接続文字列の作成 connection_string = "/dev/ttyS0, 57600" #フライトコントローラーへの接続 print("connect to FC: %s" % (connection_string)) vehicle = connect(connection_string, wait_ready = True) #main loop try: while True: if kbhit(): key = getch() #一文字取得 #keyの中身に応じた分岐 if key == 's': mode = 'STABILIZE' elif key == 'a': mode = 'ALT_HOLD' elif key == 'p': mode = 'POSHOLD' elif key == 'l': mode = 'LOITER'
from dronekit import LocationGlobal, LocationGlobalRelative import time vehicle = connect('127.0.0.1:14551', wait_ready=True) print("connected") print(vehicle) # for using kbhit.py atexit.register(set_normal_term) set_curses_term() #loop until pushing "Ctrl+c" try: while True: if kbhit(): # wait for pushing some key key = getch() # get 1 letter #select according contents of key if key == 's': # stabilize vehicle.mode = VehicleMode('STABILIZE') elif key == 'p': # PosHold vehicle.mode = VehicleMode('POSHOLD') elif key == 'l': # loiter vehicle.mode = VehicleMode('LOITER') elif key == 'g': # guided vehicle.mode = VehicleMode('GUIDED') elif key == 'r': # RTL vehicle.mode = VehicleMode('RTL') elif key == 'l': # land vehicle.mode = VehicleMode('LAND')