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()
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.pid = PID()
self.mpu = AltIMU()
self.mpu.enable()
self.mpu.calibrateGyroAngles()
self.mpu.initComplementaryFromAccel=True
self.encoder_left.start()
self.encoder_right.start()
self.cstate={}
self.cstate['theta']=0
self.cstate['gamma']=0
self.cstate['phi']=0
self.cstate['wheelAngleR']=0
self.cstate['wheelAngleL']=0
self.setpoint={}
self.setpoint['phi']=0
self.setpoint['theta']=0
self.setpoint['phidot']=0
self.setpoint['gammadot']=0
self.setpoint['gamma']=0
self.D1 = Filter(0.01, settings.edumip_params['D1']['num'], settings.edumip_params['D1']['den'])
self.D1.gain = settings.edumip_params['D1']['gain']
self.D1.filter_enable_saturation(-1.0, 1.0)
self.D1.filter_enable_soft_start(0.7)
self.D1.name='D1'
self.D2 = Filter(0.01, settings.edumip_params['D2']['num'], settings.edumip_params['D2']['den'])
self.D2.gain = settings.edumip_params['D2']['gain']
self.D2.filter_enable_saturation(-settings.edumip_params['D2']['theta_ref_max'], settings.edumip_params['D2']['theta_ref_max'])
self.D2.filter_enable_soft_start(0.7)
self.D2.name='D2'
self.D3 = Filter()
self.D3.filter_pid(settings.edumip_params['D3']['kp'], settings.edumip_params['D3']['ki'],
settings.edumip_params['D3']['kd'], 0.04, 0.01)
self.D3.name='D3'
def run(self):
pwm_velocity = pwm_turn = 0
counter_remote = counter_velocity = counter_turn = 0
P=I=oldI=oldP=D=0
bp=-60
while True:
begin_time = datetime.datetime.now()
self.mpu.update(0.05,0)
theta = self.mpu.balance_angle+settings.BOARD_MOUNT_ANGLE*180/math.pi
print("---", theta, " : ", self.mpu.balance_roll)
roll = self.mpu.balance_roll*3.14/180
oldP = P
P=(roll) +bp
oldI = I
I = I + (P*0.05)
I = I + ((I-oldI)*2)
if (I> 250):
I = 250
elif(I<-250):
I = -250
D=P-oldP
pwm = P + I + D*10
if (pwm<0): bp=bp-0.01
if (pwm>0): bp=bp+0.01
dL=pwm
dR=pwm
#print(dL, dR)
time.sleep(0.05)
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()
