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()