def roundOffMotorSpeeds(self, motorSpeeds):
# モータ値それぞれについて絶対値を100に丸めた時の差分
diffPower = [0, 0]
diffPower[0] = self.calcDiffRondOffWithin100(motorSpeeds[0])
diffPower[1] = self.calcDiffRondOffWithin100(motorSpeeds[1])
DEBUG('diffPower =', diffPower[0], ', ', diffPower[1])
returnSpeeds = self.roundOffWithin100(motorSpeeds[0]), self.roundOffWithin100(motorSpeeds[1])
# 両モータ絶対値が100を超えている場合は100に丸めるのみ
if diffPower == [0, 0]:
TRACE('both power abs over 100')
return returnSpeeds
# 左モータ値のみ絶対値が100を超える
elif diffPower[0] != 0:
TRACE('left power abs over 100')
# 右モータ値のみ差分分だけ調整する
return returnSpeeds[0], returnSpeeds[1] + diffPower[0]
# 右モータ値のみ絶対値が100を超える
elif diffPower[1] != 0:
TRACE('right power abs over 100')
# 左モータ値のみ差分分だけ調整する
return returnSpeeds[0] + diffPower[1], returnSpeeds[1]
# どちらも絶対値がを100を超えない
else:
TRACE('both power abs under 100')
return returnSpeeds
def escapeSwing(self, motorPowers):
# 首振り回避する必要がある場合
if self.isNeedToEscapeSwing(motorPowers):
TRACE('escape from swing')
# 首振り回避用のモータ値を返す
return MotorControlPostProcessor.SPEED_ESCAPE_FROM_SWING
# 首振り回避する必要がない場合
else:
TRACE('not escape from swing')
# 入力値をそのまま返す
return motorPowers
def receiveDataLoop(self, shmem):
while 1:
read_data = self.ev3_recv_serial.readline()
# 読み込んだ文字列を,区切りでリストに変換
sensorValues = read_data.strip().split(",")
# 共有メモリの値を更新
if(len(sensorValues) == 2):
if(self.is_int(sensorValues[0])):
shmem.irAngle = int(sensorValues[0])
if(self.is_int(sensorValues[1])):
shmem.isTouched = int(sensorValues[1])
TRACE('irAngle =', shmem.irAngle)
TRACE('isTouched =', shmem.isTouched)
def getBallStateByTouch(self, isTouched):
# タッチされてない
if isTouched == 0:
TRACE('ball state : not touched')
return BallStateE.NOT_HAVE_BALL
# タッチされてる
elif isTouched == 1:
TRACE('ball state : touched')
return BallStateE.HAVE_BALL
# 不正値
else:
WARN('ball state : invalid value (isTouched)')
return BallStateE.NOT_HAVE_BALL
def updateCountDerectionChangeDetectSwing(self, motorPowers):
# 方向転換した場合
if motorPowers[0] * self.pre_motor_powers[0] < 0 and motorPowers[1] * self.pre_motor_powers[1] < 0 and motorPowers[0] != motorPowers[1]:
# 前回方向転換検知時からの経過時間が閾値より小さかった場合
if (time.time() - self.pre_direction_change_time) > MotorControlPostProcessor.THRESHOLD_TIME_SWING_DETECT:
TRACE('count_direction_change_detect_swing : increment')
# 首振り検知用方向転換数をインクリメントする
self.count_direction_change_detect_swing += 1
# 前回方向転換検知時から十分時間が経過していた場合
else:
TRACE('count_direction_change_detect_swing : reset')
# 首振り検知用方向転換数をリセット
self.count_direction_change_detect_swing = 1
DEBUG('count_derection_change_detect_swing = ', self.count_direction_change_detect_swing)
def __init__(self):
TRACE('ImageProcessing generated')
# テーブルの読み込み
# TODO: テーブル情報は仮。csvファイルから読み込むようにする
self.GOAL_DISTANCE_TABLE = range(241)
self.enemy_goal_color = EnemyGoalColorE.YELLOW
self.goal_mode = 'none'
self.pre_goal_mode = 'none'
def __init__(self):
self._file_list = [
'./sound/finding_ball.mp3', './sound/detect_blue_ball.mp3',
'./sound/detect_red_ball.mp3', './sound/recv_camera_info.mp3',
'./sound/detect_station.mp3', './sound/prepare_restart.mp3',
'./sound/detect_pressing_wall.mp3',
'./sound/detect_yellow_ball.mp3', './sound/done.mp3'
]
TRACE('Sound generated')
def __init__(self, address, initial_pulse_length=390):
TRACE('Servo generated: address = ' + str(address))
self._address = address
self._pwm = Adafruit_PCA9685.PCA9685(address=address)
# Set frequency to 60hz, good for servos.
self._pwm.set_pwm_freq(60)
# Set initial pluse length
self._pwm.set_pwm(0, 0, initial_pulse_length)
self._current_pulse_length = initial_pulse_length
self._is_lifting = False
def calcMotorPowersByBallAngleAndDis(self, ballAngle, ballDis):
if self.DEBUG_CHASE_ALGORITHM == 0:
# ボールが正面にある場合
if ballAngle == 0:
TRACE('calcMotor patern : ballAngle = 0')
# 距離センサの値をボールまでの距離としてモータの値を計算
return self.getMotorPowerByDistance(ballDis)
# ボールが正面にない場合
else:
TRACE('calcMotor patern : boalAngle != 0')
# モータの値は補正をかける
speed = ballAngle / MotorController.CORRECTION_VALUE_BALL_ANGLE_TO_SPEED
# 絶対値が100を超える場合は100に丸める
#speed = self.roundOffWithin100(speed)
# debug
speed = speed / abs(speed) * 5
return (-speed), speed
if self.DEBUG_CHASE_ALGORITHM == 1:
# P制御
return MotorController.SPEED_CHASE - MotorController.K_CHASE_ANGLE * ballAngle, \
MotorController.SPEED_CHASE + MotorController.K_CHASE_ANGLE * ballAngle
def __init__(self):
# 前回モータ値
self.pre_motor_powers = 0, 0
# 前回方向転換時刻(首振り検知用)
self.pre_direction_change_time = time.time()
# 首振り検知用方向転換数
self.count_direction_change_detect_swing = 0
# 首振り回避状態
self.is_escaping_swing = False
# 首振り回避開始時刻
self.escape_swing_start_time = time.time()
TRACE('MotorControlPostProcessor generated')
def calcMotorPowersByBallAngle(self, ballAngle, speed, k):
if self.DEBUG_CHASE_ALGORITHM == 0:
# ボールが正面にある場合
if ballAngle == 0:
TRACE('calcMotor patern : ballAngle = 0')
# モータの値は固定値で前進
return MotorController.SPEED_FRONT_BALL, MotorController.SPEED_FRONT_BALL
# ボールが正面にない場合
else:
TRACE('calcMotor patern : boalAngle != 0')
# モータの値は補正をかける
speed = ballAngle / MotorController.CORRECTION_VALUE_BALL_ANGLE_TO_SPEED
# left, rightの順で返却
return speed, (-speed)
elif self.DEBUG_CHASE_ALGORITHM == 1:
# P制御
return MotorController.SPEED_CHASE + MotorController.K_CHASE_ANGLE * ballAngle, \
MotorController.SPEED_CHASE - MotorController.K_CHASE_ANGLE * ballAngle
else:
ERROR('Invalid Value : DEBUG_CHASE_ALGORITHM =', self.DEBUG_CHASE_ALGORITHM)
return MotorController.SPEED_STOP
def calcMotorPowersByBallAngle(self, ballAngle):
if self.DEBUG_CHASE_ALGORITHM == 0:
# ボールが正面にある場合
if ballAngle == 0:
TRACE('calcMotor patern : ballAngle = 0')
# モータの値は固定値で前進
return MotorController.SPEED_FRONT_BALL, MotorController.SPEED_FRONT_BALL
# ボールが正面にない場合
else:
TRACE('calcMotor patern : boalAngle != 0')
# モータの値は補正をかける
speed = ballAngle / MotorController.CORRECTION_VALUE_BALL_ANGLE_TO_SPEED
# 絶対値が100を超える場合は100に丸める
#speed = self.roundOffWithin100(speed)
return (-speed), speed
elif self.DEBUG_CHASE_ALGORITHM == 1:
# P制御
return MotorController.SPEED_CHASE - MotorController.K_CHASE_ANGLE * ballAngle, \
MotorController.SPEED_CHASE + MotorController.K_CHASE_ANGLE * ballAngle
else:
ERROR('Invalid Value : DEBUG_CHASE_ALGORITHM =', self.DEBUG_CHASE_ALGORITHM)
return MotorController.SPEED_STOP
def getSetting(self):
# モータ値設定ファイルを読み込み
try:
with open('/home/pi/Desktop/raspi_FcTronto/webiopi/motor_setting.txt') as f:
motorSetting = f.read()
if motorSetting != '':
DEBUG('motor setting = ', motorSetting)
return motorSetting
else:
TRACE('motor setting is NONE')
return 'NONE'
# ファイル読み込み失敗
except:
WARN('motor setting file read failure')
return 'NONE'
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 calcMotorPowers(self, shmem, motion_status):
# 現在の状態に応じて追跡対象を変える
if motion_status == MotionStateE.CHASE_BALL:
TRACE('motion_status = CHASE_BALL')
if shmem.ballDis != -1:
return self.calcMotorPowersByBallAngle(shmem.ballAngle, MotorController.SPEED_CHASE, MotorController.K_CHASE_ANGLE)
TRACE('shmem.ballDis is invalid value')
if self.chaseBallMode.now() == ChaseMode.NORMAL:
TRACE('chaseBallMode = NORMAL')
return self.calcMotorPowersByBallAngle(shmem.bodyAngle / 10, MotorController.SPEED_CHASE, MotorController.K_CHASE_ANGLE)
# ボールが視界になくて、首振りモードの時はボールを見つけるためにとりあえず旋回する
DEBUG('chaseBallMode = SWING')
return MotorController.SPEED_SWING_ROTATE
elif motion_status == MotionStateE.GO_TO_STATION:
TRACE('motion_status = GO_TO_STATION')
if shmem.stationDis != -1:
shmem.soundPhase = SoundPhaseE.DETECT_STATION
return self.calcMotorPowersByBallAngle(shmem.stationAngle, MotorController.SPEED_CHASE, MotorController.K_CHASE_ANGLE)
TRACE('shmem.ballDis is invalid value')
# センターカメラから指令が来ていたらguide_info.jsonに有効な値が入っている
with open('./guide_info.json', mode='r') as f:
try:
guide_info_json = json.load(f)
guide_degree = guide_info_json.get('degree', 360)
if guide_degree != 360:
shmem.soundPhase = SoundPhaseE.RECV_CAMERA_INFO
DEBUG('guide_degree = ' + str(guide_degree))
# センターから停止命令が来ていたらモータ止める
if guide_info_json.get('wait', False):
DEBUG('recieved stop order!')
return 0, 0
return self.calcMotorPowersByBallAngle(-guide_degree, MotorController.SPEED_STATION_GUIDE,
MotorController.K_STATION_GUIDE_ANGLE)
except json.JSONDecodeError:
ERROR('faild to load guide_info.json')
body_angle = shmem.bodyAngle
return self.calcMotorPowersByBallAngle(body_angle / 10 - (body_angle / abs(body_angle) * 180), MotorController.SPEED_CHASE,
MotorController.K_CHASE_ANGLE)
# PREPARE_RETART
TRACE('motion_status = PREPARE_RESTART')
return (0, 0)
def __init__(self, update_interval_sec=0.1):
TRACE('Imu generated')
self._mpu9250 = FaBo9Axis_MPU9250.MPU9250()
self._epsilon = 0
self._degree = 0
self._update_interval_sec = update_interval_sec
def calcMotorPowersByGoalAngle(self, eGoalAngle, mGoalAngle, fieldCenterAngle):
if self.DEBUG_SHOOT_ALGORITHM == 0:
# ゴールが正面にある場合
if abs(eGoalAngle) < 10:
TRACE('calcMotor patern : enemyGoalAngle = 0')
# 前進
return MotorController.SPEED_FRONT_GOAL, MotorController.SPEED_FRONT_GOAL
# ゴールが正面にない場合
elif -180 < eGoalAngle < 180:
TRACE('calcMotor patern : -180 < enemyGoalAngle < 180')
# モータの値は補正をかける
speed = eGoalAngle / MotorController.CORRECTION_VALUE_EGOAL_ANGLE_TO_SPEED
# 絶対値が100を超える場合は100に丸める
#speed = self.roundOffWithin100(speed)
return (-speed), speed
# ゴールとの角度が不正値の場合
else:
# 自分のゴールの角度を使う
# 自分のゴールとの角度が取れている場合
if -180 < mGoalAngle < 180:
TRACE('calcMotor patern : -180 < mGoalAngle < 180')
# とりあえず自軍のゴールの方向に旋回するのは危険そうなので逆に旋回する
# モータの値は補正をかける
speed = mGoalAngle / MotorController.CORRECTION_VALUE_MGOAL_ANGLE_TO_SPEED
# 絶対値が100を超える場合は100に丸める
#speed = self.roundOffWithin100(speed)
return speed, (-speed)
# 自分のゴールとの角度も不正値の場合
else:
TRACE('calcMotor patern : cannot detect goal')
# どうしようもない時用の値を使う
return MotorController.SPEED_NOTHING_TO_DO
elif self.DEBUG_SHOOT_ALGORITHM == 1:
# ゴールが正面にある場合
# P制御
if -180 < eGoalAngle < 180:
TRACE('calcMotor patern : -180 < enemyGoalAngle < 180')
# ゴール角度が正面からずれるほどたくさん曲がる
return MotorController.SPEED_SHOOT - MotorController.K_SHOOT_ANGLE * eGoalAngle,\
MotorController.SPEED_SHOOT + MotorController.K_SHOOT_ANGLE * eGoalAngle
# ゴールとの角度が不正値の場合
else:
# 自分のゴールの角度を使う
# 自分のゴールとの角度が取れている場合
if -180 < mGoalAngle < 180:
TRACE('calcMotor patern : -180 < mGoalAngle < 180')
# とりあえず自軍のゴールの方向に旋回するのは危険そうなので逆に旋回する
# モータの値は補正をかける
speed = mGoalAngle / MotorController.CORRECTION_VALUE_MGOAL_ANGLE_TO_SPEED
# 絶対値が100を超える場合は100に丸める
#speed = self.roundOffWithin100(speed)
return speed, (-speed)
# 自分のゴールとの角度も不正値の場合
else:
TRACE('calcMotor patern : cannot detect goal')
# どうしようもない時用の値を使う
return MotorController.SPEED_NOTHING_TO_DO
elif self.DEBUG_SHOOT_ALGORITHM == 2:
# ゴールが正面にある場合
# P制御
if -180 < eGoalAngle < 180:
TRACE('calcMotor patern : -180 < enemyGoalAngle < 180')
# ゴール角度が正面からずれるほどたくさん曲がる
return MotorController.SPEED_SHOOT - MotorController.K_SHOOT_ANGLE * eGoalAngle,\
MotorController.SPEED_SHOOT + MotorController.K_SHOOT_ANGLE * eGoalAngle
# ゴールとの角度が不正値の場合
else:
# フィールド中央との角度を使う
# フィールド中央との角度が取れている場合
if -180 < fieldCenterAngle < 180:
TRACE('calcMotor patern : -180 < fieldCenterAngle < 180')
# ゴール角度が正面からずれるほどたくさん曲がる
return MotorController.SPEED_GO_CENTER - MotorController.K_GO_CENTER_ANGLE * fieldCenterAngle, \
MotorController.SPEED_GO_CENTER + MotorController.K_GO_CENTER_ANGLE * fieldCenterAngle
# 自分のゴールとの角度も不正値の場合
else:
TRACE('calcMotor patern : cannot detect goal and center')
# どうしようもない時用の値を使う
return MotorController.SPEED_NOTHING_TO_DO
else:
ERROR('Invalid Value : DEBUG_SHOOT_ALGORITHM =', self.DEBUG_SHOOT_ALGORITHM)
return MotorController.SPEED_STOP
import cv2
import time
import glob
"""
unit test for imageProcessing.py
"""
class TestPoint(Structure):
_fields_ = [('ballAngle', c_int), ('ballDis', c_int)]
if __name__ == '__main__':
TRACE('test main line')
# テスト用共有メモリの準備
shmem = Value(TestPoint, 0)
imageProcessing = ImageProcessing()
file_path = 'data/temp/camera_capture_wall_6.jpg'
TRACE(file_path)
stream = cv2.imread(file_path)
ball_angle, ball_distance, station_angle, station_distance, wall_x, wall_size = imageProcessing.imageProcessingFrame(
stream, shmem)
cv2.imshow('Frame', cv2.flip(stream, -1))
cv2.moveWindow('Frame', 0, 30)
def __init__(self):
TRACE('ImageProcessing generated')
def __init__(self, addr):
TRACE('MiniMotorDriver generated')
self._i2c = smbus.SMBus(1)
self._addr = addr
def target(self, shmem):
TRACE('imageProcessingMain target() start')
self.imageProcessingMain(shmem)
import time
import glob
# テスト用共有メモリの構造体
class TestPoint(Structure):
_fields_ = [('irAngle', c_int), ('isTouched', c_int),
('enemyGoalAngle', c_int), ('enemyGoalDis', c_int),
('myGoalAngle', c_int), ('myGoalDis', c_int),
('fieldCenterAngle', c_int), ('fieldCenterDis', c_int)]
if __name__ == '__main__':
from multiprocessing import Process, Value
TRACE('test main line')
# テスト用共有メモリの準備
shmem = Value(TestPoint, 0)
# モータ制御インスタンスの生成
imageProcessing = ImageProcessing()
p_imageProcessing = Process(target=imageProcessing.target, args=(shmem, ))
file_list = glob.glob("./camera_distance/0degree/*jpg")
TRACE(file_list)
for file in file_list:
TRACE(file)
stream = cv2.imread(file)