def pid_process(output, p, i, d, box_coord, origin_coord, action): signal.signal(signal.SIGINT, signal_handler) p = PIDController(p.value, i.value, d.value) p.reset() while True: error = origin_coord - box_coord.value output.value = p.update(error)
def pid_process(output, p, i, d, box_coord, origin_coord, action): # signal trap to handle keyboard interrupt signal.signal(signal.SIGINT, signal_handler) # create a PID and initialize it pid = PIDController(p.value, i.value, d.value) pid.reset() # loop indefinitely while True: error = origin_coord - box_coord.value output.value = pid.update(error) logging.info(f'{action} error {error} angle: {output.value}')
def test_pid(kp, ki, kd, stime, use_pid=True): left_encoder = Encoder(motor_pin_a=cfg.LEFT_MOTOR_PIN_A, motor_pin_b=cfg.LEFT_MOTOR_PIN_B, sampling_time_s=stime) left_encoder_counter = EncoderCounter(encoder=left_encoder) left_encoder_counter.start() pid_controller = PIDController(proportional_coef=kp, integral_coef=ki, derivative_coef=kd, windup_guard=cfg.PID_WINDUP_GUARD, current_time=None) max_steps_velocity_sts = cfg.ENCODER_RESOLUTION * cfg.MAX_ANGULAR_VELOCITY_RPM / 60 kit = MotorKit(0x40) left_motor = kit.motor1 velocity_levels = [1000, 2000, 3000, 4000, 5000, 6000, 0] velocity_levels = [3000, 0] sleep_time = 5 velocities_level_records = deque([]) velocities_steps_records = deque([]) pid_velocities_steps_records = deque([]) timestamps_records = deque([]) for v in velocity_levels: pid_controller.reset() pid_controller.set_set_point(v) left_motor.throttle = max(min(1, v / max_steps_velocity_sts), 0) start = time.time() current_time = time.time() while current_time - start < sleep_time: timestamp, measured_steps_velocity_sts = left_encoder_counter.get_velocity( ) # PID control if use_pid: new_steps_velocity_sts = pid_controller.update( -measured_steps_velocity_sts, current_time) left_motor.throttle = max( min(1, new_steps_velocity_sts / max_steps_velocity_sts), 0) else: new_steps_velocity_sts = -1 velocities_level_records.append(v) velocities_steps_records.append(-measured_steps_velocity_sts) pid_velocities_steps_records.append(new_steps_velocity_sts) timestamps_records.append(timestamp) current_time = time.time() left_motor.throttle = 0 left_encoder_counter.finish() records_left = pd.DataFrame({ 'velocity_steps': velocities_steps_records, 'velocity_levels': velocities_level_records, 'pid_velocity_steps': pid_velocities_steps_records, 'timestamp': timestamps_records }) records_left['velocity_ms'] = records_left[ 'velocity_steps'] * cfg.WHEEL_DIAMETER_MM * np.pi / ( 1000 * cfg.ENCODER_RESOLUTION) records_left.set_index('timestamp', drop=True) return records_left
class Controller(object): def __init__( self, dbw_parameters, # Set of base parameters ): self.is_initialized = False self.last_time = rospy.get_time() self.dbw_parameters = dbw_parameters self.steering_controller = YawController( wheel_base = self.dbw_parameters['wheel_base'], steer_ratio = self.dbw_parameters['steer_ratio'], min_speed = self.dbw_parameters['min_vehicle_speed'], max_lat_accel = self.dbw_parameters['max_lat_accel'], max_steer_angle = self.dbw_parameters['max_steer_angle']) self.low_pass_filter_vel = LowPassFilter(samples_num = LOW_PASS_FREQ_VEL * self.dbw_parameters['dbw_rate']) self.low_pass_filter_yaw = LowPassFilter(samples_num = LOW_PASS_FREQ_YAW * self.dbw_parameters['dbw_rate']) self.throttle_controller = PIDController( kp = 0.3, ki = 0.01, kd = 0.01, mn = self.dbw_parameters['vehicle_min_throttle_val'], mx = self.dbw_parameters['vehicle_max_throttle_val'], integral_period_sec = INTEGRAL_PERIOD_SEC) self.steering_controller2 = PIDController( kp = 3.0, ki = 0.0, kd = 1.0, mn = -self.dbw_parameters['max_steer_angle'], mx = self.dbw_parameters['max_steer_angle'], integral_period_sec = INTEGRAL_PERIOD_SEC) def control( self, target_dx, target_dyaw, current_dx, current_dyaw, dbw_status = True): throttle = 0 brake = 0 steering = 0 correct_data = False cur_time = rospy.get_time() current_dx_smooth = self.low_pass_filter_vel.filt(current_dx) target_dyaw_smooth = self.low_pass_filter_yaw.filt(target_dyaw) if not dbw_status: self.is_initialized = False else: if not self.is_initialized: self.reset() self.is_initialized = True dv = target_dx - current_dx_smooth dt = cur_time - self.last_time if dt >= MIN_TIME_DELTA: steering = self.steering_controller.get_steering(target_dx, target_dyaw_smooth, current_dx_smooth) + self.steering_controller2.step(target_dyaw_smooth, dt) steering = max(min(steering, self.dbw_parameters['max_steer_angle']), -self.dbw_parameters['max_steer_angle']) throttle = self.throttle_controller.step(dv, dt) if target_dx <= current_dx_smooth and current_dx_smooth <= self.dbw_parameters['min_vehicle_speed']: throttle = 0.0 brake = self.dbw_parameters['min_vehicle_break_torque'] elif throttle <= self.dbw_parameters['vehicle_throttle_dead_zone']: deceleration = abs(self.dbw_parameters['decel_limit'] * (throttle - self.dbw_parameters['vehicle_throttle_dead_zone']) / (self.dbw_parameters['vehicle_throttle_dead_zone'] - self.dbw_parameters['vehicle_min_throttle_val'])) throttle = 0.0 brake = self.dbw_parameters['vehicle_mass'] * self.dbw_parameters['wheel_radius'] * deceleration correct_data = True self.last_time = cur_time return throttle, brake, steering, correct_data def reset(self): self.steering_controller.reset() self.steering_controller2.reset() self.throttle_controller.reset()