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')