Python PID.step方法代码示例

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
    def model(self, p):

        model = nengo.Network()
        with model:

            system = System(p.D, p.D, dt=p.dt, seed=p.seed,
                    motor_noise=p.noise, sense_noise=p.noise,
                    scale_add=p.scale_add,
                    motor_scale=10,
                    motor_delay=p.delay, sensor_delay=p.delay,
                    motor_filter=p.filter, sensor_filter=p.filter)

            self.system_state = []
            self.system_times = []
            self.system_desired = []
            def minsim_system(t, x):
                r = system.step(x)
                self.system_state.append(system.state)
                self.system_times.append(t)
                self.system_desired.append(signal.value(t))
                return r

            minsim = nengo.Node(minsim_system, size_in=p.D, size_out=p.D)

            state_node = nengo.Node(lambda t: system.state)

            pid = PID(p.Kp, p.Kd, p.Ki, tau_d=p.tau_d)
            control = nengo.Node(lambda t, x: pid.step(x[:p.D], x[p.D:]),
                                 size_in=p.D*2)
            nengo.Connection(minsim, control[:p.D], synapse=0)
            nengo.Connection(control, minsim, synapse=None)

            if p.adapt:

                adapt = nengo.Ensemble(p.n_neurons, dimensions=p.D,
                                       radius=p.radius)
                nengo.Connection(minsim, adapt, synapse=None)
                conn = nengo.Connection(adapt, minsim, synapse=p.synapse,
                        function=lambda x: [0]*p.D,
                        solver=ZeroDecoder(),
                        learning_rule_type=nengo.PES())
                conn.learning_rule_type.learning_rate *= p.learning_rate
                nengo.Connection(control, conn.learning_rule, synapse=None,
                        transform=-1)

            signal = Signal(p.D, p.period, dt=p.dt, max_freq=p.max_freq, seed=p.seed)
            desired = nengo.Node(signal.value)
            nengo.Connection(desired, control[p.D:], synapse=None)

            #self.p_desired = nengo.Probe(desired, synapse=None)
            #self.p_q = nengo.Probe(state_node, synapse=None)
        return model

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
class Controller(object):
    def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit,
                accel_limit, wheel_radius, wheel_base,
                steer_ratio, max_lat_accel, max_steer_angle):
        min_speed = 1.0 * ONE_MPH
        self.pid = PID(2.0, 0.4, 0.1)
        self.lpf = LowPassFilter(0.5, 0.02)
        self.yaw = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle)
        self.v_mass = vehicle_mass
        self.w_radius = wheel_radius
        self.d_limit = decel_limit
        self.last_time = None

    def control(self, proposed_v, proposed_omega, current_v):
        # Return throttle, brake, steer
        if self.last_time is None:
            throttle, brake, steer = 1.0, 0.0, 0.0
        else:
            dt = time.time() - self.last_time
            # get throttle, if negative, change it to break
            error = proposed_v.x - current_v.x
            throttle = self.pid.step(error, time.time() - self.last_time)
            throttle = max(0.0, min(1.0, throttle))

            if error < 0: # if we need to decelerate
                deceleration = abs(error) / dt
                if abs(deceleration ) > abs(self.d_limit) * 500:
                    deceleration = self.d_limit * 500
                longitudinal_force = self.v_mass * deceleration
                brake = longitudinal_force * self.w_radius

                throttle = 0
            else:
                brake = 0
            steer = self.yaw.get_steering(proposed_v.x, proposed_omega.z, current_v.x)

        self.last_time = time.time() # update time
        # rospy.logwarn('proposed v:    {}'.format(proposed_v.x))
        # rospy.logwarn('current v:    {}'.format(current_v.x))
        # rospy.logwarn('Error:    {}'.format(proposed_v.x - current_v.x))
        # rospy.logwarn('Throttle: {}'.format(throttle))
        # rospy.logwarn('Brake:    {}'.format(brake))
        # rospy.logwarn('Steer:    {}'.format(steer))
        # rospy.logwarn('Speed:    {}'.format(current_v))
        # rospy.logwarn('')
        return throttle, brake, steer

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
class Controller(object):
    def __init__(self, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle,
                 decel_limit, vehicle_mass):
        self.yaw_controller = YawController(
            wheel_base,
            steer_ratio,
            0.1,  # min_speed
            max_lat_accel,
            max_steer_angle,
        )

        # PID coefficients
        kp = 0.3
        ki = 0.1
        kd = 0.0

        # For convenience (no real limits on throttle):
        mn_th = 0.0  # Minimal throttle
        mx_th = 0.2  # Maximal throttle
        self.throttle_controller = PID(kp, ki, kd, mn_th, mx_th)

        tau = 0.5  # 1 / (2pi*tau) = cutoff frequency
        ts = 0.02  # Sample time
        self.vel_lpf = LowPassFilter(tau, ts)

        self.wheel_radius = wheel_radius
        self.decel_limit = decel_limit
        self.vehicle_mass = vehicle_mass

        self.last_time = rospy.get_time()

    def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
        if not dbw_enabled:
            self.throttle_controller.reset()
            return 0.0, 0.0, 0.0

        filtered_vel = self.vel_lpf.filt(current_vel)

        # rospy.logwarn(
        #     '\nAngular vel: {}\n'
        #     'Target velocity: {}\n'
        #     'Current velocity: {}\n'
        #     'Filtered velocity: {}\n'
        #     .format(angular_vel, linear_vel, current_vel, filtered_vel)
        # )

        steering = self.yaw_controller.get_steering(linear_vel, angular_vel, filtered_vel)

        vel_error = linear_vel - filtered_vel

        current_time = rospy.get_time()
        sample_time = current_time - self.last_time
        self.last_time = current_time

        throttle = self.throttle_controller.step(vel_error, sample_time)

        brake = 0

        if linear_vel == 0 and filtered_vel < 0.1:
            throttle = 0
            brake = 400  # [N*m] -- to hold the car in place if we are stopped
                         # at a light. Acceleration ~ 1 m/s/s
        elif throttle < 0.1 and vel_error < 0:
            throttle = 0
            decel = max(vel_error, self.decel_limit)
            brake = abs(decel) * self.vehicle_mass * self.wheel_radius  # Torque [N*m]

        return throttle, brake, steering

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
class Controller(object):
    def __init__(self, yaw_controller, p_brake):
        # TODO: Implement
        # defining 2 PID controllers
        self.throttle_pid = PID(kp=0.4, ki=0.02, kd=0.7, mn=-1.0, mx=1.0)
        self.brake_pid = PID(kp=p_brake, ki=0.0, kd=0.0, mn=0.0, mx=100000.)
        self.yaw_controller = yaw_controller
        self.des_speed_filter = LowPassFilter2(1.5, 0.) 
        self.throttle_brake_offs = -1.0
        self.throttle_direct = 0.0
        self.standstill_velocity = 0.01
        self.standstill_brake = -2.0*p_brake*self.throttle_brake_offs
        self.standstill_filter_time = 0.75
        self.standstill_filter = LowPassFilter2(self.standstill_filter_time, 0.0)
        # take 2 secs to reach max speed, init speed = 0.

    #def control(self, delta_time, lin_vel_err, ang_vel_err):
    def control(self, delta_time, linear_velocity, angular_velocity, current_velocity):
        # TODO: Change the arg, kwarg list to suit your needs
        # Return throttle, brake, steer
        #rospy.logwarn(lin_vel_err)
        
        filt_linear_velocity = self.des_speed_filter.filt(linear_velocity, delta_time)
        filt_lin_vel_err = filt_linear_velocity - current_velocity
        lin_vel_err = linear_velocity - current_velocity
        
        throttle_ = self.throttle_pid.step(filt_lin_vel_err, delta_time) + self.throttle_direct * linear_velocity
        if throttle_ < 0.0:
            throttle_ = 0.0
        
        brake_ = self.brake_pid.step(-(lin_vel_err-self.throttle_brake_offs), delta_time)
        #rospy.logwarn(self.throttle_pid.get_state())
        
        if lin_vel_err > self.throttle_brake_offs:
            #self.brake_pid.reset()
            if linear_velocity < self.standstill_velocity and lin_vel_err < 0.0:
                brake_ = self.standstill_filter.filt(self.standstill_brake, delta_time)
                throttle_ = 0.
                self.throttle_pid.reset()
                rospy.logwarn("--- standstill --- "+str(brake_))
            else:
                brake_ = 0.
                self.standstill_filter.init(0.)
        elif lin_vel_err <= self.throttle_brake_offs:
            self.throttle_pid.reset()
            throttle_ = 0.
            self.standstill_filter.init(brake_)
            rospy.logwarn("--- brake --- "+str(brake_))
        else:
            throttle_ = 0.
            brake_ = 0.
            
        steer_ = self.yaw_controller.get_steering(filt_linear_velocity, angular_velocity, current_velocity)
        
        #rospy.logwarn(str(linear_velocity)+" "+str(current_velocity)+" "+str(throttle_)+" "+str(brake_)+" "+str(steer_))
        
        return throttle_, brake_, steer_
    
    def init(self, curr_lin_vel):
        self.throttle_pid.reset()
        self.brake_pid.reset()
        self.des_speed_filter.init(curr_lin_vel)
        self.standstill_filter.init(0.0)

    def reload_params(self):
        #rospy.logwarn(os.path.dirname(os.path.realpath(__file__)))
        fn=os.path.dirname(os.path.realpath(__file__))+"/controlparams.json"
        with open(fn) as json_data:
            d = json.load(json_data)
            speed_err = d['speed_err']
            self.throttle_pid.set_params(speed_err['p'], speed_err['i'], speed_err['d']);
            self.throttle_direct = d['throttle_direct']
            self.des_speed_filter.set_tau(d['lowpass_linvel'])
            self.throttle_brake_offs = d['throttle_brake_offs']
            standstill = d['standstill']
            self.standstill_velocity = standstill['velocity']
            self.standstill_brake = standstill['brake']
            self.standstill_filter_time = standstill['filter_time']
            self.standstill_filter.set_tau(self.standstill_filter_time)
            

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
class YawController(object):
    def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle):
        self.wheel_base = wheel_base
        self.steer_ratio = steer_ratio
        self.min_speed = 5
        self.max_lat_accel = max_lat_accel
        self.previous_dbw_enabled = False
        self.min_angle = -max_steer_angle
        self.max_angle = max_steer_angle
        self.linear_pid = PID(0.9, 0.001, 0.0004, self.min_angle, self.max_angle)
        #self.cte_pid = PID(0.4, 0.1, 0.1, self.min_angle, self.max_angle)
        self.cte_pid = PID(0.4, 0.1, 0.2, self.min_angle, self.max_angle)
        self.tau = 0.2
        self.ts = 0.1
        self.low_pass_filter = LowPassFilter(self.tau, self.ts)

    def get_angle(self, radius, current_velocity):
         angle = math.atan(self.wheel_base / radius) * self.steer_ratio
         return max(self.min_angle, min(self.max_angle, angle))

    def get_steering_calculated(self, linear_velocity, angular_velocity, current_velocity):
        """
        Formulas:
        angular_velocity_new / current_velocity = angular_velocity_old / linear_velocity
        radius = current_velocity / angular_velocity_new
        angle = atan(wheel_base / radius) * self.steer_ratio
        """
        angular_velocity = current_velocity * angular_velocity / linear_velocity if abs(linear_velocity) > 0. else 0.

        if abs(current_velocity) > 0.1:
            max_yaw_rate = abs(self.max_lat_accel / current_velocity)
            angular_velocity = max(-max_yaw_rate, min(max_yaw_rate, angular_velocity))

        return self.get_angle(max(current_velocity, self.min_speed) / angular_velocity, current_velocity) if abs(angular_velocity) > 0. else 0.0;

    def get_steering_pid(self, angular_velocity, angular_current, dbw_enabled):
        angular_error = angular_velocity - angular_current
        sample_step = 0.02
        if not(self.previous_dbw_enabled) and dbw_enabled:
            self.previous_dbw_enabled = True
            self.linear_pid.reset()
            self.low_pass_filter = LowPassFilter(self.tau, self.ts)
        else:
            self.previous_dbw_enabled = False
        steering = self.linear_pid.step(angular_error, sample_step)
        steering = self.low_pass_filter.filt(steering)
        return steering

    def get_steering_pid_cte(self, final_waypoint1, final_waypoint2, current_location, dbw_enabled):
        steering = 0
        if final_waypoint1 and final_waypoint2:
            # vector from car to first way point
            a = np.array([current_location.x - final_waypoint1.pose.pose.position.x, current_location.y - final_waypoint1.pose.pose.position.y, current_location.z - final_waypoint1.pose.pose.position.z])
            # vector from first to second way point
            b = np.array([final_waypoint2.pose.pose.position.x-final_waypoint1.pose.pose.position.x, final_waypoint2.pose.pose.position.y-final_waypoint1.pose.pose.position.y, final_waypoint2.pose.pose.position.z-final_waypoint1.pose.pose.position.z])
            # progress on vector b
            # term = (a.b / euclidian_norm(b)**2) * b where a.b is dot product
            # term = progress * b => progress = term / b => progress = (a.b / euclidian_norm(b)**2)
            progress = (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]) / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2])
            # position where the car should be: waypoint1 + progress * b
            error_pos = np.array([final_waypoint1.pose.pose.position.x, final_waypoint1.pose.pose.position.y, final_waypoint1.pose.pose.position.z]) + progress * b
            # difference vector between where the car should be and where the car currently is
            error = (error_pos - np.array([current_location.x, current_location.y, current_location.z]))
            # is ideal track (b) left or right of the car's current heading?
            dot_product = a[0]*-b[1] + a[1]*b[0]
            direction = 1.0
            if dot_product >= 0:
                direction = -1.0
            else:
                direction = 1.0
            # Cross track error is the squared euclidian norm of the error vector: CTE = direction*euclidian_norm(error)**2
            cte = direction * math.sqrt(error[0]*error[0]+error[1]*error[1]+error[2]*error[2])
            sample_step = 0.02
            if not(self.previous_dbw_enabled) and dbw_enabled:
                self.previous_dbw_enabled = True
                self.cte_pid.reset()
                self.low_pass_filter = LowPassFilter(self.tau, self.ts)
            else:
                self.previous_dbw_enabled = False
            steering = self.cte_pid.step(cte, sample_step)
            #steering = self.low_pass_filter.filt(steering)
        return steering

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
class Controller(object):
    def __init__(
            self,
            vehicle_mass,
            fuel_capacity,
            brake_deadband,
            decel_limit,
            accel_limit,
            wheel_radius,
            wheel_base,
            steer_ratio,
            max_lat_accel,
            max_steer_angle,
            max_throttle):
        # Yaw controller to controller the turning
        self.vehicle_min_velocity = .1
        self.yaw_controller = YawController(
            wheel_base=wheel_base,
            steer_ratio=steer_ratio,
            min_speed=self.vehicle_min_velocity,
            max_lat_accel=max_lat_accel,
            max_steer_angle=max_steer_angle)

        self.steering_controller = PID(
            kp=.0, ki=.001, kd=.1, min=-.2, max=.2)

        # Throttle controller
        self.throttle_controller = PID(
            kp=.8, ki=.001, kd=.1, min=0, max=max_throttle)

        # Remove high frequency noise on the velocity
        self.low_pass = LowPassFilter(tau=.5, ts=.02)

        # Assume the tank is full
        fuel_mass = fuel_capacity * GAS_DENSITY
        self.total_mass = vehicle_mass + fuel_mass
        self.decel_limit = decel_limit
        self.wheel_radius = wheel_radius

        self.last_time = rospy.get_time()

    def control(
            self,
            current_velocity,
            linear_velocity,
            angular_velocity,
            cte):
        current_velocity = self.low_pass.filter(current_velocity)

        # Calculate the velocity error
        dv = linear_velocity - current_velocity

        # Update time
        current_time = rospy.get_time()
        dt = current_time - self.last_time
        self.last_time = current_time

        # Calculate the predictive path and correct for the cross track error
        steering = self.yaw_controller.get_steering(
            linear_velocity,
            angular_velocity,
            current_velocity)
        steering -= self.steering_controller.step(cte, dt)

        # Calculate the throttle and brake
        throttle = self.throttle_controller.step(dv, dt)
        brake = 0.

        if linear_velocity < 0. and current_velocity < self.vehicle_min_velocity:
            # Keep car in place when it is stopped
            throttle = 0.
            brake = 700.
        elif throttle < .1 and dv < 0.:
            throttle = 0.
            decel = max(dv, self.decel_limit)
            brake = abs(decel) * self.total_mass * self.wheel_radius

        return throttle, brake, steering

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
class Controller(object):
    def __init__(self, *args, **kwargs):
        vehicle_mass    = kwargs['vehicle_mass']
        fuel_capacity   = kwargs['fuel_capacity']
        self.brake_deadband  = kwargs['brake_deadband']
        self.decel_limit 	= kwargs['decel_limit']
        accel_limit 	= kwargs['accel_limit']
        wheel_radius 	= kwargs['wheel_radius']
        wheel_base 		= kwargs['wheel_base']
        steer_ratio 	= kwargs['steer_ratio']
        max_lat_accel 	= kwargs['max_lat_accel']
        max_steer_angle = kwargs['max_steer_angle']

        self.brake_tourque_const = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius
        self.current_dbw_enabled = False
        yaw_params = [wheel_base, steer_ratio, max_lat_accel, max_steer_angle]
        self.yaw_controller = YawController(*yaw_params)
        self.linear_pid = PID(0.9, 0.0005, 0.07, self.decel_limit, accel_limit)
        self.tau_correction = 0.2
        self.ts_correction = 0.1
        self.low_pass_filter_correction = LowPassFilter(self.tau_correction, self.ts_correction)
        self.previous_time = None
        pass

    def update_sample_step(self):
        current_time = rospy.get_time() 
        sample_step = current_time - self.previous_time if self.previous_time else 0.05
        self.previous_time = current_time
        return sample_step

    def control(self, linear_velocity_setpoint, angular_velocity_setpoint, linear_current_velocity, angular_current, dbw_enabled, final_waypoint1, final_waypoint2, current_location):
        if (not self.current_dbw_enabled) and dbw_enabled:
            self.current_dbw_enabled = True
            self.linear_pid.reset()
            self.previous_time = None
        else:
            self.current_dbw_enabled = False
        linear_velocity_error = linear_velocity_setpoint - linear_current_velocity

        sample_step = self.update_sample_step()

        velocity_correction = self.linear_pid.step(linear_velocity_error, sample_step)
        velocity_correction = self.low_pass_filter_correction.filt(velocity_correction)
        if abs(linear_velocity_setpoint)<0.01 and abs(linear_current_velocity) < 0.3:
            velocity_correction = self.decel_limit
        throttle = velocity_correction
        brake = 0.
        if throttle < 0.:
            decel = abs(throttle)
            #[alexm]NOTE: let engine decelerate the car if required deceleration below brake_deadband
            brake = self.brake_tourque_const * decel if decel > self.brake_deadband else 0.
            throttle = 0.
        
        #[alexm]::NOTE this lowpass leads to sending both throttle and brake nonzero. Maybe it is better to filter velocity_correction
        #brake = self.low_pass_filter_brake.filt(brake)
        #steering = self.yaw_controller.get_steering_pid(angular_velocity_setpoint, angular_current, dbw_enabled)
        #steering = self.yaw_controller.get_steering_pid_cte(final_waypoint1, final_waypoint2, current_location, dbw_enabled)

        #[alexm]::NOTE changed static 10.0 to linear_current_velocity and surprisingly car behave better on low speeds. Need to look close to formulas...
        #PID also improves the same with the factor
        #moved factor into function because limits are checked in that function
        steering = self.yaw_controller.get_steering_calculated(linear_velocity_setpoint, angular_velocity_setpoint, linear_current_velocity)

        return throttle, brake, steering

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
class Controller(object):
    def __init__(self, *args, **kwargs):
        # TODO: Implement
        self.vehicle_mass = kwargs['vehicle_mass']
        self.fuel_capacity = kwargs['fuel_capacity']
        self.brake_deadband = kwargs['brake_deadband']
        self.decel_limit = kwargs['decel_limit']
        self.accel_limit = kwargs['accel_limit']
        self.wheel_radius = kwargs['wheel_radius']
        self.wheel_base = kwargs['wheel_base']
        self.steer_ratio = kwargs['steer_ratio']
        self.max_lat_accel = kwargs['max_lat_accel']
        self.max_steer_angle = kwargs['max_steer_angle']

        self.min_speed = 0.0
        self.yaw_contoller = YawController(self.wheel_base,
                                           self.steer_ratio,
                                           self.min_speed,
                                           self.max_lat_accel,
                                           self.max_steer_angle)

        kp = 0.1
        ki = 0.007
        kd = 0.0
        minth = 0.0
        maxth = 0.2
        self.throttle_contoller = PID(kp, ki, kd, minth, maxth)

        tau = 150
        ts = 75
        self.lpf_velocity = LowPassFilter(tau, ts)
        # self.lpf_steering = LowPassFilter(tau, ts)

        self.t0 = rospy.get_time()

    def control(self, proposed_twist, current_twist, dbw_enabled):
        # TODO: Change the arg, kwarg list to suit your needs
        # Return throttle, brake, steer
        if not dbw_enabled:
            self.throttle_contoller.reset()
            return 0.0, 0.0, 0.0

        proposed_linear_velocity = proposed_twist.twist.linear.x
        proposed_angular_velocity = proposed_twist.twist.angular.z
        current_linear_velocity = current_twist.twist.linear.x
        current_angular_velocity = current_twist.twist.angular.z

        current_linear_velocity = self.lpf_velocity.filt(current_linear_velocity)   # Filter current linear velocity
        err_linear_velocity = proposed_linear_velocity - current_linear_velocity

        t1 = rospy.get_time()
        sample_time = t1 - self.t0
        self.t0 = t1

        throttle = self.throttle_contoller.step(err_linear_velocity, sample_time)
        brake = 0.0
        if proposed_linear_velocity == 0.0 and current_linear_velocity < 0.1:
            throttle = 0.0
            brake = 400     # from walkthrough
        elif throttle < 0.1 and err_linear_velocity < 0:
            throttle = 0.0
            decel = max(err_linear_velocity, self.decel_limit)
            brake = abs(decel) * self.vehicle_mass * self.wheel_radius

        steering = self.yaw_contoller.get_steering(proposed_linear_velocity,
                                                   proposed_angular_velocity,
                                                   current_linear_velocity)

        return throttle, brake, steering

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import step [as 别名]
class Controller(object):
    def __init__(self, **ros_param):
        """
        :param ros_param:
        Note:
            sample time (sec) is based on the dbw node frequency 50Hz
            low_pass filter:
                val = w * cur_val + (1 - w) * prev_val
                w is 0 ~ 1
        """
        # used ros_param
        self.vehicle_mass = ros_param['vehicle_mass']
        # self.fuel_capacity = ros_param['fuel_capacity']
        self.brake_deadband = ros_param['brake_deadband']
        self.decel_limit = ros_param['decel_limit']
        self.accel_limit = ros_param['accel_limit']
        self.wheel_radius = ros_param['wheel_radius']

        self.last_time = rospy.get_time()

        # low pass filter for velocity
        self.vel_lpf = LowPassFilter(0.5, .02)

        # Init yaw controller
        min_speed = 0.1   # I think min_speed
        self.steer_controller = YawController(min_speed, **ros_param)
        self.throttle_lpf = LowPassFilter(0.05, 0.02)  # w = 0.28

        # Init throttle PID controller
        # TODO: tweaking
        kp = 0.5
        ki = 0.005
        kd = 0.1
        acc_min = 0.
        acc_max = 0.5 #self.accel_limit
        self.throttle_controller = PID(kp, ki, kd, acc_min, acc_max)

    def control(self, target_linear_velocity, target_angular_velocity,
                      cur_linear_velocity, dbw_status):
        # Check input info is ready
        if not dbw_status:
            self.throttle_controller.reset()
            return 0., 0., 0.

        # dbw enabled: control!
        cur_linear_velocity = self.vel_lpf.filt(cur_linear_velocity)

        # get steer value
        steering = self.steer_controller.get_steering(target_linear_velocity,
                                                      target_angular_velocity,
                                                      cur_linear_velocity)

        # get throttle (could be < 0 so it will be updated by `get brake` as well)
        vel_err = target_linear_velocity - cur_linear_velocity

        current_time = rospy.get_time()
        sample_time = current_time - self.last_time
        self.last_time = current_time

        throttle = self.throttle_controller.step(vel_err, sample_time)

        # get brake
        brake = 0
        if target_linear_velocity == 0. and cur_linear_velocity < 0.1:
            throttle = 0
            brake = 400  # N * m - hold the car in place for the red traffic light
        elif throttle < .1 and vel_err < 0.:
            throttle = 0.
            decel_velocity = max(vel_err, self.decel_limit)   # attention this value is < 0
            # if less than brake_deaband, we don't need to add brake
            # The car will deceleration by friction just release peddle
            if abs(decel_velocity) > self.brake_deadband:
                brake = abs(decel_velocity) * self.vehicle_mass * self.wheel_radius
            else:
                brake = 0

        return throttle, brake, steering

    def reset(self):
        self.throttle_controller.reset()