def __init__(self):
# USBポートを使い、シリアル通信開始
# 送信用と受信用で2つポートを使う
self.ev3_recv_serial = serial.Serial('/dev/ttyUSB0', 115200, timeout=10)
self.ev3_send_serial = serial.Serial('/dev/ttyUSB1', 115200, timeout=10)
INFO(self.ev3_recv_serial.portstr)
INFO(self.ev3_send_serial.portstr)
def message_received(client, server, message):
DEBUG('Message has been received from ' + str(client['address'][0]) + ':' +
str(client['address'][1]))
try:
message_dict = json.loads(message)
if message_dict['type'] == 'guide_info':
try:
with open('./guide_info.json', mode='w') as f:
f.write(message)
except AttributeError:
INFO('faild to get color data')
except json.JSONDecodeError:
INFO('faild to decode message to json')
def calcAndSendMotorPowers(self, shmem, serial):
while 1:
# ボール状態取得
ballState = self.getBallStateByTouch(shmem.isTouched)
DEBUG('ballState = ', ballState)
# モータ値計算
motorPowers = self.calcMotorPowers(ballState, shmem)
# モーター値後処理(現在は首振り検知処理のみ)
motorPowers = self.motorControlPostProcessor.escapeSwing(motorPowers)
# 設定ファイルの内容を反映
setting = self.getSetting()
if setting != 'NONE':
motorPowers = MotorController.DIC_SETTING_TO_MOTOR_VALUE.get(setting)
# モータ値を正常値にまるめる
motorPowers = self.roundOffMotorSpeeds(motorPowers)
# 送信伝文生成
sendText = str(int(motorPowers[0])) + "," + str(int(motorPowers[1])) + "\n"
# モータ値送信
serial.write(sendText)
# 0.05sごとに実行
# 本当は全力でぶん回したい
# ラズパイ側はマルチプロセスを採用しているので問題ないと思うが
# EV3側は計算資源を通信に占有される恐れがあるためとりあえず0.05sにしておく
INFO('ball=' + str(ballState).rjust(15),
'motor=' + str(motorPowers[0]).rjust(4) + ',' + str(motorPowers[1]).rjust(4),
'IR=' + str(shmem.irAngle).rjust(4),
'touch=' + str(shmem.isTouched).rjust(4),
'enemy=' + str(shmem.enemyGoalAngle).rjust(4) + ',' + str(shmem.enemyGoalDis).rjust(4),
'my=' + str(shmem.myGoalAngle).rjust(4) + ',' + str(shmem.myGoalDis).rjust(4),
'center=' + str(shmem.fieldCenterAngle).rjust(4) + ',' + str(shmem.fieldCenterDis).rjust(4),
)
time.sleep(0.1)
def getParameterSetting(self):
# パラメータ値設定ファイルを読み込み
config = ConfigParser.SafeConfigParser()
config.read(MotorController.PARAMETER_INI_PATH)
self.DEBUG_SHOOT_ALGORITHM = int(config.get(MotorController.PARAMETER_CUSTOM_SECTION, 'shoot_algo'))
self.DEBUG_CHASE_ALGORITHM = int(config.get(MotorController.PARAMETER_CUSTOM_SECTION, 'chase_algo'))
MotorController.SPEED_SHOOT = int(config.get(MotorController.PARAMETER_CUSTOM_SECTION, 'shoot_speed'))
MotorController.K_SHOOT_ANGLE = float(config.get(MotorController.PARAMETER_CUSTOM_SECTION, 'k_shoot_angle'))
MotorController.SPEED_CHASE = int(config.get(MotorController.PARAMETER_CUSTOM_SECTION, 'chase_speed'))
MotorController.K_CHASE_ANGLE = float(config.get(MotorController.PARAMETER_CUSTOM_SECTION, 'k_chase_angle'))
MotorController.SPEED_GO_CENTER = int(config.get(MotorController.PARAMETER_CUSTOM_SECTION, 'center_speed'))
MotorController.K_GO_CENTER_ANGLE = float(config.get(MotorController.PARAMETER_CUSTOM_SECTION, 'k_center_angle'))
INFO('------- motor control parameter read -------')
INFO('shoot_algo=' + str(self.DEBUG_SHOOT_ALGORITHM).rjust(3) + ',',
'chase_algo=' + str(self.DEBUG_CHASE_ALGORITHM).rjust(3) + ',',
'shoot=' + str(MotorController.SPEED_SHOOT).rjust(4) + ',' + str(MotorController.K_SHOOT_ANGLE).rjust(4) + ',',
'chase=' + str(MotorController.K_CHASE_ANGLE).rjust(4) + ',' + str(MotorController.K_CHASE_ANGLE).rjust(4) + ',',
'center=' + str(MotorController.SPEED_GO_CENTER).rjust(4) + ',' + str(MotorController.K_GO_CENTER_ANGLE).rjust(4)
)
def is_going_into_corner(self, wall_size):
if wall_size == -1:
self._is_near_wall = False
return False
else:
if self._is_near_wall:
if time.time() - self._near_wall_start_time > MotorController.TIME_WALL_STUCK:
INFO('### going into corner now')
return True
return False
else:
DEBUG('### detect near wall start')
self._near_wall_start_time = time.time()
self._is_near_wall = True
return False
def escape_from_corner(self, shmem):
shmem.soundPhase = SoundPhaseE.DETECT_PRESS_WALL
INFO('### escape from corner')
self.left_motor.drive(-MotorController.SPEED_BACK)
self.right_motor.drive(-MotorController.SPEED_BACK)
time.sleep(2)
if shmem.wallX > 0:
self.left_motor.drive(-100)
self.right_motor.drive(100)
else:
self.left_motor.drive(100)
self.right_motor.drive(-100)
time.sleep(1)
self.left_motor.drive(MotorController.SPEED_CHASE)
self.right_motor.drive(MotorController.SPEED_CHASE)
time.sleep(3)
def update_loop(self, shmem):
while 1:
# どうやら10-20秒くらいで補正値を更新しないと、ドリフトの影響が無視出来なくなる
# リスタート準備中フラグが立っていたら、他のモジュールが待っていてくれているので、補正値の調整をする
# 補正が終わったらフラグをクリアして、他のモジュールに調整完了を知らせる
if shmem.preparingRestart:
INFO('calibrate IMU to prepare restart')
self.calibrate()
shmem.preparingRestart = False
gyro = self._mpu9250.readGyro()
self._degree += gyro["z"] - self._epsilon
# 画像認識は右側正、左側負なので、そちらの挙動と合わせるために符号反転させる
shmem.bodyAngle = -int(
self.calc_shortcut_degree(self._degree, self.ONE_ROUND_VALUE))
DEBUG('bodyAngle = ', shmem.bodyAngle)
time.sleep(self._update_interval_sec)
def setEnemyGoalColorFromFile(self):
# ゴールモード設定ファイルを読み込み
try:
with open('/home/pi/Desktop/raspi_FcTronto/webiopi/goal_mode.txt'
) as f:
self.goal_mode = f.read()
# ファイル読み込み失敗
except:
WARN('goal mode file read failure')
# 前回読み込み時から変更があれば設定
if self.goal_mode != self.pre_goal_mode:
if self.goal_mode == 'yellow':
self.enemy_goal_color = EnemyGoalColorE.YELLOW
elif self.goal_mode == 'blue':
self.enemy_goal_color = EnemyGoalColorE.BLUE
else:
WARN('invalid goal mode')
INFO('goal_mode changed:', self.pre_goal_mode, '->',
self.goal_mode)
# ゴールモードを記憶
self.pre_goal_mode = self.goal_mode
def __escape_press_wall(self, motorPowers, bodyAngle):
# すでにバック中の場合
if self._pw_enable_force_back:
if (time.time() - self._pw_start_back_time) > self._pw_back_time:
DEBUG('### END back for escape press wall')
self._pw_enable_force_back = False
# 壁押し付け状態判定用変数初期化
self._pw_error_count_now = 0
TRACE('### CONTINUE back for escape press wall')
return -self._pw_speed, -self._pw_speed
# 旋回しようとしているかどうか
# TODO: このパラメータも上位側で指定可能にする
if abs(self._pw_pre_power[0] - self._pw_pre_power[1]) > 50 and abs(self._pw_pre_body_angle - bodyAngle) < self._pw_diff_angle:
DEBUG('### pressing wall count +1')
self._pw_error_count_now += 1
else:
self._pw_error_count_now = 0
if self._pw_error_count_now >= self._pw_error_count_limit:
INFO('### detect pressing wall count')
self._pw_enable_force_back = True
self._pw_start_back_time = time.time()
# 別にこの時はバックしなくてもいいかもしれないが一応
return -self._pw_speed, -self._pw_speed
return motorPowers
def calcAndSendMotorPowers(self, shmem):
self.chaseBallMode.set_mode(ChaseMode.NORMAL)
while 1:
if self.motion_status == MotionStateE.CHASE_BALL:
# ボール捕獲に移る
if 100 < shmem.ballDis < 160 and -20 < shmem.ballAngle < 20:
INFO('capture ball')
self.left_motor.drive(0)
self.right_motor.drive(0)
self.servo.down()
time.sleep(1)
self.motion_status = MotionStateE.GO_TO_STATION
elif self.motion_status == MotionStateE.GO_TO_STATION:
distanceSensorValue = self.distanceSensor.read()
DEBUG('distanceSensor = ' + str(distanceSensorValue))
# ボール消失してる
if distanceSensorValue > MotorController.DISTANCE_HAVE_BALL:
INFO('lost ball')
self.left_motor.drive(0)
self.right_motor.drive(0)
self.servo.up()
# ステーションが見えている場合はステーションにボールを渡せたと判断して再スタートする
if shmem.stationDis != -1:
shmem.soundPhase = SoundPhaseE.DONE
INFO('lost ball -> in station')
self.chaseBallMode.set_mode(ChaseMode.NORMAL)
self.motion_status = MotionStateE.PREPARE_RESTART
self.motion_status = MotionStateE.CHASE_BALL
# TODO: ステーション到着後の動き
if 200 < shmem.stationDis < 310:
INFO('reached station')
self.left_motor.drive(0)
self.right_motor.drive(0)
self.servo.up()
# ステーションに向かっている間に追跡モードがどうなっているか分からないので、通常にリセットしておく
shmem.soundPhase = SoundPhaseE.DONE
self.chaseBallMode.set_mode(ChaseMode.NORMAL)
self.motion_status = MotionStateE.PREPARE_RESTART
elif self.motion_status == MotionStateE.PREPARE_RESTART:
INFO('PREPARE RESTART start')
self.left_motor.drive(0)
self.right_motor.drive(0)
shmem.preparingRestart = True
shmem.soundPhase = SoundPhaseE.PREPARE_RESTART
while(shmem.preparingRestart):
time.sleep(0.1)
self.prepare_restart(shmem)
self.chaseBallMode.set_mode(ChaseMode.NORMAL)
INFO('PREPARE RESTART end -> CHASE BALL')
self.motion_status = MotionStateE.CHASE_BALL
continue
# モータ値計算
motorPowers = self.calcMotorPowers(shmem, self.motion_status)
# モーター値後処理(現在は首振り検知処理のみ)
# motorPowers = self.motorControlPostProcessor.escapeSwing(motorPowers)
# ステーションに戻るとき隅っこにハマる場合があるので、その時は中央に戻ることを試みる
if self.motion_status == MotionStateE.GO_TO_STATION and self.is_going_into_corner(shmem.wallSize):
self.escape_from_corner(shmem)
motorPowers = self.motorControlPostProcessor.run(motorPowers, shmem.bodyAngle / 10)
# ボール保持中じゃないのに前方近くに何かあったら後退して回避する
if self.servo.is_lifting() and self.distanceSensor.read() < MotorController.DISTANCE_STUCK:
INFO('### detect something barrier')
motorPowers = (-MotorController.SPEED_BACK, -MotorController.SPEED_BACK)
# モータ値を正常値にまるめる
motorPowers = self.roundOffMotorSpeeds(motorPowers)
# モータ値送信
self.left_motor.drive(motorPowers[0])
self.right_motor.drive(motorPowers[1])
# とりあえず一定時間間隔で動かす
INFO('motor r=' + str(motorPowers[0]).rjust(4) + ', l=' + str(motorPowers[1]).rjust(4),
',ball angle, distance=' + str(shmem.ballAngle).rjust(4) + ',' + str(shmem.ballDis).rjust(4),
',station angle, distance=' + str(shmem.stationAngle).rjust(4) + ',' + str(shmem.stationDis).rjust(4),
',body angle=' + str(shmem.bodyAngle / 10).rjust(4),
)
time.sleep(0.1)
def info(title):
INFO(title)
INFO('module name:', __name__)
INFO('parent process:', os.getppid())
INFO('process id:', os.getpid())
def imageProcessingFrame(self, frame, shmem):
# HSV色空間に変換
hsv_img = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
cv2.line(frame, (self.CAMERA_CENTER_CX, 0),
(self.CAMERA_CENTER_CX, 480), (0, 0, 255),
thickness=1)
cv2.line(frame, (0, self.CAMERA_CENTER_CY),
(480, self.CAMERA_CENTER_CY), (0, 0, 255),
thickness=1)
# 赤色領域の検知
red_cx, red_cy, red_area_size, red_convex = self.colorDetect2(
hsv_img, 'RED')
if red_cx > -1:
self.draw_marker(frame, red_cx, red_cy, (255, 30, 30))
# 黄色領域の検知
yellow_cx, yellow_cy, yellow_area_size, yellow_convex = self.colorDetect2(
hsv_img, 'YELLOW')
if yellow_cx > -1:
self.draw_marker(frame, yellow_cx, yellow_cy, (30, 255, 255))
# 青色領域の検知
blue_cx, blue_cy, blue_area_size, blue_convex = self.colorDetect2(
hsv_img, 'BLUE')
if blue_cx > -1:
self.draw_marker(frame, blue_cx, blue_cy, (30, 30, 255))
# 緑色領域の検知
#green_cx, green_cy, green_area_size, green_convex = self.colorDetect2(hsv_img, 'GREEN')
#if green_cx > -1:
# self.draw_marker(frame, green_cx, green_cy, (30, 255, 30))
# 黒色領域の検知
black_cx, black_cy, black_area_size, black_convex = self.colorDetect2(
hsv_img, 'BLACK')
if black_cx > -1:
self.draw_marker(frame, black_cx, black_cy, (0, 0, 0))
INFO('RedSize :' + str(red_area_size).rjust(8))
INFO('YellowSize :' + str(yellow_area_size).rjust(8))
INFO('BlueSize :' + str(blue_area_size).rjust(8))
#INFO('GreenSize :' + str(green_area_size).rjust(8))
INFO('BlackSize :' + str(black_area_size).rjust(8))
# 認識できた部分の面積が小さい場合は結果を無視し、distanceに不正な値を入れる
if red_area_size > blue_area_size and red_area_size > self.IGNORE_AREA_SIZE_BALL:
DEBUG('use RED area')
shmem.soundPhase = SoundPhaseE.DETECT_RED_BALL
ball_angle, ball_distance = self.calcBallDirection(red_cx, red_cy)
elif blue_area_size > self.IGNORE_AREA_SIZE_BALL:
DEBUG('use BLUE area')
shmem.soundPhase = SoundPhaseE.DETECT_BLUE_BALL
ball_angle, ball_distance = self.calcBallDirection(
blue_cx, blue_cy)
#elif yellow_area_size > self.IGNORE_AREA_SIZE_BALL:
# DEBUG('use YELLOW area')
# shmem.soundPhase = SoundPhaseE.DETECT_YELLOW_BALL
# ball_angle, ball_distance = self.calcBallDirection(yellow_cx, yellow_cy)
else:
ball_angle = 0
ball_distance = -1
#if green_area_size > self.IGNORE_AREA_SIZE_GREEN:
# station_angle, station_distance = self.calcBallDirection(green_cx, green_cy)
if yellow_area_size > self.IGNORE_AREA_SIZE_YELLOW:
station_angle, station_distance = self.calcBallDirection(
yellow_cx, yellow_cy)
else:
station_angle = 0
station_distance = -1
if black_area_size > self.IGNORE_AREA_SIZE_WALL:
wall_x = black_cx - self.CAMERA_CENTER_CX
wall_size = black_area_size
else:
wall_x = 0
wall_size = -1
return ball_angle, ball_distance, station_angle, station_distance, wall_x, wall_size
def new_client(client, server):
INFO('New client ' + str(client['address'][0]) + ':' +
str(client['address'][1]) + ' has joined.')
def client_left(client, server):
INFO('Client ' + str(client['address'][0]) + ':' +
str(client['address'][1]) + ' has left.')