class Robot:
def __init__(self):
self.motor_left = Motor(settings.PINS['motor']['left'])
self.motor_right = Motor(settings.PINS['motor']['right'])
self.encoder_left = Encoder.Worker(settings.PINS['encoder']['left'])
self.encoder_right = Encoder.Worker(settings.PINS['encoder']['right'])
self.mpu = MPU6050(0x6b)
self.pid = PID()
self.encoder_left.start()
self.encoder_right.start()
def run(self):
pwm_velocity = pwm_turn = 0
counter_remote = counter_velocity = counter_turn = 0
while True:
begin_time = datetime.datetime.now()
self.mpu.update()
pwm_balance = self.pid.get_balance_pwm(
settings.pid_params['balance'], self.mpu.balance_angle + 1,
self.mpu.balance_gyro)
if counter_velocity >= 2:
pwm_velocity = self.pid.get_velocity_pwm(
settings.pid_params['velocity'], self.encoder_left.speed,
-self.encoder_right.speed)
print(self.encoder_left.speed, self.encoder_right.speed)
self.encoder_left.speed = 0
self.encoder_right.speed = 0
counter_velocity = 0
counter_velocity += 1
if self.mpu.balance_angle > 40 or self.mpu.balance_angle < -40:
self.pause()
continue
pwm_left = int(pwm_balance + pwm_velocity + pwm_turn)
pwm_right = int(pwm_balance + pwm_velocity - pwm_turn)
self.motor_left.set_pwm(pwm_left)
self.motor_right.set_pwm(pwm_right)
dt = (datetime.datetime.now() - begin_time).total_seconds()
dt = (0.005 - dt) if dt < 0.005 else 0
time.sleep(dt)
def pause(self):
self.motor_left.stop()
self.motor_right.stop()
self.encoder_left.reset()
self.encoder_right.reset()
self.pid.clean()
def stop(self):
self.motor_left.stop()
self.motor_right.stop()
self.encoder_left.cancel()
self.encoder_right.cancel()
class Robot:
def __init__(self):
self.motor_left = Motor(settings.PINS['motor']['left'])
self.motor_right = Motor(settings.PINS['motor']['right'])
self.encoder_left = Encoder.Worker(settings.PINS['encoder']['left'])
self.encoder_right = Encoder.Worker(settings.PINS['encoder']['right'])
self.mpu = MPU6050(0x68)
self.remote = RemoteControlServer(settings.pid_params)
self.pid = PID()
self.encoder_left.start()
self.encoder_right.start()
def run(self):
self.remote.start()
pwm_velocity = pwm_turn = 0
counter_remote = counter_velocity = counter_turn = 0
while True:
begin_time = datetime.datetime.now()
if self.remote.received['start']:
self.mpu.update()
pwm_balance = self.pid.get_balance_pwm(
self.remote.received['pid']['balance'],
self.mpu.balance_angle + 1, self.mpu.balance_gyro)
if counter_velocity >= 2:
pwm_velocity = self.pid.get_velocity_pwm(
self.remote.received['pid']['velocity'],
self.encoder_left.speed, -self.encoder_right.speed,
self.remote.received['joystick'][0])
print(self.encoder_left.speed, self.encoder_right.speed)
self.encoder_left.speed = 0
self.encoder_right.speed = 0
counter_velocity = 0
counter_velocity += 1
if counter_turn >= 4:
pwm_turn = self.pid.get_turn_pwm(
self.remote.received['pid']['turn'],
self.mpu.gyro['z'],
self.remote.received['joystick'][1])
counter_turn = 0
counter_turn += 1
# 小车倒下停车
if self.mpu.balance_angle > 40 or self.mpu.balance_angle < -40:
self.pause()
self.remote.received['start'] = False
continue
# 设置电机PWM
pwm_left = int(pwm_balance + pwm_velocity + pwm_turn)
pwm_right = int(pwm_balance + pwm_velocity - pwm_turn)
self.motor_left.set_pwm(pwm_left)
self.motor_right.set_pwm(pwm_right)
# 发送远程数据，五个周期执行一次
if counter_remote > 5:
self.remote.send_data(self.mpu.balance_angle,
self.mpu.balance_gyro,
self.mpu.turn_gyro, pwm_left,
pwm_right)
counter_remote = 0
counter_remote += 1
if settings.DEBUG:
# print(self.mpu.gyro,self.mpu.accel)
print("PWM：%d,%d\t倾角：%f\t加速度%f" %
(pwm_left, pwm_right, self.mpu.balance_angle,
self.mpu.balance_gyro))
else:
self.pause()
# 5毫秒执行一次主循环
dt = (datetime.datetime.now() - begin_time).total_seconds()
dt = (0.005 - dt) if dt < 0.005 else 0
time.sleep(dt)
def pause(self):
self.motor_left.stop()
self.motor_right.stop()
self.encoder_left.reset()
self.encoder_right.reset()
self.pid.clean()
def stop(self):
self.motor_left.stop()
self.motor_right.stop()
self.encoder_left.cancel()
self.encoder_right.cancel()
self.mpu.close()
