はじめに
こんにちは！この記事は、前回の「Jetson NanoでRoomba 980を操作しよう！初心者のための完全ガイド」の続編です。前回の記事で基本的な操作方法を学んだ皆さんに、さらに一歩進んだルンバのプログラミングテクニックをお伝えします。
今回は、Pythonを使ってより高度な制御を行い、Jetson Nanoの能力を最大限に活用してルンバをよりインテリジェントに動作させる方法を探っていきます。では、さっそく始めましょう！
前回のおさらい
前回の記事では、以下の内容を学びました：
- Jetson NanoとRoomba 980の接続方法
- 基本的な動作コマンドの実行
Jetson NanoでRoomba 980を操作しよう！初心者のための完全ガイド
はじめにこんにちは！この記事では、Jetson NanoというミニコンピューターをRoomba 980に接続し、操作する方法を詳しくご紹介します。プログラミングで掃除機ロボットを動かすなんて、ワクワクしますよね。難しそうに思えるかもしれませ...
hamaruki.com
2024.07.02
これらの基礎知識を踏まえて、今回はさらに高度なテクニックに挑戦していきます。
高度なセンサーデータの取得と活用
ルンバには多くのセンサーが搭載されています。これらのセンサーから詳細な情報を取得することで、より高度な制御が可能になります。以下のコードを見てみましょう：
from time import sleep
from pyroombaadapter import PyRoombaAdapter
from loguru import logger
PORT = "/dev/ttyACM0"
@logger.catch
def main():
logger.info("PyRoombaAdapterを初期化中")
adapter = PyRoombaAdapter(PORT)
adapter.change_mode_to_passive()
logger.info("センサー値を手動でリクエスト中")
sensor_functions = [
("Charging State (充電状態)", adapter.request_charging_state),
("Voltage (電圧)", adapter.request_voltage),
("Current (電流)", adapter.request_current),
("Temperature (温度)", adapter.request_temperature),
("Charge (充電量)", adapter.request_charge),
("Capacity (バッテリー容量)", adapter.request_capacity),
("OI Mode (OIモード)", adapter.request_oi_mode),
("Distance (移動距離)", adapter.request_distance),
("Angle (回転角度)", adapter.request_angle),
]
for sensor_name, sensor_func in sensor_functions:
value = sensor_func()
logger.info(f"{sensor_name}: {value}")
logger.info("データストリームを開始")
adapter.data_stream_start([
"Charging State", # 充電状態
"Voltage", # 電圧
"Current", # 電流
"Temperature", # 温度
"Battery Charge", # バッテリー充電量
"Battery Capacity",# バッテリー容量
"OI Mode" # OIモード
])
for i in range(3):
sleep(1)
data = adapter.data_stream_read()
logger.info(f"データストリーム（{i+1}回目）: {data}")
logger.info("データストリームを停止")
adapter.data_stream_stop()
if __name__ == "__main__":
main()
maki@maki-jetson5:~/SPARK$ python3 sensors.py
2024-07-03 20:01:39.911 | INFO | __main__:main:9 - PyRoombaAdapterを初期化中
Serial port is open, presumably to a roomba...
2024-07-03 20:01:40.914 | INFO | __main__:main:13 - センサー値を手動でリクエスト中
2024-07-03 20:01:40.928 | INFO | __main__:main:28 - Charging State (充電状態): 0
2024-07-03 20:01:40.946 | INFO | __main__:main:28 - Voltage (電圧): 15697
2024-07-03 20:01:40.964 | INFO | __main__:main:28 - Current (電流): -250
2024-07-03 20:01:40.976 | INFO | __main__:main:28 - Temperature (温度): 19
2024-07-03 20:01:40.994 | INFO | __main__:main:28 - Charge (充電量): 3140
2024-07-03 20:01:41.006 | INFO | __main__:main:28 - Capacity (バッテリー容量): 3414
2024-07-03 20:01:41.024 | INFO | __main__:main:28 - OI Mode (OIモード): 1
2024-07-03 20:01:41.042 | INFO | __main__:main:28 - Distance (移動距離): 0
2024-07-03 20:01:41.054 | INFO | __main__:main:28 - Angle (回転角度): 0.0
2024-07-03 20:01:41.054 | INFO | __main__:main:30 - データストリームを開始
2024-07-03 20:01:42.056 | INFO | __main__:main:44 - データストリーム（1回目）: [0, 15697, -258, 18, 3140, 3414, 1]
2024-07-03 20:01:43.059 | INFO | __main__:main:44 - データストリーム（2回目）: [0, 15697, -258, 17, 3140, 3414, 1]
2024-07-03 20:01:44.061 | INFO | __main__:main:44 - データストリーム（3回目）: [0, 15697, -258, 17, 3140, 3414, 1]
2024-07-03 20:01:44.061 | INFO | __main__:main:46 - データストリームを停止
maki@maki-jetson5:~/SPARK$ このコードでは、以下のような高度な操作を行っています：
- 多様なセンサーデータの取得: 充電状態、電圧、電流、温度など、様々なセンサーの情報を取得しています。
- データストリームの利用: 連続的にデータを取得するためのデータストリーム機能を使用しています。
- ログ記録:
loguruライブラリを使用して、詳細なログを記録しています。
これらの情報を活用することで、ルンバの状態をリアルタイムで把握し、適切な判断を行うことができます。
ルンバの動作制御
次に、ルンバの動作を制御する方法を見ていきましょう。以下のコードは、ルンバを回転させる例です：
import create
import time
from loguru import logger
# ログ設定
logger.add("roomba.log", rotation="500 MB", encoding="utf-8")
ROOMBA_PORT = "/dev/ttyACM0"
def main():
try:
# Roombaに接続
logger.info(f"ポート {ROOMBA_PORT} に接続しています...")
robot = create.Create(ROOMBA_PORT)
logger.info("接続に成功しました！")
# センサー情報を出力
logger.info("センサー情報:")
robot.printSensors()
# 安全モードに移行
logger.info("安全モードに移行しています...")
robot.toSafeMode()
logger.info("安全モードになりました！")
# 回転開始
logger.info("回転を開始します (速度: 0cm/s, 角速度: 10deg/s)")
robot.go(0, 10)
logger.info("回転中...")
time.sleep(2) # 2秒間回転
# Roombaを停止
logger.info("Roombaを停止します。")
robot.stop()
# 接続を終了
robot.close()
logger.info("接続を終了しました。")
except serial.SerialException as e:
logger.error(f"シリアル接続エラー: {e}")
if __name__ == "__main__":
main()
024-07-03 18:06:55.481 | INFO | __main__:main:13 - ポート /dev/ttyACM0 に接続しています...
PORT is /dev/ttyACM0
Serial port did open, presumably to a roomba...
Putting the robot into safe mode...
2024-07-03 18:06:56.896 | INFO | __main__:main:15 - 接続に成功しました！
2024-07-03 18:06:56.897 | INFO | __main__:main:18 - センサー情報:
LEFT_BUMP: 0
RIGHT_BUMP: 0
LEFT_WHEEL_DROP: 0
RIGHT_WHEEL_DROP: 0
CENTER_WHEEL_DROP: 0
WALL_IR_SENSOR: 0
CLIFF_LEFT: 0
CLIFF_FRONT_LEFT: 0
CLIFF_FRONT_RIGHT: 0
CLIFF_RIGHT: 0
VIRTUAL_WALL: 0
LEFT_WHEEL_OVERCURRENT: 0
RIGHT_WHEEL_OVERCURRENT: 0
INFRARED_BYTE: 0
PLAY_BUTTON: 0
ADVANCE_BUTTON: 0
POSE X (cm): 0.0
POSE Y (cm): 0.0
POSE TH (deg): 0.0
CHARGING_STATE: 0
VOLTAGE: 15808
CURRENT: -250
BATTERY_TEMP: 21
BATTERY_CHARGE: 3197
BATTERY_CAPACITY: 3414
WALL_SIGNAL: 7
CLIFF_LEFT_SIGNAL: 2144
CLIFF_FRONT_LEFT_SIGNAL: 2132
CLIFF_FRONT_RIGHT_SIGNAL: 2177
CLIFF_RIGHT_SIGNAL: 2131
OI_MODE: 2
SONG_NUMBER: 0
SONG_PLAYING: 0
ENCODER_LEFT: 63047
ENCODER_RIGHT: 13753
LIGHTBUMP: 0
LIGHTBUMP_LEFT: 0
LIGHTBUMP_FRONT_LEFT: 1
LIGHTBUMP_CENTER_LEFT: 1
LIGHTBUMP_CENTER_RIGHT: 0
LIGHTBUMP_CENTER_RIGHT: 0
LIGHTBUMP_RIGHT: 28
CHARGING_SOURCES_AVAILABLE: 0
2024-07-03 18:06:56.922 | INFO | __main__:main:22 - 安全モードに移行しています...
2024-07-03 18:06:57.234 | INFO | __main__:main:24 - 安全モードになりました！
2024-07-03 18:06:57.235 | INFO | __main__:main:27 - 回転を開始します (速度: 0cm/s, 角速度: 10deg/s)
2024-07-03 18:06:57.237 | INFO | __main__:main:29 - 回転中...
2024-07-03 18:06:59.240 | INFO | __main__:main:33 - Roombaを停止します。
2024-07-03 18:06:59.721 | INFO | __main__:main:38 - 接続を終了しました。
maki@maki-jetson5:~/SPARK$
このコードでは以下のような操作を行っています：
- 接続とモード設定: ルンバに接続し、安全モードに設定しています。
- 動作制御:
goメソッドを使用して、ルンバを回転させています。 - エラーハンドリング:
try-except文を使用して、エラーを適切に処理しています。
音楽再生機能の活用
ルンバには音楽を再生する機能もあります。これを使って、動作の開始や終了を音で知らせることができます：
from time import sleep
from pyroombaadapter import PyRoombaAdapter
PORT = "/dev/ttyACM0"
adapter = PyRoombaAdapter(PORT)
adapter.send_song_cmd(0, 9,
[69, 69, 69, 65, 72, 69, 65, 72, 69],
[40, 40, 40, 30, 10, 40, 30, 10, 80])
adapter.send_play_cmd(0)
sleep(10.0)このコードは、スターウォーズのダース・ベイダーのテーマ曲の一部を再生します。音楽再生機能を使うことで、ルンバの動作をより直感的に理解できるようになります。
ログ記録とデバッグ
前述のコード例では、loguruライブラリを使用してログを記録しています。ログを取ることで、以下のような利点があります：
- プログラムの動作を詳細に追跡できる
- エラーが発生した際に、原因の特定が容易になる
- ルンバの長期的な動作パターンを分析できる
ログ情報を活用することで、より安定したプログラムの開発が可能になります。
まとめと次のステップ
今回の記事では、Pythonを使ってJetson NanoでRoomba 980をより高度に制御する方法を紹介しました。センサーデータの詳細な取得と活用、複雑な動作制御、音楽再生機能の利用、そしてログ記録とデバッグについて学びました。
これらの技術を組み合わせることで、非常に柔軟で高度なルンバの制御が可能になります。例えば：
- センサーデータを分析して、効率的な清掃ルートを自動生成する
- 音楽再生機能を使って、清掃の開始や終了を知らせる
- ログデータを分析して、ルンバの動作パターンや効率を改善する
- 複数のセンサーデータを組み合わせて、より高度な判断ロジックを実装する
など、アイデア次第で様々な応用が考えられます。
次のステップとしては、以下のような発展的なトピックに挑戦してみるのも面白いでしょう：
- 画像認識を用いた障害物の検出と回避
- 機械学習を使用した最適な清掃パターンの学習
- Webインターフェースによるリモートコントロールシステムの構築
ルンバのプログラミングは、実世界の問題を解決するロボット工学の素晴らしい入り口です。ここで学んだ技術は、他の種類のロボットや、IoTデバイスのプログラミングにも応用できるでしょう。
ぜひ、これらの技術を活用して、あなただけのユニークなルンバプログラムを作ってみてください。プログラミングの楽しさと、ロボット工学の奥深さを存分に味わえるはずです。
Happy coding!
付録
create.py
Python2からPython3に変更したスクリプトです。
GitHub - martinschaef/roomba: Python scripts to control the Roomba via serial cable.
Python scripts to control the Roomba via serial cable. - martinschaef/roomba
github.com
#
# create.py
#
# Python interface for the iRobot Create
# Licensed as Artistic License
#
# Zach Dodds dodds@cs.hmc.edu
# updated for SIGCSE 3/9/07
#
# modified by Sean Luke Oct 20 2007
#
# modified by James O'Beirne Dec 9 2009
# 1. Two new functions (senseFunc and sleepTill).
# 2. getPose fixed (call to r.sensors([POSE]) added to stop()).
# 3. move() changed to accept parameters in centimeters instead of milimeters
# for consistency with printSensors/getPose.
#
# modified by Martin Schaef Feb 2016
# 1. Added support for dirt sensor, encoders, and light bump.
# 2. Adjusted the size of the Roomba in WHEEL_SPAN
# 3. getPose seems to be broken.
import serial
import math
import time
import datetime
import _thread
import threading
# some module-level definitions for the robot commands
START = bytes([128])
BAUD = bytes([129])
CONTROL = bytes([130]) # deprecated for Create
SAFE = bytes([131])
FULL = bytes([132])
POWER = bytes([133])
SPOT = bytes([134]) # Same for the Roomba and Create
CLEAN = bytes([135]) # Clean button - Roomba
COVER = bytes([135]) # Cover demo - Create
MAX = bytes([136]) # Roomba
DEMO = bytes([136]) # Create
DRIVE = bytes([137]) # + 4 bytes
MOTORS = bytes([138]) # + 1 byte
LEDS = bytes([139]) # + 3 bytes
SONG = bytes([140]) # + 2N+2 bytes, where N is the number of notes
PLAY = bytes([141]) # + 1 byte
SENSORS = bytes([142]) # + 1 byte
CHANGE_TIME = bytes([168])
FORCESEEKINGDOCK = bytes([143]) # same on Roomba and Create
# the above command is called "Cover and Dock" on the Create
DRIVEDIRECT = bytes([145]) # Create only
STREAM = bytes([148]) # Create only
QUERYLIST = bytes([149]) # Create only
PAUSERESUME = bytes([150]) # Create only
#### Sean
SCRIPT = bytes([152])
ENDSCRIPT = bytes([153])
WAITDIST = bytes([156])
WAITANGLE = bytes([157])
# the four SCI modes
# the code will try to keep track of which mode the system is in,
# but this might not be 100% trivial...
OFF_MODE = 0
PASSIVE_MODE = 1
SAFE_MODE = 2
FULL_MODE = 3
# the sensors
BUMPS_AND_WHEEL_DROPS = 7
WALL_IR_SENSOR = 8
CLIFF_LEFT = 9
CLIFF_FRONT_LEFT = 10
CLIFF_FRONT_RIGHT = 11
CLIFF_RIGHT = 12
VIRTUAL_WALL = 13
LSD_AND_OVERCURRENTS = 14
DIRT_DETECTED = 15
INFRARED_BYTE = 17
BUTTONS = 18
DISTANCE = 19
ANGLE = 20
CHARGING_STATE = 21
VOLTAGE = 22
CURRENT = 23
BATTERY_TEMP = 24
BATTERY_CHARGE = 25
BATTERY_CAPACITY = 26
WALL_SIGNAL = 27
CLIFF_LEFT_SIGNAL = 28
CLIFF_FRONT_LEFT_SIGNAL = 29
CLIFF_FRONT_RIGHT_SIGNAL = 30
CLIFF_RIGHT_SIGNAL = 31
CARGO_BAY_DIGITAL_INPUTS = 32
CARGO_BAY_ANALOG_SIGNAL = 33
CHARGING_SOURCES_AVAILABLE = 34
OI_MODE = 35
SONG_NUMBER = 36
SONG_PLAYING = 37
NUM_STREAM_PACKETS = 38
REQUESTED_VELOCITY = 39
REQUESTED_RADIUS = 40
REQUESTED_RIGHT_VELOCITY = 41
REQUESTED_LEFT_VELOCITY = 42
ENCODER_LEFT = 43
ENCODER_RIGHT = 44
LIGHTBUMP = 45
LIGHTBUMP_LEFT = 46
LIGHTBUMP_FRONT_LEFT = 47
LIGHTBUMP_CENTER_LEFT = 48
LIGHTBUMP_CENTER_RIGHT = 49
LIGHTBUMP_FRONT_RIGHT = 50
LIGHTBUMP_RIGHT = 51
# others just for easy access to particular parts of the data
POSE = 100
LEFT_BUMP = 101
RIGHT_BUMP = 102
LEFT_WHEEL_DROP = 103
RIGHT_WHEEL_DROP = 104
CENTER_WHEEL_DROP = 105
LEFT_WHEEL_OVERCURRENT = 106
RIGHT_WHEEL_OVERCURRENT = 107
ADVANCE_BUTTON = 108
PLAY_BUTTON = 109
# 0 1 2 3 4 5 6 7 8 9101112131415161718192021222324252627282930313233343536373839404142434445464748495051
SENSOR_DATA_WIDTH = [0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,2,2,1,2,2,1,2,2,2,2,2,2,2,1,2,1,1,1,1,1,2,2,2,2,2,2,1,2,2,2,2,2,2]
#The original value was 258.0 but my roomba has 235.0
WHEEL_SPAN = 235.0
WHEEL_DIAMETER = 72.0
TICK_PER_REVOLUTION = 508.8 # original 508.8
TICK_PER_MM = TICK_PER_REVOLUTION/(math.pi*WHEEL_DIAMETER)
# on my floor, a full turn is measured as sth like 450 deg
# add an error to the computation to account for that.
ANGULAR_ERROR = 360.0/450.0
# for printing the SCI modes
def modeStr( mode ):
""" prints a string representing the input SCI mode """
if mode == OFF_MODE: return 'OFF_MODE'
if mode == PASSIVE_MODE: return 'PASSIVE_MODE'
if mode == SAFE_MODE: return 'SAFE_MODE'
if mode == FULL_MODE: return 'FULL_MODE'
print(('Warning: unknown mode', mode, 'seen in modeStr'))
return 'UNKNOWN_MODE'
#
# some module-level functions for dealing with bits and bytes
#
def _bytesOfR( r ):
""" for looking at the raw bytes of a sensor reply, r """
print('raw r is', r)
for i in range(len(r)):
print('byte', i, 'is', ord(r[i]))
print('finished with formatR')
def _bitOfByte( bit, byte ):
""" returns a 0 or 1: the value of the 'bit' of 'byte' """
if bit < 0 or bit > 7:
print('Your bit of', bit, 'is out of range (0-7)')
print('returning 0')
return 0
return ((byte >> bit) & 0x01)
def _toBinary( val, numbits ):
""" prints numbits digits of val in binary """
if numbits == 0: return
_toBinary( val>>1 , numbits-1 )
print((val & 0x01), end=' ') # print least significant bit
def _fromBinary( s ):
""" s is a string of 0's and 1's """
if s == '': return 0
lowbit = ord(s[-1]) - ord('0')
return lowbit + 2*_fromBinary( s[:-1] )
def _twosComplementInt1byte( byte ):
""" returns an int of the same value of the input
int (a byte), but interpreted in two's
complement
the output range should be -128 to 127
"""
# take everything except the top bit
topbit = _bitOfByte( 7, byte )
lowerbits = byte & 127
if topbit == 1:
return lowerbits - (1 << 7)
else:
return lowerbits
def _twosComplementInt2bytes( highByte, lowByte ):
""" returns an int which has the same value
as the twosComplement value stored in
the two bytes passed in
the output range should be -32768 to 32767
chars or ints can be input, both will be
truncated to 8 bits
"""
# take everything except the top bit
topbit = _bitOfByte( 7, highByte )
lowerbits = highByte & 127
unsignedInt = lowerbits << 8 | (lowByte & 0xFF)
if topbit == 1:
# with sufficient thought, I've convinced
# myself of this... we'll see, I suppose.
return unsignedInt - (1 << 15)
else:
return unsignedInt
def _toTwosComplement2Bytes( value ):
""" returns two bytes (ints) in high, low order
whose bits form the input value when interpreted in
two's complement
"""
# if positive or zero, it's OK
if value >= 0:
eqBitVal = value
# if it's negative, I think it is this
else:
eqBitVal = (1<<16) + value
return ( (eqBitVal >> 8) & 0xFF, eqBitVal & 0xFF )
#
# this class represents a snapshot of the robot's data
#
class SensorFrame:
""" the sensorFrame class is really a struct whose
fields are filled in by sensorStatus
"""
def __init__(self):
""" constructor -- set all fields to 0
"""
self.casterDrop = 0
self.leftWheelDrop = 0
self.rightWheelDrop = 0
self.leftBump = 0
self.rightBump = 0
self.wallSensor = 0
self.leftCliff = 0
self.frontLeftCliff = 0
self.frontRightCliff = 0
self.rightCliff = 0
self.virtualWall = 0
self.driveLeft = 0
self.driveRight = 0
self.mainBrush = 0
self.vacuum = 0
self.sideBrush = 0
self.leftDirt = 0
self.rightDirt = 0
self.remoteControlCommand = 0
self.powerButton = 0
self.spotButton = 0
self.cleanButton = 0
self.maxButton = 0
self.distance = 0
self.rawAngle = 0
self.angleInRadians = 0
self.chargingState = 0
self.voltage = 0
self.current = 0
self.temperature = 0
self.charge = 0
self.capacity = 0
self.lightBumpLeft = 0
self.lightBumpFrontLeft = 0
self.lightCenterLeft = 0
self.lightCenterRight = 0
self.lightBumpFrontRight = 0
self.lightBumpRight = 0
self.dirt = 0
def __str__(self):
""" returns a string with the information
from this SensorFrame
"""
# there's probably a more efficient way to do this...
# perhaps just making it all + instead of the separate
# += would be more efficient
#
# actually, we should make a list and call ''.join(list)
# not that we will...
#
s = ''
s += 'casterDrop: ' + str(self.casterDrop) + '\n'
s += 'leftWheelDrop: ' + str(self.leftWheelDrop) + '\n'
s += 'rightWheelDrop: ' + str(self.rightWheelDrop) + '\n'
s += 'leftBump: ' + str(self.leftBump) + '\n'
s += 'rightBump: ' + str(self.rightBump) + '\n'
s += 'wallSensor: ' + str(self.wallSensor) + '\n'
s += 'leftCliff: ' + str(self.leftCliff) + '\n'
s += 'frontLeftCliff: ' + str(self.frontLeftCliff) + '\n'
s += 'frontRightCliff: ' + str(self.frontRightCliff) + '\n'
s += 'rightCliff: ' + str(self.rightCliff) + '\n'
s += 'virtualWall: ' + str(self.virtualWall) + '\n'
s += 'driveLeft: ' + str(self.driveLeft) + '\n'
s += 'driveRight: ' + str(self.driveRight) + '\n'
s += 'mainBrush: ' + str(self.mainBrush) + '\n'
s += 'vacuum: ' + str(self.vacuum) + '\n'
s += 'sideBrush: ' + str(self.sideBrush) + '\n'
s += 'leftDirt: ' + str(self.leftDirt) + '\n'
s += 'rightDirt: ' + str(self.rightDirt) + '\n'
s += 'remoteControlCommand: ' + str(self.remoteControlCommand) + '\n'
s += 'powerButton: ' + str(self.powerButton) + '\n'
s += 'spotButton: ' + str(self.spotButton) + '\n'
s += 'cleanButton: ' + str(self.cleanButton) + '\n'
s += 'maxButton: ' + str(self.maxButton) + '\n'
s += 'distance: ' + str(self.distance) + '\n'
s += 'rawAngle: ' + str(self.rawAngle) + '\n'
s += 'angleInRadians: ' + str(self.angleInRadians) + '\n'
# no data member needed for this next line
s += 'angleInDegrees: ' + str(math.degrees(self.angleInRadians)) + '\n'
s += 'chargingState: ' + str(self.chargingState) + '\n'
s += 'voltage: ' + str(self.voltage) + '\n'
s += 'current: ' + str(self.current) + '\n'
s += 'temperature: ' + str(self.temperature) + '\n'
s += 'charge: ' + str(self.charge) + '\n'
s += 'capacity: ' + str(self.capacity) + '\n'
return s
def _toBinaryString(self):
""" this converts the calling SensorFrame into a 26-byte
string of the format the roomba sends back
"""
# todo: handle the different subsets (frames) of sensor data
# here are the 26 bytes in list form
slist = [0]*26
# First Frame
# byte 0: bumps and wheeldrops
slist[0] = self.casterDrop << 4 | \
self.leftWheelDrop << 3 | \
self.rightWheelDrop << 2 | \
self.leftBump << 1 | \
self.rightBump
# byte 1: wall data
slist[1] = self.wallSensor
# byte 2: cliff left
slist[2] = self.leftCliff
# byte 3: cliff front left
slist[3] = self.frontLeftCliff
# byte 4: cliff front right
slist[4] = self.frontRightCliff
# byte 5: cliff right
slist[5] = self.rightCliff
# byte 6: virtual wall
slist[6] = self.virtualWall
# byte 7: motor overcurrents
slist[7] = self.driveLeft << 4 | \
self.driveRight << 3 | \
self.mainBrush << 2 | \
self.vacuum << 1 | \
self.sideBrush
# byte 8: dirt detector left
slist[8] = self.leftDirt
# byte 9: dirt detector left
slist[9] = self.rightDirt
# Second Frame
# byte 10: remote control command
slist[10] = self.remoteControlCommand
# byte 11: buttons
slist[11] = self.powerButton << 3 | \
self.spotButton << 2 | \
self.cleanButton << 1 | \
self.maxButton
# bytes 12, 13: distance
highVal, lowVal = _toTwosComplement2Bytes( self.distance )
slist[12] = highVal
slist[13] = lowVal
# bytes 14, 15: angle
highVal, lowVal = _toTwosComplement2Bytes( self.rawAngle )
slist[14] = highVal
slist[15] = lowVal
# Third Frame
# byte 16: charging state
slist[16] = self.chargingState
# bytes 17, 18: voltage
slist[17] = (self.voltage >> 8) & 0xFF
slist[18] = self.voltage & 0xFF
# bytes 19, 20: current
highVal, lowVal = _toTwosComplement2Bytes( self.current )
slist[19] = highVal
slist[20] = lowVal
# byte 21: temperature
slist[21] = self.temperature
# bytes 22, 23: charge
slist[22] = (self.charge >> 8) & 0xFF
slist[23] = self.charge & 0xFF
# bytes 24, 25: capacity
slist[24] = (self.capacity >> 8) & 0xFF
slist[25] = self.capacity & 0xFF
# convert to a string
s = ''.join([ chr(x) for x in slist ])
return s
#
# the robot class
#
class Create:
""" the Create class is an abstraction of the iRobot Create's
SCI interface, including communication and a bit
of processing of the strings passed back and forth
when you create an object of type Create, the code
will try to open a connection to it - so, it will fail
if it's not attached!
"""
# to do: check if we can start in other modes...
def __init__(self, PORT, BAUD_RATE=115200, startingMode=SAFE_MODE):
""" the constructor which tries to open the
connection to the robot at port PORT
"""
_debug = True
# to do: find the shortest safe serial timeout value...
# to do: use the timeout to do more error checking than
# is currently done...
#
# the -1 here is because windows starts counting from 1
# in the hardware control panel, but not in pyserial, it seems
# if PORT is the string 'simulated' (or any string for the moment)
# we use our SRSerial class
print('PORT is', PORT)
if type(PORT) == type('string'):
if PORT == 'sim':
print('In simulated mode...')
self.ser = 'sim'; # SRSerial('mapSquare.txt')
else:
# for Mac/Linux - use whole port name
# print 'In Mac/Linux mode...'
self.ser = serial.Serial(PORT, baudrate=BAUD_RATE, timeout=0.5)
# otherwise, we try to open the numeric serial port...
else:
# print 'In Windows mode...'
self.ser = serial.Serial(PORT-1, baudrate=BAUD_RATE, timeout=0.5)
# did the serial port actually open?
if self.ser != 'sim' and self.ser.isOpen():
print('Serial port did open, presumably to a roomba...')
else:
print('Serial port did NOT open, check the')
print(' - port number')
print(' - physical connection')
print(' - baud rate of the roomba (it\'s _possible_, if unlikely,')
print(' that it might be set to 19200 instead')
print(' of the default 57600 - removing and')
print(' reinstalling the battery should reset it.')
# our OI mode
self.sciMode = OFF_MODE
# our sensor dictionary, currently empty
self.sensord = {}
# here are the variables that constitute the robot's
# estimated odometry, thr is theta in radians...
# these are updated by integrateNextOdometricStep
self.xPose = 0.0
self.yPose = 0.0
self.thrPose = 0.0
self.leftEncoder = -1
self.rightEncoder = -1
self.leftEncoder_old = -1
self.rightEncoder_old = -1
time.sleep(0.3)
self._start() # go to passive mode - want to do this
# regardless of the final mode we'd like to be in...
time.sleep(0.3)
if (startingMode == SAFE_MODE):
print('Putting the robot into safe mode...')
self.toSafeMode()
if (startingMode == FULL_MODE):
print('Putting the robot into full mode...')
self.toSafeMode()
time.sleep(0.3)
self.toFullMode()
# We need to read the angle and distance sensors so that
# their values clear out!
time.sleep(0.25)
#self.sensors(6) # read all sensors to establish the sensord dictionary
self.setPose(0,0,0)
_debug = False
def _write(self, byte):
if self._debug==True:
print(ord(byte))
# byteがすでにバイトオブジェクトであるため、そのまま渡す
self.ser.write(byte)
def getPose(self, dist='cm', angle='deg'):
""" getPose returns the current estimate of the
robot's global pose
dist may be 'cm' or 'mm'
angle may be 'deg' or 'rad'
"""
x = 0; y = 0; th = 0
if dist == 'cm':
x = self.xPose/10.0; y = self.yPose/10.0
else:
x = self.xPose; y = self.yPose
if angle == 'deg':
th = math.degrees(self.thrPose)
else:
th = self.thrPose
return (x,y,th)
def setPose(self, x, y, th, dist='cm', angle='deg'):
""" setPose sets the internal odometry to the input values
x: global x in mm
y: global y in mm
th: global th in radians
dist: 'cm' or 'mm' for x and y
angle: 'deg' or 'rad' for th
"""
if dist == 'cm':
self.xPose = x*10.0; self.yPose = y*10.0
else:
self.xPose = x; self.yPose = y
if angle == 'deg':
self.thrPose = math.radians(th)
else:
self.thrPose = th
def resetPose(self):
""" resetPose simply sets the internal odometry to 0,0,0
"""
self.setPose(0.0,0.0,0.0)
def _getEncoderDelta(self, oldEnc, newEnc):
#encoder wrap around at 2^16
#check if the step is bigger than half the
#possible range and treat this as wraparound
delta = newEnc-oldEnc
if delta< -65536/2:
delta = (newEnc+65536)-oldEnc
if delta> 65536/2:
delta = newEnc-(oldEnc+65536)
return delta
def _integrateNextEncoderStep(self):
if self.leftEncoder_old == -1:
self.leftEncoder_old = self.leftEncoder
self.rightEncoder_old = self.rightEncoder
return
left_diff = self._getEncoderDelta(self.leftEncoder_old,self.leftEncoder)
right_diff = self._getEncoderDelta(self.rightEncoder_old,self.rightEncoder)
left_mm = left_diff / TICK_PER_MM;
right_mm = right_diff / TICK_PER_MM;
distance = (left_mm + right_mm) / 2.0;
dAngle = (right_mm - left_mm) / WHEEL_SPAN
dAngle *= ANGULAR_ERROR
self.thrPose += dAngle
if self.thrPose > 100*math.pi:
self.thrPose -= 101*math.pi
if self.thrPose < -100*math.pi:
self.thrPose += 101*math.pi
self.xPose += distance * math.cos(self.thrPose)
self.yPose += distance * math.sin(self.thrPose)
self.leftEncoder_old = self.leftEncoder
self.rightEncoder_old = self.rightEncoder
def _integrateNextOdometricStepCreate(self, distance, rawAngle):
""" integrateNextOdometricStep adds the reported inputs
distance in mm
rawAngle in degrees
to the estimate of the robot's global pose
"""
# OK, so this _should_ be easy
# distance is, supposedly, the arc length that the center
# of the robot has traveled (the average of
# the two wheel's linear distances)
#
# rawAngle is, supposedly, the (RightWheel-LeftWheel)/2.0
#
# the distance (diameter) between the two wheels is 258mm
# keep in mind that the robot's physical diameter is larger ~
#
# 0.5*258 == 129mm radius
#
# perhaps there's nothing to do...
if distance == 0 and rawAngle == 0:
return
print(rawAngle)
# then again, maybe there is something to do...
dthr = math.radians(rawAngle) # angle traveled
d = distance # distance traveled
# compute offsets in the local coordinate system,
# with the x-axis pointing in the direction the robot was
# initially facing (for this move) and the y-axis pointing
# perpendicularly to the left (when facing forward)
#
# first, the special case when the angle is zero...
if rawAngle == 0:
dx = float(d)
dy = 0.0
# or if the distance is zero...
elif distance == 0:
dx = 0.0
dy = 0.0
# or when neither is zero...
else:
# finite radius of curvature
ROC = float(d)/dthr # remember, this is signed!
dx = ROC*math.sin(dthr) # because ROC is signed,
dy = ROC-ROC*math.cos(dthr) # we don't need two cases
#
# we need to add dx, dy, and dthr to the global pose
# and so we need to do so in the global direction in
# which the robot was facing at the start of this movement
#
# here is the unit vector describing that direction
unitForwardX = math.cos( self.thrPose )
unitForwardY = math.sin( self.thrPose )
# here is the unit vector perpendicular to the left
unitPerpX = math.cos( self.thrPose + math.pi/2.0 )
unitPerpY = math.sin( self.thrPose + math.pi/2.0 )
# now we compute our global offsets
dx_global = dx*unitForwardX + dy*unitPerpX
dy_global = dx*unitForwardY + dy*unitPerpY
##print 'distance and rawAngle', distance, rawAngle
##print 'local offsets, x, y, thd', dx, dy, math.degrees(dthr)
##print 'global offsets, x, y, thd', dx_global, dy_global, math.degrees(dthr)
# and we add them all in...
self.xPose += dx_global
self.yPose += dy_global
self.thrPose += dthr
#print 'final pose', self.xPose, self.yPose, self.thrPose
return
def setWheelVelocities( self, left_cm_sec, right_cm_sec ):
""" sends velocities of each wheel independently
left_cm_sec: left wheel velocity in cm/sec (capped at +- 50)
right_cm_sec: right wheel velocity in cm/sec (capped at +- 50)
"""
if left_cm_sec < -50: left_cm_sec = -50;
if left_cm_sec > 50: left_cm_sec = 50;
if right_cm_sec < -50: right_cm_sec = -50;
if right_cm_sec > 50: right_cm_sec = 50;
# convert to mm/sec, ensure we have integers
leftHighVal, leftLowVal = _toTwosComplement2Bytes( int(left_cm_sec*10) )
rightHighVal, rightLowVal = _toTwosComplement2Bytes( int(right_cm_sec*10) )
# send these bytes and set the stored velocities
self._write( DRIVEDIRECT )
self._write( chr(rightHighVal) )
self._write( chr(rightLowVal) )
self._write( chr(leftHighVal) )
self._write( chr(leftLowVal) )
def stop(self):
""" stop calls go(0,0) """
self.go(0,0)
# we've gotta update pose information
foo = self.sensors([POSE])
def motors ( self, side_brush = 0, main_brush = 0, vacuum = 0):
if (side_brush > 1):
side_brush = 1
elif (side_brush < -1):
side_brush = -1
if (main_brush > 1):
main_brush = 1
elif (main_brush < -1):
main_brush = -1
if (vacuum > 1):
vacuum = 1
elif (vacuum < 0):
vacuum = 0
byteToWrite = chr((16 if main_brush < 0 else 0) |
(8 if side_brush < 0 else 0) |
(4 if main_brush != 0 else 0) |
(2 if vacuum != 0 else 0) |
(1 if side_brush > 0 else 0))
self._write( MOTORS )
self._write( byteToWrite)
def go( self, cm_per_sec=0, deg_per_sec=0 ):
""" go(cmpsec, degpsec) sets the robot's velocity to
cmpsec centimeters per second
degpsec degrees per second
go() is equivalent to go(0,0)
"""
# need to convert to the roomba's drive parameters
#
# for now, just one or the other...
if cm_per_sec == 0:
# just handle rotation
# convert to radians
rad_per_sec = math.radians(deg_per_sec)
# make sure the direction is correct
if rad_per_sec >= 0: dirstr = 'CCW'
else: dirstr = 'CW'
# compute the velocity, given that the robot's
# radius is 258mm/2.0
vel_mm_sec = math.fabs(rad_per_sec) * (WHEEL_SPAN/2.0)
# send it off to the robot
self._drive( vel_mm_sec, 0, dirstr )
elif deg_per_sec == 0:
# just handle forward/backward translation
vel_mm_sec = 10.0*cm_per_sec
big_radius = 32767
# send it off to the robot
self._drive( vel_mm_sec, big_radius )
else:
# move in the appropriate arc
rad_per_sec = math.radians(deg_per_sec)
vel_mm_sec = 10.0*cm_per_sec
radius_mm = vel_mm_sec / rad_per_sec
# check for extremes
if radius_mm > 32767: radius_mm = 32767
if radius_mm < -32767: radius_mm = -32767
self._drive( vel_mm_sec, radius_mm )
return
def _start(self):
""" changes from OFF_MODE to PASSIVE_MODE """
self._write( START )
# they recommend 20 ms between mode-changing commands
time.sleep(0.25)
# change the mode we think we're in...
return
def close(self):
""" tries to shutdown the robot as kindly as possible, by
clearing any remaining odometric data
going to passive mode
closing the serial port
"""
# is there other clean up to be done?
# let's get rid of any lingering odometric data
# we don't call getSensorList, because we don't want to integrate the odometry...
self._getRawSensorDataAsList( [19,20] )
time.sleep(0.1)
self._start() # send Create back to passive mode
time.sleep(0.1)
self.ser.close()
return
def _closeSer(self):
""" just disconnects the serial port """
self.ser.close()
return
def _openSer(self):
""" opens the port again """
self.ser.open()
return
def _drive(self, roomba_mm_sec, roomba_radius_mm, turn_dir='CCW'):
""" implements the drive command as specified
the turn_dir should be either 'CW' or 'CCW' for
clockwise or counterclockwise - this is only
used if roomba_radius_mm == 0 (or rounds down to 0)
other drive-related calls are available
"""
# first, they should be ints
# in case they're being generated mathematically
if type(roomba_mm_sec) != type(42):
roomba_mm_sec = int(roomba_mm_sec)
if type(roomba_radius_mm) != type(42):
roomba_radius_mm = int(roomba_radius_mm)
# we check that the inputs are within limits
# if not, we cap them there
if roomba_mm_sec < -500:
roomba_mm_sec = -500
if roomba_mm_sec > 500:
roomba_mm_sec = 500
# if the radius is beyond the limits, we go straight
# it doesn't really seem to go straight, however...
if roomba_radius_mm < -2000:
roomba_radius_mm = 32768
if roomba_radius_mm > 2000:
roomba_radius_mm = 32768
# get the two bytes from the velocity
# these come back as numbers, so we will chr them
velHighVal, velLowVal = _toTwosComplement2Bytes( roomba_mm_sec )
# get the two bytes from the radius in the same way
# note the special cases
if roomba_radius_mm == 0:
if turn_dir == 'CW':
roomba_radius_mm = -1
else: # default is 'CCW' (turning left)
roomba_radius_mm = 1
radiusHighVal, radiusLowVal = _toTwosComplement2Bytes( roomba_radius_mm )
#print 'bytes are', velHighVal, velLowVal, radiusHighVal, radiusLowVal
# send these bytes and set the stored velocities
self._write( DRIVE )
# chr(velHighVal) を bytes([velHighVal]) に変更
self._write( bytes([velHighVal]) )
self._write( bytes([velLowVal]) )
self._write( bytes([radiusHighVal]) )
self._write( bytes([radiusLowVal]) )
def setLEDs(self, power_color, power_intensity, play, advance ):
""" The setLEDs method sets each of the three LEDs, from left to right:
the power LED, the play LED, and the status LED.
The power LED at the left can display colors from green (0) to red (255)
and its intensity can be specified, as well. Hence, power_color and
power_intensity are values from 0 to 255. The other two LED inputs
should either be 0 (off) or 1 (on).
"""
# make sure we're within range...
if advance != 0: advance = 1
if play != 0: play = 1
try:
power = int(power_intensity)
powercolor = int(power_color)
except TypeError:
power = 128
powercolor = 128
print('Type excpetion caught in setAbsoluteLEDs in roomba.py')
print('Your power_color or power_intensity was not of type int.')
if power < 0: power = 0
if power > 255: power = 255
if powercolor < 0: powercolor = 0
if powercolor > 255: powercolor = 255
# create the first byte
#firstByteVal = (status << 4) | (spot << 3) | (clean << 2) | (max << 1) | dirtdetect
firstByteVal = (advance << 3) | (play << 1)
# send these as bytes
# print 'bytes are', firstByteVal, powercolor, power
self._write( LEDS )
self._write( chr(firstByteVal) )
self._write( chr(powercolor) )
self._write( chr(power) )
return
#
# DO NOT CALL THIS FUNCTION!
# call readSensors instead - it will integrate odometry
# for what that's worth, admittedly...
# if you call this without integrating odometry, the
# distance and rawAngle reported will be lost...
#
def _getRawSensorFrameAsList(self, packetnumber):
""" gets back a raw string of sensor data
which then can be used to create a SensorFrame
"""
if type(packetnumber) != type(1):
packetnumber = 6
if packetnumber < 0 or packetnumber > 6:
packetnumber = 6
self._write( SENSORS )
self._write( chr(packetnumber) )
if packetnumber == 0:
r = self.ser.read(size=26)
if packetnumber == 1:
r = self.ser.read(size=10)
if packetnumber == 2:
r = self.ser.read(size=6)
if packetnumber == 3:
r = self.ser.read(size=10)
if packetnumber == 4:
r = self.ser.read(size=14)
if packetnumber == 5:
r = self.ser.read(size=12)
if packetnumber == 6:
r = self.ser.read(size=52)
r = [ ord(c) for c in r ] # convert to ints
return r
def _getRawSensorDataAsList(self, listofsensors):
""" gets the chosen sensors
and returns the raw bytes, as a string
needs to be converted to integers...
"""
numberOfSensors = len(listofsensors)
self._write( QUERYLIST )
self._write( bytes([numberOfSensors]) )
resultLength = 0
for sensornum in listofsensors:
self._write( bytes([sensornum]) )
resultLength += SENSOR_DATA_WIDTH[sensornum]
r = self.ser.read(size=resultLength)
# ord(c) を c に変更
r = [ c for c in r ] # convert to ints
#print 'r is ', r
return r
def seekDock(self):
""" sends the force-seeking-dock signal
"""
self.demo(1)
def demo(self, demoNumber=-1):
""" runs one of the built-in demos for Create
if demoNumber is
<omitted> or
-1 stop current demo
0 wander the surrounding area
1 wander and dock, when the docking station is seen
2 wander a more local area
3 wander to a wall and then follow along it
4 figure 8
5 "wimp" demo: when pushed, move forward
when bumped, move back and away
6 home: will home in on a virtual wall, as
long as the back and sides of the IR receiver
are covered with tape
7 tag: homes in on sequential virtual walls
8 pachelbel: plays the first few notes of the canon in D
9 banjo: plays chord notes according to its cliff sensors
chord key is selected via the bumper
"""
if (demoNumber < -1 or demoNumber > 9):
demoNumber = -1 # stop current demo
self._write( DEMO )
if demoNumber < 0 or demoNumber > 9:
# invalid values are equivalent to stopping
self._write( chr(255) ) # -1
else:
self._write( chr(demoNumber) )
def setSong(self, songNumber, songDataList):
""" this stores a song to roomba's memory to play later
with the playSong command
songNumber must be between 0 and 15 (inclusive)
songDataList is a list of (note, duration) pairs (up to 16)
note is the midi note number, from 31 to 127
(outside this range, the note is a rest)
duration is from 0 to 255 in 1/64ths of a second
"""
# any notes to play?
if type(songDataList) != type([]) and type(songDataList) != type(()):
print('songDataList was', songDataList)
return
if len(songDataList) < 1:
print('No data in the songDataList')
return
if songNumber < 0: songNumber = 0
if songNumber > 15: songNumber = 15
# indicate that a song is coming
self._write( SONG )
self._write( chr(songNumber) )
L = min(len(songDataList), 16)
self._write( chr(L) )
# loop through the notes, up to 16
for note in songDataList[:L]:
# make sure its a tuple, or else we rest for 1/4 second
if type(note) == type( () ):
#more error checking here!
self._write( chr(note[0]) ) # note number
self._write( chr(note[1]) ) # duration
else:
self._write( chr(30) ) # a rest note
self._write( chr(16) ) # 1/4 of a second
return
def playSong(self, list_of_notes):
""" The input to <tt>playSong</tt> should be specified as a list
of pairs of [ note_number, note_duration ] format. Thus,
r.playSong( [(60,8),(64,8),(67,8),(72,8)] ) plays a quick C chord.
"""
# implemented by setting song #1 to the notes and then playing it
self.setSong(1, list_of_notes)
self.playSongNumber(1)
def playSongNumber(self, songNumber):
""" plays song songNumber """
if songNumber < 0: songNumber = 0
if songNumber > 15: songNumber = 15
self._write( PLAY )
self._write( chr(songNumber) )
def playNote(self, noteNumber, duration, songNumber=0):
""" plays a single note as a song (at songNumber)
duration is in 64ths of a second (1-255)
the note number chart is on page 12 of the open interface manual
"""
# set the song
self.setSong(songNumber, [(noteNumber,duration)])
self.playSongNumber(songNumber)
def _getLower5Bits( self, r ):
""" r is one byte as an integer """
return [ _bitOfByte(4,r), _bitOfByte(3,r), _bitOfByte(2,r), _bitOfByte(1,r), _bitOfByte(0,r) ]
def _getOneBit( self, r ):
""" r is one byte as an integer """
if r == 1: return 1
else: return 0
def _getOneByteUnsigned( self, r ):
""" r is one byte as an integer """
return r
def _getOneByteSigned( self, r ):
""" r is one byte as a signed integer """
return _twosComplementInt1byte( r )
def _getTwoBytesSigned( self, r1, r2 ):
""" r1, r2 are two bytes as a signed integer """
return _twosComplementInt2bytes( r1, r2 )
def _getTwoBytesUnsigned( self, r1, r2 ):
""" r1, r2 are two bytes as an unsigned integer """
return r1 << 8 | r2
def _getButtonBits( self, r ):
""" r is one byte as an integer """
return [ _bitOfByte(2,r), _bitOfByte(0,r) ]
def _setNextDataFrame(self):
""" This function _asks_ the robot to collect ALL of
the sensor data into the next packet to send back.
"""
self._write( SENSORS )
self._write( chr(6) )
def _getNextDataFrame(self):
""" This function then gets back ALL of
the sensor data and organizes it into the sensor
dictionary, sensord.
"""
r = self.ser.read(size=52)
r = [ ord(c) for c in r ]
#return self._readSensorList(r)
def _rawSend( self, listofints ):
for x in listofints:
self._write( chr(x) )
def _rawRecv( self ):
nBytesWaiting = self.ser.inWaiting()
#print 'nBytesWaiting is', nBytesWaiting
r = self.ser.read(size=nBytesWaiting)
r = [ ord(x) for x in r ]
#print 'r is', r
return r
def _rawRecvStr( self ):
nBytesWaiting = self.ser.inWaiting()
#print 'nBytesWaiting is', nBytesWaiting
r = self.ser.read(size=nBytesWaiting)
return r
def sensors( self, list_of_sensors_to_poll=6 ):
""" this function updates the robot's currently maintained
state of its robot sensors for those sensors requested
If none are requested, then all of the sensors are updated
(which takes a bit more time...)
"""
if type(list_of_sensors_to_poll) == type([]):
# first, we change any pieces of sensor values to
# the single digit that is required here
distangle = 0
if POSE in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(POSE)
# should check if they're already there
list_of_sensors_to_poll.append(DISTANCE)
list_of_sensors_to_poll.append(ANGLE)
if LEFT_BUMP in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(LEFT_BUMP)
if BUMPS_AND_WHEEL_DROPS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(BUMPS_AND_WHEEL_DROPS)
if RIGHT_BUMP in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(RIGHT_BUMP)
if BUMPS_AND_WHEEL_DROPS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(BUMPS_AND_WHEEL_DROPS)
if RIGHT_WHEEL_DROP in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(RIGHT_WHEEL_DROP)
if BUMPS_AND_WHEEL_DROPS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(BUMPS_AND_WHEEL_DROPS)
if LEFT_WHEEL_DROP in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(LEFT_WHEEL_DROP)
if BUMPS_AND_WHEEL_DROPS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(BUMPS_AND_WHEEL_DROPS)
if CENTER_WHEEL_DROP in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(CENTER_WHEEL_DROP)
if BUMPS_AND_WHEEL_DROPS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(BUMPS_AND_WHEEL_DROPS)
if LEFT_WHEEL_OVERCURRENT in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(LEFT_WHEEL_OVERCURRENT)
if LSD_AND_OVERCURRENTS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(LSD_AND_OVERCURRENTS)
if RIGHT_WHEEL_OVERCURRENT in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(RIGHT_WHEEL_OVERCURRENT)
if LSD_AND_OVERCURRENTS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(LSD_AND_OVERCURRENTS)
if ADVANCE_BUTTON in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(ADVANCE_BUTTON)
if BUTTONS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(BUTTONS)
if PLAY_BUTTON in list_of_sensors_to_poll:
list_of_sensors_to_poll.remove(PLAY_BUTTON)
if BUTTONS not in list_of_sensors_to_poll:
list_of_sensors_to_poll.append(BUTTONS)
r = self._getRawSensorDataAsList(list_of_sensors_to_poll)
else:
# if it's an integer, its a frame number
r = self._getRawSensorFrameAsList( list_of_sensors_to_poll )
# now, we set list_of_sensors_to_poll
frameNumber = list_of_sensors_to_poll
if frameNumber == 0:
list_of_sensors_to_poll = list(range(7,27))
elif frameNumber == 1:
list_of_sensors_to_poll = list(range(7,17))
elif frameNumber == 2:
list_of_sensors_to_poll = list(range(17,21))
elif frameNumber == 3:
list_of_sensors_to_poll = list(range(21,27))
elif frameNumber == 4:
list_of_sensors_to_poll = list(range(27,35))
elif frameNumber == 5:
list_of_sensors_to_poll = list(range(35,43))
else:
list_of_sensors_to_poll = list(range(7,43))
# change our dictionary
self._readSensorList(list_of_sensors_to_poll, r)
return self.sensord
def printSensors(self):
""" convenience function to show sensed data in d
if d is None, the current self.sensord is used instead
"""
self.sensors([LEFT_BUMP,RIGHT_BUMP,LEFT_WHEEL_DROP,RIGHT_WHEEL_DROP,CENTER_WHEEL_DROP,WALL_IR_SENSOR,CLIFF_LEFT,CLIFF_FRONT_LEFT,CLIFF_FRONT_RIGHT,CLIFF_RIGHT,VIRTUAL_WALL,LEFT_WHEEL_OVERCURRENT,RIGHT_WHEEL_OVERCURRENT,INFRARED_BYTE,PLAY_BUTTON,ADVANCE_BUTTON,POSE,CHARGING_STATE,VOLTAGE,CURRENT,BATTERY_TEMP,BATTERY_CHARGE,BATTERY_CAPACITY,WALL_SIGNAL,CLIFF_LEFT_SIGNAL,CLIFF_FRONT_LEFT_SIGNAL,CLIFF_FRONT_RIGHT_SIGNAL,CLIFF_RIGHT_SIGNAL,OI_MODE,SONG_NUMBER,SONG_PLAYING,CHARGING_SOURCES_AVAILABLE, ENCODER_LEFT, ENCODER_RIGHT,LIGHTBUMP_LEFT,LIGHTBUMP_FRONT_LEFT,LIGHTBUMP_CENTER_LEFT,LIGHTBUMP_CENTER_RIGHT,LIGHTBUMP_FRONT_RIGHT,LIGHTBUMP_RIGHT,LIGHTBUMP])
d = self.sensord
pose = d[POSE]
print(' LEFT_BUMP:', d[LEFT_BUMP])
print(' RIGHT_BUMP:', d[RIGHT_BUMP])
print(' LEFT_WHEEL_DROP:', d[LEFT_WHEEL_DROP])
print(' RIGHT_WHEEL_DROP:', d[RIGHT_WHEEL_DROP])
print(' CENTER_WHEEL_DROP:', d[CENTER_WHEEL_DROP])
print(' WALL_IR_SENSOR:', d[WALL_IR_SENSOR])
print(' CLIFF_LEFT:', d[CLIFF_LEFT])
print(' CLIFF_FRONT_LEFT:', d[CLIFF_FRONT_LEFT])
print(' CLIFF_FRONT_RIGHT:', d[CLIFF_FRONT_RIGHT])
print(' CLIFF_RIGHT:', d[CLIFF_RIGHT])
print(' VIRTUAL_WALL:', d[VIRTUAL_WALL])
print(' LEFT_WHEEL_OVERCURRENT:', d[LEFT_WHEEL_OVERCURRENT])
print(' RIGHT_WHEEL_OVERCURRENT:', d[RIGHT_WHEEL_OVERCURRENT])
print(' INFRARED_BYTE:', d[INFRARED_BYTE])
print(' PLAY_BUTTON:', d[PLAY_BUTTON])
print(' ADVANCE_BUTTON:', d[ADVANCE_BUTTON])
print(' POSE X (cm):', pose[0])
print(' POSE Y (cm):', pose[1])
print(' POSE TH (deg):', pose[2])
print(' CHARGING_STATE:', d[CHARGING_STATE])
print(' VOLTAGE:', d[VOLTAGE])
print(' CURRENT:', d[CURRENT])
print(' BATTERY_TEMP:', d[BATTERY_TEMP])
print(' BATTERY_CHARGE:', d[BATTERY_CHARGE])
print(' BATTERY_CAPACITY:', d[BATTERY_CAPACITY])
print(' WALL_SIGNAL:', d[WALL_SIGNAL])
print(' CLIFF_LEFT_SIGNAL:', d[CLIFF_LEFT_SIGNAL])
print(' CLIFF_FRONT_LEFT_SIGNAL:', d[CLIFF_FRONT_LEFT_SIGNAL])
print(' CLIFF_FRONT_RIGHT_SIGNAL:', d[CLIFF_FRONT_RIGHT_SIGNAL])
print(' CLIFF_RIGHT_SIGNAL:', d[CLIFF_RIGHT_SIGNAL])
print(' OI_MODE:', d[OI_MODE])
print(' SONG_NUMBER:', d[SONG_NUMBER])
print(' SONG_PLAYING:', d[SONG_PLAYING])
print(' ENCODER_LEFT:', d[ENCODER_LEFT])
print(' ENCODER_RIGHT:', d[ENCODER_RIGHT])
print(' LIGHTBUMP:', d[LIGHTBUMP])
print(' LIGHTBUMP_LEFT:', d[LIGHTBUMP_LEFT])
print(' LIGHTBUMP_FRONT_LEFT:', d[LIGHTBUMP_FRONT_LEFT])
print(' LIGHTBUMP_CENTER_LEFT:', d[LIGHTBUMP_CENTER_LEFT])
print(' LIGHTBUMP_CENTER_RIGHT:', d[LIGHTBUMP_CENTER_RIGHT])
print(' LIGHTBUMP_CENTER_RIGHT:', d[LIGHTBUMP_CENTER_RIGHT])
print(' LIGHTBUMP_RIGHT:', d[LIGHTBUMP_RIGHT])
print(' CHARGING_SOURCES_AVAILABLE:', d[CHARGING_SOURCES_AVAILABLE])
return d
def _readSensorList(self, sensor_data_list, r):
""" this returns the latest values from the particular
sensors requested in the listofvalues
"""
if len(sensor_data_list) == 0:
print('No data was read in _readSensorList.')
return self.sensord
sensorDataInterpreter = [ None, # 0
None, # 1
None, # 2
None, # 3
None, # 4
None, # 5
None, # 6
self._getLower5Bits, # 7 BUMPS_AND_WHEEL_DROPS
self._getOneBit, # 8 WALL_IR_SENSOR
self._getOneBit, # 9 CLIFF_LEFT = 9
self._getOneBit, # 10 CLIFF_FRONT_LEFT = 10
self._getOneBit, # 11 CLIFF_FRONT_RIGHT = 11
self._getOneBit, # 12 CLIFF_RIGHT = 12
self._getOneBit, # 13 VIRTUAL_WALL
self._getLower5Bits, # 14 LSD_AND_OVERCURRENTS
self._getOneBit, # 15 DIRT_DETECTED
self._getOneBit, # 16 unused
self._getOneByteUnsigned, # 17 INFRARED_BYTE
self._getButtonBits, # 18 BUTTONS
self._getTwoBytesSigned, # 19 DISTANCE
self._getTwoBytesSigned, # 20 ANGLE
self._getOneByteUnsigned, # 21 CHARGING_STATE
self._getTwoBytesUnsigned, # 22 VOLTAGE
self._getTwoBytesSigned, # 23 CURRENT
self._getOneByteSigned, # 24 BATTERY_TEMP
self._getTwoBytesUnsigned, # 25 BATTERY_CHARGE
self._getTwoBytesUnsigned, # 26 BATTERY_CAPACITY
self._getTwoBytesUnsigned, # 27 WALL_SIGNAL
self._getTwoBytesUnsigned, # 28 CLIFF_LEFT_SIGNAL
self._getTwoBytesUnsigned, # 29 CLIFF_FRONT_LEFT_SIGNAL
self._getTwoBytesUnsigned, # 30 CLIFF_FRONT_RIGHT_SIGNAL
self._getTwoBytesUnsigned, # 31 CLIFF_RIGHT_SIGNAL
self._getLower5Bits, # 32 CARGO_BAY_DIGITAL_INPUTS
self._getTwoBytesUnsigned, # 33 CARGO_BAY_ANALOG_SIGNAL
self._getOneByteUnsigned, # 34 CHARGING_SOURCES_AVAILABLE
self._getOneByteUnsigned, # 35 OI_MODE
self._getOneByteUnsigned, # 36 SONG_NUMBER
self._getOneByteUnsigned, # 37 SONG_PLAYING
self._getOneByteUnsigned, # 38 NUM_STREAM_PACKETS
self._getTwoBytesSigned, # 39 REQUESTED_VELOCITY
self._getTwoBytesSigned, # 40 REQUESTED_RADIUS
self._getTwoBytesSigned, # 41 REQUESTED_RIGHT_VELOCITY
self._getTwoBytesSigned, # 42 REQUESTED_LEFT_VELOCITY
self._getTwoBytesUnsigned, # 43 Left Encoder Counts
self._getTwoBytesUnsigned, # 44 Right Encoder Counts
self._getOneByteUnsigned, # 45 LIGH_BUMP
self._getTwoBytesUnsigned, # 46 LIGHTBUMP_LEFT
self._getTwoBytesUnsigned, # 47 LIGHTBUMP_FRONT_LEFT
self._getTwoBytesUnsigned, # 48 LIGHTBUMP_CENTER_LEFT
self._getTwoBytesUnsigned, # 49 LIGHTBUMP_CENTER_RIGHT
self._getTwoBytesUnsigned, # 50 LIGHTBUMP_FRONT_RIGHT
self._getTwoBytesUnsigned, # 51 LIGHTBUMP_RIGHT
None # only 51 as of right now
]
startofdata = 0
distance = 0
angle = 0
update_pose = False
for sensorNum in sensor_data_list:
width = SENSOR_DATA_WIDTH[sensorNum]
dataGetter = sensorDataInterpreter[sensorNum]
interpretedData = 0
if (width == 1):
if startofdata >= len(r):
print("Incomplete Sensor Packet")
break
else: interpretedData = dataGetter(r[startofdata])
if (width == 2):
if startofdata >= len(r) - 1:
print("Incomplete Sensor Packet")
break
else: interpretedData = dataGetter(r[startofdata], r[startofdata+1] )
# add to our dictionary
self.sensord[sensorNum] = interpretedData
# POSE = 100 - later
#LEFT_BUMP = 101
#RIGHT_BUMP = 102
#LEFT_WHEEL_DROP = 103
#RIGHT_WHEEL_DROP = 104
#CENTER_WHEEL_DROP = 105
if sensorNum == BUMPS_AND_WHEEL_DROPS:
self.sensord[CENTER_WHEEL_DROP] = interpretedData[0]
self.sensord[LEFT_WHEEL_DROP] = interpretedData[1]
self.sensord[RIGHT_WHEEL_DROP] = interpretedData[2]
self.sensord[LEFT_BUMP] = interpretedData[3]
self.sensord[RIGHT_BUMP] = interpretedData[4]
#LEFT_WHEEL_OVERCURRENT = 106
#RIGHT_WHEEL_OVERCURRENT = 107
if sensorNum == LSD_AND_OVERCURRENTS:
self.sensord[LEFT_WHEEL_OVERCURRENT] = interpretedData[0]
self.sensord[RIGHT_WHEEL_OVERCURRENT] = interpretedData[1]
#ADVANCE_BUTTON = 108
#PLAY_BUTTON = 109
if sensorNum == BUTTONS:
self.sensord[ADVANCE_BUTTON] = interpretedData[0]
self.sensord[PLAY_BUTTON] = interpretedData[1]
if sensorNum == DIRT_DETECTED:
self.sensord[DIRT_DETECTED] = interpretedData
# handle special cases
if (sensorNum == DISTANCE):
distance = interpretedData
if (sensorNum == ANGLE):
angle = interpretedData
if (sensorNum == ENCODER_LEFT):
self.leftEncoder = interpretedData
update_pose = True
if (sensorNum == ENCODER_RIGHT):
self.rightEncoder = interpretedData
update_pose = True
#resultingValues.append(interpretedData)
# update index for next sensor...
startofdata = startofdata + width
#if (distance != 0 or angle != 0):
# self._integrateNextOdometricStepCreate(distance,angle)
if update_pose == True:
self._integrateNextEncoderStep()
self.sensord[POSE] = self.getPose(dist='cm',angle='deg')
def toFullMode(self):
""" changes the state to FULL_MODE
"""
self._start()
time.sleep(0.03)
self.toSafeMode()
time.sleep(0.03)
self._write( FULL )
time.sleep(0.03)
self.sciMode = FULL_MODE
return
def toSafeMode(self):
""" changes the state (from PASSIVE_MODE or FULL_MODE)
to SAFE_MODE
"""
self._start()
time.sleep(0.03)
# now we're in PASSIVE_MODE, so we repeat the above code...
self._write( SAFE )
# they recommend 20 ms between mode-changing commands
time.sleep(0.03)
# change the mode we think we're in...
self.sciMode = SAFE_MODE
# no response here, so we don't get any...
return
def getMode(self):
""" returns one of OFF_MODE, PASSIVE_MODE, SAFE_MODE, FULL_MODE """
# but how right is it?
return self.sciMode
def _setBaudRate(self, baudrate=10):
""" sets the communications rate to the desired value """
# check for OK value
#baudcode = 10 # 57600, the default
if baudrate == 300: baudcode = 0
elif baudrate == 600: baudcode = 1
elif baudrate == 1200: baudcode = 2
elif baudrate == 2400: baudcode = 3
elif baudrate == 4800: baudcode = 4
elif baudrate == 9600: baudcode = 5
elif baudrate == 14400: baudcode = 6
elif baudrate == 19200: baudcode = 7
elif baudrate == 28800: baudcode = 8
elif baudrate == 38400: baudcode = 9
elif baudrate == 57600: baudcode = 10
elif baudrate == 115200: baudcode = 11
else:
print('The baudrate of', baudrate, 'in _setBaudRate')
print('was not recognized. Not sending anything.')
return
# otherwise, send off the message
self._write( START )
self._write( chr(baudcode) )
# the recommended pause
time.sleep(0.1)
# change the mode we think we're in...
self.sciMode = PASSIVE_MODE
# no response here, so we don't get any...
return
# Some new stuff added by Sean
def _startScript(self, number_of_bytes):
self._write( SCRIPT )
self._write( chr(number_of_bytes) )
return
def _endScript(self, timeout=-1.0):
# issue the ENDSCRIPT command to start the script
self._write( ENDSCRIPT )
interval = 1.0
total = 0.0
# strip out all existing crap
while(self.ser.read(8192) != ''):
continue
# poll
while(timeout<0.0 or total < timeout):
self._write(SENSORS)
self._write(chr(7)) # smallest packet value that I can tell
if self.ser.read(1) != '':
break
time.sleep(interval - 0.5)
total = total + interval
# strip out again, we buffered up lots of junk
while(self.ser.read(8192) != ''):
continue
def _waitForDistance(self, distance_mm):
self._write(WAITDIST)
leftHighVal, leftLowVal = _toTwosComplement2Bytes( distance_mm )
self._write( chr(leftHighVal) )
self._write( chr(leftLowVal) )
return
def _waitForAngle(self, angle_deg):
self._write(WAITANGLE)
leftHighVal, leftLowVal = _toTwosComplement2Bytes( angle_deg )
self._write( chr(leftHighVal) )
self._write( chr(leftLowVal) )
return
def turn(self, angle_deg, deg_per_sec=20):
if angle_deg==0:
return
if deg_per_sec==0:
deg_per_sec=20
if (angle_deg < 0 and deg_per_sec > 0) or (angle_deg > 0 and deg_per_sec < 0):
deg_per_sec = 0 - deg_per_sec
self._startScript(13)
self.go(0, deg_per_sec)
self._waitForAngle(angle_deg)
self.stop()
self._endScript()
#self.sensors([POSE]) # updated by Sean
def move(self, distance_cm, cm_per_sec=10):
if distance_cm==0:
return
if cm_per_sec==0:
cm_per_sec=10
if (distance_cm < 0 and cm_per_sec > 0) or (distance_cm > 0 and cm_per_sec < 0):
cm_per_sec = 0 - cm_per_sec
self._startScript(13)
self.go(cm_per_sec, 0)
self._waitForDistance(distance_cm*10)
self.stop()
self._endScript()
#self.sensors([POSE]) # updated by Sean
# James' syntactic sugar/kludgebox
def senseFunc(self, sensorName):
"""Returns a function which, when called, updates and returns
information for a specified sensor (sensorName).
e.g. cliffState = robot.senseFunc(create.CLIFF_FRONT_LEFT_SIGNAL)
info = cliffState()
No added functionality, just nicer to look at."""
f = lambda: self.sensors([sensorName])[sensorName]
return f
def sleepTill(self, sensorFunc, comparison, value):
"""Have the robot continue what it's doing until some halting
criterion is met, determined by a repeated polling of
sensorFunc until it's comparison (a function) to value is true.
e.g. greater = lambda a,b: a > b
bumpSense = robot.sensorFunc(create.LEFT_BUMP)
robot.go(10)
robot.sleepTill(bumpSense, greater, 0)
This will have the robot go until the left bump sensor is pushed.
"""
while (not comparison(sensorFunc(), value)):
time.sleep(0.05)
# Some new stuff added by PaperPieceCode
#Change Time of Roomba internal Clock to System-Time over Serial Port
def change_Time():
self.write(START)
self.write(CHANGE_TIME)
weekdays = ['Sunday', 'Monday', 'Tuesday', 'Wednesday', 'Thursday', 'Friday', 'Saturday']
i = 0
now = datetime.datetime.now()
for day in weekdays:
if datetime.datetime.today().strftime('%A') == day: daycode = chr(i)
i += 1
self.write(daycode)
self.write(chr(now.hour))
self.write(chr(now.minute))
コメント