示例#1
0
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()
示例#2
0
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()
示例#3
0
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()