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