if __name__ == '__main__':
info('main line')
# 共有メモリの準備
shmem = Value(Point, 0)
shmem.preparingRestart = False
shmem.relyStation = False
shmem.soundPhase = 0
# モータ制御インスタンスの生成
motorController = MotorController()
# 画像処理インスタンスの生成
imageProcessing = ImageProcessing()
# 慣性計測インスタンスの生成
imu = Imu()
# スタート時に一度ジャイロセンサの補正をしておく
imu.calibrate()
# websocketサーバの生成
# server = Server(9001)
sound = Sound()
p_motorContl = Process(target=motorController.target, args=(shmem, ))
p_imageProcessing = Process(target=imageProcessing.target, args=(shmem, ))
p_imu = Process(target=imu.target, args=(shmem, ))
# p_server = Process(target=server.target, args=())
p_sound = Process(target=sound.target, args=(shmem, ))
p_motorContl.start()
DEBUG('p_motorContl started')
p_imageProcessing.start()
DEBUG('p_imageProcessing started')
p_imu.start()
class RovMovement:
def __init__(self,
xy_lf_pin,
xy_rf_pin,
xy_lb_pin,
xy_rb_pin,
z_lf_pin,
z_rf_pin,
z_lb_pin,
z_rb_pin,
arm_pin,
initialize_motors=True,
ssc_control=True):
with open('p2t.json', 'r') as json_file:
p2t = json.load(json_file)
self.ssc_control = ssc_control
self.motors = {
"xy_lf": ContinuousRotationServo(xy_lf_pin, p2t=p2t),
"xy_rf": ContinuousRotationServo(xy_rf_pin, p2t=p2t),
"xy_lb": ContinuousRotationServo(xy_lb_pin, p2t=p2t),
"xy_rb": ContinuousRotationServo(xy_rb_pin, p2t=p2t),
"z_lf": ContinuousRotationServo(z_lf_pin, p2t=p2t),
"z_rf": ContinuousRotationServo(z_rf_pin, p2t=p2t),
"z_lb": ContinuousRotationServo(z_lb_pin, p2t=p2t),
"z_rb": ContinuousRotationServo(z_rb_pin, p2t=p2t)
}
self.motor_prev_powers = {
"xy_lf": 0,
"xy_rf": 0,
"xy_lb": 0,
"xy_rb": 0,
"z_lf": 0,
"z_rf": 0,
"z_lb": 0,
"z_rb": 0
}
self.arm = StandardServo(arm_pin)
self.arm_status = False
self.open_arm()
# PID
self.old_err = np.zeros((2, 2))
self.imu = Imu()
self.imu.start()
if initialize_motors:
self.initialize_motors()
self._initialize_imu()
sleep(2)
def select_motors(self, search_list):
return {
key: value
for (key, value) in self.motors.items()
if not any(s not in key for s in search_list)
}
def initialize_motors(self, mp=30):
print("All motors initializing...")
for cp in list(range(0, mp)) + list(range(mp, -mp, -1)) + list(
range(-mp, 1)):
print("Power:", cp)
for motor in self.motors.values():
motor.run_bidirectional(cp)
sleep(0.01)
print("All motors initialized...")
def _initialize_imu(self, seconds=5):
print("IMU is being calibrated...")
self.imu.calibrate(seconds)
print("IMU is calibrated...")
def _get_z_balance_p(self, kp=0.0, kd=0.0, type_=int, tilt_degree=0):
# if not kp > kd:
# raise Exception("Kp must be bigger than Kd. Kp: %s - Kd: %s" % (kp, kd))
try:
x, y, _ = self.imu.get_degree().get()
# if abs(tilt_degree) == 90:
# if 45 < y < 90:
# tilt_degree = 90
# elif -90 < y < -45:
# tilt_degree = -90
y = y - tilt_degree
comp_sign = +1 if (x < 0 and y < 0) or (x > 0 and y > 0) else -1
if -1 < x < 1: x = 0
if -1 < y < 1: y = 0
lf = sign_n(-y - x) * math.sqrt(
abs(-y * -y + comp_sign * -x * -x)) # -y - x
rf = sign_n(-y + x) * math.sqrt(
abs(-y * -y - comp_sign * +x * +x)) # -y + x
lb = sign_n(+y - x) * math.sqrt(
abs(+y * +y - comp_sign * -x * -x)) # +y - x
rb = sign_n(+y + x) * math.sqrt(
abs(+y * +y + comp_sign * +x * +x)) # +y + x
err = np.array([[lf, rf], [lb, rb]])
ret = kp * err + kd * (err - self.old_err)
self.old_err = err
if type_ == int:
ret = np.rint(ret)
return ret.ravel()
except Exception as e:
print("Exception in _get_balance_p:", e)
return 0, 0, 0, 0
def _gradual_power_change(self,
current_powers,
ssc_control=True,
ssc_step=5,
ssc_sleep=0.01):
start_powers = self.motor_prev_powers
stop_powers = current_powers
motor_keys = stop_powers.keys()
diff = [
abs(stop_powers[key] - start_powers[key]) for key in motor_keys
]
steps = int(max(diff) / ssc_step)
power_list = {
key: np.linspace(start_powers[key],
stop_powers[key],
steps,
endpoint=False,
dtype=int)
for key in motor_keys
}
if steps and ssc_control:
for k in range(1, steps):
for key in motor_keys:
self.motors[key].run_bidirectional(int(power_list[key][k]))
print(str(key) + ":", power_list[key][k], end="\t")
print()
sleep(ssc_sleep)
for key in motor_keys:
self.motors[key].run_bidirectional(int(stop_powers[key]))
print(str(key) + ":", stop_powers[key], end="\t")
print()
def go_z_bidirectional(self, power, with_balance=True, tilt_degree=0):
power_per_motor = int(power / 4)
lf_p, rf_p, lb_p, rb_p = self._get_z_balance_p(
kp=0.35, kd=0.30, tilt_degree=tilt_degree) if with_balance else (0,
0,
0,
0)
current_powers = {
"z_lf": power_per_motor + lf_p,
"z_rf": power_per_motor + rf_p,
"z_lb": power_per_motor + lb_p,
"z_rb": power_per_motor + rb_p
}
self._gradual_power_change(current_powers,
ssc_control=self.ssc_control)
self.motor_prev_powers.update(current_powers)
def go_xy_and_turn(self, power, degree, turn_power):
"""
:param power: Power sent to the vehicle's movement
:param degree: degree of movement (0between 0-360 degree)
0 -> ileri
90 -> sağa
180 -> geri
270 -> sola
:param turn_power: Turn power
Positive value -> Turn right
Negative value -> Turn left
:return:
"""
# if turn_power:
# turn_power = turn_power / 4
# turn_power_per_motor = int(turn_power / 4)
# else:
# _, _, z = self.imu.get_instant_gyro().get()
# turn_power_per_motor = int(z)
turn_power = turn_power / 4
turn_power_per_motor = int(turn_power / 4)
go_power_per_motor = int(power / 2)
radian_rf = (45 - degree) / 180 * math.pi
radian_lf = (135 - degree) / 180 * math.pi
radian_lb = (225 - degree) / 180 * math.pi
radian_rb = (315 - degree) / 180 * math.pi
pow_rf = int(
math.sin(radian_rf) * go_power_per_motor - turn_power_per_motor)
pow_lf = int(
math.sin(radian_lf) * go_power_per_motor + turn_power_per_motor)
pow_lb = int(
math.sin(radian_lb) * go_power_per_motor - turn_power_per_motor)
pow_rb = int(
math.sin(radian_rb) * go_power_per_motor + turn_power_per_motor)
current_powers = {
"xy_lf": pow_lf,
"xy_rf": pow_rf,
"xy_lb": pow_lb,
"xy_rb": pow_rb
}
self._gradual_power_change(current_powers)
self.motor_prev_powers.update(current_powers)
def go_xyz_with_tilt(self,
xy_power,
z_power,
turn_power,
with_balance=True,
tilt_degree=0):
"""
:param xy_power: between from -70 to 70
:param z_power: between from -70 to 70
:param turn_power: Turn power
Positive value -> Turn right
Negative value -> Turn left
:param with_balance: Should the balance of the vehicle be achieved with pid control?
:param tilt_degree: the inclination the vehicle will make forward
negative value -> leaning forward of the vehicle
positive value -> raising the front of the vehicle
:return:
"""
tilt_radian = tilt_degree * math.pi / 180
xy_power_ = math.cos(-tilt_radian) * xy_power - math.sin(
-tilt_radian) * z_power
z_power_ = math.sin(-tilt_radian) * xy_power + math.cos(
-tilt_radian) * z_power
self.go_xy_and_turn(xy_power_, 0, turn_power)
self.go_z_bidirectional(z_power_,
with_balance=with_balance,
tilt_degree=tilt_degree)
def open_arm(self):
self.arm.change_angle(100)
self.arm_status = True
def close_arm(self):
self.arm.change_angle(30)
self.arm_status = False
def toggle_arm(self, arm_status=None):
if self.arm_status and (arm_status is None or arm_status == False):
self.close_arm()
elif not self.arm_status and (arm_status is None
or arm_status == True):
self.open_arm()
def run_all_motors_cw(self, power):
for motor in self.motors.values():
motor.run_clockwise(power)
def run_all_motors_ccw(self, power):
for motor in self.motors.values():
motor.run_counterclockwise(power)
def stop(self):
print("RovMovement is terminating...")
for motor in self.motors.values():
motor.stop()
self.arm.stop()
self.imu.stop()
print("RovMovement has been terminated.")
def close(self):
self.stop()
class RovMovement:
def __init__(self,
xy_lf_pin,
xy_rf_pin,
xy_lb_pin,
xy_rb_pin,
z_lf_pin,
z_rf_pin,
z_lb_pin,
z_rb_pin,
arm_pin,
initialize_motors=True,
ssc_control=True):
with open('p2t.json', 'r') as json_file:
p2t = json.load(json_file)
self.xy_lf = ContinuousRotationServo(xy_lf_pin,
ssc_control=ssc_control,
p2t=p2t)
self.xy_rf = ContinuousRotationServo(xy_rf_pin,
ssc_control=ssc_control,
p2t=p2t)
self.xy_lb = ContinuousRotationServo(xy_lb_pin,
ssc_control=ssc_control,
p2t=p2t)
self.xy_rb = ContinuousRotationServo(xy_rb_pin,
ssc_control=ssc_control,
p2t=p2t)
self.z_lf = ContinuousRotationServo(z_lf_pin,
ssc_control=ssc_control,
p2t=p2t)
self.z_rf = ContinuousRotationServo(z_rf_pin,
ssc_control=ssc_control,
p2t=p2t)
self.z_lb = ContinuousRotationServo(z_lb_pin,
ssc_control=ssc_control,
p2t=p2t)
self.z_rb = ContinuousRotationServo(z_rb_pin,
ssc_control=ssc_control,
p2t=p2t)
self.arm = StandardServo(arm_pin)
self.imu = Imu()
self.z_motors_list = [self.z_lf, self.z_rf, self.z_lb, self.z_rb]
self.xy_motors_list = [self.xy_lf, self.xy_rf, self.xy_lb, self.xy_rb]
self.all_motors_list = self.z_motors_list + self.xy_motors_list
self.arm_status = False
self.open_arm()
# PID
self.old_err = np.zeros((2, 2))
self.imu.start()
if initialize_motors:
self.initialize_motors()
self._initialize_imu()
sleep(2)
def initialize_motors(self, mp=30):
print("All motors initializing...")
for cp in list(range(0, mp)) + list(range(mp, -mp, -1)) + list(
range(-mp, 1)):
print("Power:", cp)
for motor in self.all_motors_list:
motor.run_bidirectional(cp)
sleep(0.01)
print("All motors initialized...")
def _initialize_imu(self, seconds=5):
print("IMU is being calibrated...")
self.imu.calibrate(seconds)
print("IMU is calibrated...")
def _get_z_balance_p(self, kp=0.0, kd=0.0, type_=int):
# if not kp > kd:
# raise Exception("Kp must be bigger than Kd. Kp: %s - Kd: %s" % (kp, kd))
try:
x, y, _ = self.imu.get_degree().get()
comp_sign = +1 if (x < 0 and y < 0) or (x > 0 and y > 0) else -1
if -1 < x < 1: x = 0
if -1 < y < 1: y = 0
lf = sign_n(-y - x) * math.sqrt(
abs(-y * -y + comp_sign * -x * -x)) # -y - x
rf = sign_n(-y + x) * math.sqrt(
abs(-y * -y - comp_sign * +x * +x)) # -y + x
lb = sign_n(+y - x) * math.sqrt(
abs(+y * +y - comp_sign * -x * -x)) # +y - x
rb = sign_n(+y + x) * math.sqrt(
abs(+y * +y + comp_sign * +x * +x)) # +y + x
err = np.array([[lf, rf], [lb, rb]])
ret = kp * err + kd * (err - self.old_err)
self.old_err = err
if type_ == int:
ret = np.rint(ret)
return ret.ravel()
except Exception as e:
print("Exception in _get_balance_p:", e)
return 0, 0, 0, 0
def go_z_bidirectional(self, power, with_balance=True):
power_per_motor = int(power / 4)
lf_p, rf_p, lb_p, rb_p = self._get_z_balance_p(
kp=0.3, kd=0.15) if with_balance else (0, 0, 0, 0)
self.z_lf.run_bidirectional(power_per_motor + int(lf_p))
self.z_rf.run_bidirectional(power_per_motor + int(rf_p))
self.z_lb.run_bidirectional(power_per_motor + int(lb_p))
self.z_rb.run_bidirectional(power_per_motor + int(rb_p))
def go_up(self, power):
power_per_motor = int(power / 4)
for motor in self.z_motors_list:
motor.run_clockwise(power_per_motor)
def go_down(self, power):
power_per_motor = int(power / 4)
for motor in self.z_motors_list:
motor.run_counterclockwise(power_per_motor)
def turn_left(self, power):
power = power / 4
power_per_motor = int(power / 4)
self.xy_rf.run_clockwise(power_per_motor)
self.xy_lf.run_counterclockwise(power_per_motor)
self.xy_lb.run_clockwise(power_per_motor)
self.xy_rb.run_counterclockwise(power_per_motor)
def turn_right(self, power):
power = power / 4
power_per_motor = int(power / 4)
self.xy_rf.run_counterclockwise(power_per_motor)
self.xy_lf.run_clockwise(power_per_motor)
self.xy_lb.run_counterclockwise(power_per_motor)
self.xy_rb.run_clockwise(power_per_motor)
def go_xy(self, power, degree):
"""
:param power: Power sent to the vehicle's movement
:param degree: degree of movement (0between 0-360 degree)
0 -> ileri
90 -> sağa
180 -> geri
270 -> sola
:return:
"""
power_per_motor = int(power / 2)
radian_rf = (45 - degree) / 180 * math.pi
radian_lf = (135 - degree) / 180 * math.pi
radian_lb = (225 - degree) / 180 * math.pi
radian_rb = (315 - degree) / 180 * math.pi
pow_rf = int(math.sin(radian_rf) * power_per_motor)
pow_lf = int(math.sin(radian_lf) * power_per_motor)
pow_lb = int(math.sin(radian_lb) * power_per_motor)
pow_rb = int(math.sin(radian_rb) * power_per_motor)
self.xy_rf.run_bidirectional(pow_rf)
self.xy_lf.run_bidirectional(pow_lf)
self.xy_lb.run_bidirectional(pow_lb)
self.xy_rb.run_bidirectional(pow_rb)
def go_xy_and_turn(self, power, degree, turn_power):
"""
:param power: Power sent to the vehicle's movement
:param degree: degree of movement (0between 0-360 degree)
0 -> ileri
90 -> sağa
180 -> geri
270 -> sola
:param turn_power: Turn power
Positive value -> Turn right
Negative value -> Turn left
:return:
"""
turn_power = turn_power / 4
turn_power_per_motor = int(turn_power / 4)
go_power_per_motor = int(power / 2)
radian_rf = (45 - degree) / 180 * math.pi
radian_lf = (135 - degree) / 180 * math.pi
radian_lb = (225 - degree) / 180 * math.pi
radian_rb = (315 - degree) / 180 * math.pi
pow_rf = int(
math.sin(radian_rf) * go_power_per_motor - turn_power_per_motor)
pow_lf = int(
math.sin(radian_lf) * go_power_per_motor + turn_power_per_motor)
pow_lb = int(
math.sin(radian_lb) * go_power_per_motor - turn_power_per_motor)
pow_rb = int(
math.sin(radian_rb) * go_power_per_motor + turn_power_per_motor)
self.xy_rf.run_bidirectional(pow_rf)
self.xy_lf.run_bidirectional(pow_lf)
self.xy_lb.run_bidirectional(pow_lb)
self.xy_rb.run_bidirectional(pow_rb)
def open_arm(self):
self.arm.change_angle(100)
self.arm_status = True
def close_arm(self):
self.arm.change_angle(30)
self.arm_status = False
def toggle_arm(self, arm_status=None):
if self.arm_status and (arm_status is None or arm_status == False):
self.close_arm()
elif not self.arm_status and (arm_status is None
or arm_status == True):
self.open_arm()
def run_all_motors_cw(self, power):
for motor in self.all_motors_list:
motor.run_clockwise(power)
def run_all_motors_ccw(self, power):
for motor in self.all_motors_list:
motor.run_counterclockwise(power)
def stop(self):
print("RovMovement is terminating...")
for motor in self.all_motors_list:
motor.stop()
self.arm.stop()
self.imu.stop()
print("RovMovement has been terminated.")
def close(self):
self.stop()