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 main(): elbowPid = PIDController(10., 1, 1) shoulderPid = PIDController(1., .001, .1) lastTime = time.time() print "actual, delinearizedSetPoint, afterPID, target" while True: reading = readAin(AIN3, ELBOW_RANGE) log.debug("Read: %s" % reading) pistonPressure = getElbowPressureFromGravity(reading) now = time.time() dt = now - lastTime elbowRate = 0 # shoulderControl = 0 if sinWave: elbowRate = elbowPid.update(math.sin(now/2.)/2.+.5, reading, dt) # shoulderControl = shoulderPid.update(math.sin(now/3.)/8.+.85, reading, dt) else: elbowRate = elbowPid.update(elbowSetPoint, reading, dt) # shoulderControl = shoulderPid.update(shoulderSetPoint, reading, dt) relevantPressure = pistonPressure[1 if elbowRate > 0 else 0] kv = getValveCommandFromControlSignal(elbowRate, relevantPressure) print "%s, %s, %s, %s" % (reading, kv, elbowRate, math.sin(now/2.)/2.) elbowPair = mapControlToElbowPwmPair(kv) # shoulderPair = mapControlToShoulderPwmPair(shoulderControl) executeElbowPwmPair(elbowPair) # executeShoulderPwmPair(shoulderPair) lastTime = now time.sleep(.01) # because the valve response rate is 10Hz
def main(): elbowPid = PIDController(10., 1, 1) shoulderPid = PIDController(1., .001, .1) lastTime = time.time() print "actual, delinearizedSetPoint, afterPID, target" while True: reading = readAin(AIN3, ELBOW_RANGE) log.debug("Read: %s" % reading) pistonPressure = getElbowPressureFromGravity(reading) now = time.time() dt = now - lastTime elbowRate = 0 # shoulderControl = 0 if sinWave: elbowRate = elbowPid.update( math.sin(now / 2.) / 2. + .5, reading, dt) # shoulderControl = shoulderPid.update(math.sin(now/3.)/8.+.85, reading, dt) else: elbowRate = elbowPid.update(elbowSetPoint, reading, dt) # shoulderControl = shoulderPid.update(shoulderSetPoint, reading, dt) relevantPressure = pistonPressure[1 if elbowRate > 0 else 0] kv = getValveCommandFromControlSignal(elbowRate, relevantPressure) print "%s, %s, %s, %s" % (reading, kv, elbowRate, math.sin(now / 2.) / 2.) elbowPair = mapControlToElbowPwmPair(kv) # shoulderPair = mapControlToShoulderPwmPair(shoulderControl) executeElbowPwmPair(elbowPair) # executeShoulderPwmPair(shoulderPair) lastTime = now time.sleep(.01) # because the valve response rate is 10Hz
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_P_positive(self): kp = 0.01 ki = 0 kd = 0 state = 0 minOutput = 0.0 maxOutput = 5.0 pid = PIDController(kp, ki, kd, state, minOutput, maxOutput) currentState = 4900 targetState = 5000 timeDelta = 0.134 # timeDelta is irrelevant for P output = pid.compute(currentState, targetState, timeDelta) self.assertEqual(output, 1.0)
def test_P_zero(self): kp = 1 ki = 0 kd = 0 state = 0 minOutput = -10 maxOutput = 10 pid = PIDController(kp, ki, kd, state, minOutput, maxOutput) currentState = 0.0 targetState = 0.0 timeDelta = 1 # timeDelta is irrelevant for P output = pid.compute(currentState, targetState, timeDelta) self.assertTrue(output == 0)
def test_P_negative(self): kp = 1 ki = 0 kd = 0 state = 0 minOutput = -10 maxOutput = 10 pid = PIDController(kp, ki, kd, state, minOutput, maxOutput) currentState = 0.1 targetState = 0 timeDelta = 10009 # timeDelta is irrelevant for P output = pid.compute(currentState, targetState, timeDelta) self.assertEqual(output, -0.1)
def __init__(self): self.image_pub = rospy.Publisher("image_topic_2", Image) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image, self.image_callback) self.depth_sub = rospy.Subscriber("/camera/depth/image", Image, self.depth_callback) self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=1) self.app = App(0) self.image_updated = False self.depth_updated = False self.image = None self.depth = None self.control = PIDController(Kp=0.1, Ki=0, Kd=0)
def test_D_zero(self): kp = 0 ki = 0 kd = 1 state = 1 minOutput = 0.0 maxOutput = 500.0 pid = PIDController(kp, ki, kd, state, minOutput, maxOutput) currentState = 1 targetState = 13 # a change in targetState should not influence derivative timeDelta = 1 output = pid.compute(currentState, targetState, timeDelta) self.assertEqual(output, 0.0)
def test_D_one(self): kp = 0 ki = 0 kd = 1 state = 1 minOutput = -500.0 maxOutput = 500.0 pid = PIDController(kp, ki, kd, state, minOutput, maxOutput) currentState = 2 targetState = 1 timeDelta = 1 output = pid.compute(currentState, targetState, timeDelta) self.assertEqual(output, -1)
def __init__(self): self.pids = {} # self.pids['aileron'] = PIDController( # 'aileron', p=0.8, i=0.01, d=0.30, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0)) # self.pids['pitch'] = PIDController( # 'pitch', p=0.6, i=0.01, d=0.40, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0)) # self.pids['throttle'] = PIDController( # 'throttle', p=1.6, i=0.30, d=0.80, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0)) # self.pids['yaw'] = PIDController( # 'yaw', p=0.5, i=0.01, d=0.20, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0)) self.pids['aileron'] = PIDController('aileron', p=0.8, i=0.01, d=0.30, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0)) self.pids['pitch'] = PIDController('pitch', p=0.6, i=0.01, d=0.40, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0)) self.pids['throttle'] = PIDController('throttle', p=1.6, i=0.30, d=0.80, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0)) self.pids['yaw'] = PIDController('yaw', p=0.5, i=0.01, d=0.20, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0)) self.setpoints = { 'aileron': 0.0, 'pitch': 0.0, 'throttle': 0.0, 'yaw': 0.0 }
def test_I_count(self): kp = 0 ki = 1 kd = 0 state = 1 minOutput = -500.0 maxOutput = 500.0 pid = PIDController(kp, ki, kd, state, minOutput, maxOutput) currentState = 2 targetState = 1 timeDelta = 0.5 output = pid.compute(currentState, targetState, timeDelta) self.assertEqual(output, -0.5) currentState = 3 targetState = 2.5 timeDelta = 0.5 output = pid.compute(currentState, targetState, timeDelta) self.assertEqual(output, -0.75) currentState = 300 targetState = 250 timeDelta = 2 output = pid.compute(currentState, targetState, timeDelta) self.assertEqual(output, -100.75) currentState = 400 targetState = 0 timeDelta = 2 output = pid.compute(currentState, targetState, timeDelta) self.assertEqual(output, -500)
class image_converter: def __init__(self): self.image_pub = rospy.Publisher("image_topic_2", Image) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image, self.image_callback) self.depth_sub = rospy.Subscriber("/camera/depth/image", Image, self.depth_callback) self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=1) self.app = App(0) self.image_updated = False self.depth_updated = False self.image = None self.depth = None self.control = PIDController(Kp=0.1, Ki=0, Kd=0) def image_callback(self, data): self.image = self.bridge.imgmsg_to_cv2(data, "bgr8") self.image_updated = True self.callback_uniform() def depth_callback(self, data): self.depth = self.bridge.imgmsg_to_cv2(data, "16UC1") self.depth_updated = True def callback_uniform(self): if self.image_updated and self.depth_updated: cv_image = self.image (rows, cols, channels) = cv_image.shape ret = self.app.run(cv_image) self.image_updated = False self.depth_updated = False if ret is not None: y, x = ret # print y, cols dist = self.depth[int(x), int(y)][0] turn_cmd = Twist() turn_cmd.linear.x = 0.1 if dist > 0 else 0 diff = y - cols / 2 output = self.control.output(-diff) print(output) turn_cmd.angular.z = radians(output) self.cmd_vel.publish(turn_cmd)
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()
class ApogeeSimulator(KFSimulator): brake_angles = [0.] # Servo angle values for drag brakes (rads) def __init__(self, engine): super(ApogeeSimulator, self).__init__() self._eng = engine self.pid = PIDController(TARGET_APOGEE, KP, KI, KD) @property def brake_angle(self): return self.brake_angles[-1] @brake_angle.setter def brake_angle(self, angle): self.brake_angles.append(angle) def _get_drag_factor(self, brake_angle): """Map from drag brake angle to drag factor brake_angle: (rad) returns: velocity (m/s) """ return DRAG_FACTOR * (1 + DRAG_GAIN * np.sin(brake_angle)**2) def _estimate_peak_altitude(self, alt, velocity, brake_angle): term_vel_sqrd = (MASS * -GRAVITY[1]) / self._get_drag_factor(brake_angle) return alt[1] + (term_vel_sqrd / (2 * -GRAVITY[1]) * np.log( (velocity[1]**2 + term_vel_sqrd) / term_vel_sqrd)) def _actuate(self, commanded_brake_angle): """Takes a slew rate for the drag brakes, clamps it, and uses it to compute the new brake angle. """ commanded_brake_rate = commanded_brake_angle - self.brake_angle slew_rate = commanded_brake_rate / CMD_PERIOD clamped_slew_rate = np.clip(slew_rate, -MAX_SERVO_SLEW_RATE, MAX_SERVO_SLEW_RATE) return np.clip((self.brake_angle + (clamped_slew_rate * CMD_PERIOD)), 0, (np.pi / 2)) def _accelerometer_model(self, acceleration): """ Accelerometer sensor model """ accel = acceleration - GRAVITY accel_vector = np.cos(np.arctan2(accel[0], accel[1]) - self.pitch) * np.linalg.norm(accel) return np.random.normal(accel_vector, ACCEL_STD) def _altimeter_model(self, altitude): """ Barometric altimeter sensor model """ return np.random.normal(altitude[1], BARO_STD) def simulate(self): while not self.terminated: # Runs PID controller and returns commanded drag brake angle. # Actuate drag brakes if rocket is coasting est_apogee = self._estimate_peak_altitude( np.array([0, self.kf.x[0]]), np.array([0, self.kf.x[1]]), self.brake_angle) # Get the attempted servo command from the PID controller. # This may not end up being the actual servo angle, because # the servo can only move so fast brake_angle = self._actuate(self.brake_angle + self.pid.update(self.time, est_apogee)) # TODO rewrite this portion; slight messy. sim_time_end = self.time + CMD_PERIOD - SIM_TIME_INC / 2 first = True while self.time < sim_time_end: self.brake_angle = brake_angle self.tick() self.kf.predict() if first: # Barometer sensor inline self.kf.update( np.array([ self._altimeter_model(self.altitude), self._accelerometer_model(self.acceleration) + GRAVITY[1] ])) else: self.kf.update2( np.array([ self._accelerometer_model(self.acceleration) + GRAVITY[1] ])) first = False # Appends values to lists self.estimated_altitude = self.kf.x[0] self.estimated_velocity = self.kf.x[1] self.estimated_acceleration = self.kf.x[2] # Increment the time self.time += SIM_TIME_INC self._print_results() def _print_results(self): print('') print('========== RESULTS ==========') print('PEAK ALTITUDE:: {} meters'.format( max([x[1] for x in self.altitude_values]))) print('=============================') print('') def _calculate_forces(self): # Add Engine thrust and attitude to forces # Gravitational force forces = MASS * GRAVITY # Thrust and rotation forces += self._eng.thrust(self.time) * np.array( [np.sin(self.pitch), np.cos(self.pitch)]) # Air drag forces += self._get_drag_factor( self.brake_angle) * -self.velocity**2 * np.sign(self.velocity) return forces def _terminate(self): return self.velocity[1] < 0 def plot(self, title=None): # create PID graph self.pid.figure('Apogee PID Controller Error') kf_label = 'KF estimation' plt.figure(title or 'Simulation Outcomes') plt.subplot(4, 1, 1) plt.plot(self.time_values, [x[1] for x in self.altitude_values], label='Altitude') plt.plot(self.time_values, self.kalman_altitude_values, label=kf_label) plt.ylabel('Altitude (m)') plt.legend() plt.subplot(4, 1, 2) plt.plot(self.time_values, [x[1] for x in self.velocity_values], label='Velocity') plt.plot(self.time_values, self.kalman_velocity_values, label=kf_label) plt.ylabel('Velocity (m/s)') plt.legend() plt.subplot(4, 1, 3) plt.plot(self.time_values, [x[1] for x in self.acceleration_values], label='Acceleration') plt.plot(self.time_values, self.kalman_acceleration_values, label=kf_label) plt.ylabel('Acceleration (m/s^2)') plt.legend() plt.subplot(4, 1, 4) plt.plot(self.time_values, [np.rad2deg(x) for x in self.brake_angles]) plt.ylabel('Servo Angle (degrees)') plt.xlabel('Time (s)') return plt
def __init__(self, engine): super(ApogeeSimulator, self).__init__() self._eng = engine self.pid = PIDController(TARGET_APOGEE, KP, KI, KD)
from driver import RobotDriver from encoders import EncoderController from pid import PIDController if __name__ == '__main__': encoder_controller = EncoderController( left_motor_pin_a=cfg.LEFT_MOTOR_PIN_A, left_motor_pin_b=cfg.LEFT_MOTOR_PIN_B, right_motor_pin_a=cfg.RIGHT_MOTOR_PIN_A, right_motor_pin_b=cfg.RIGHT_MOTOR_PIN_B, encoder_resolution=cfg.ENCODER_RESOLUTION) pid_controller = PIDController(proportional_coef=cfg.PID_KP, integral_coef=cfg.PID_KI, derivative_coef=cfg.PID_KD, windup_guard=cfg.PID_WINDUP_GUARD, current_time=None) rd = RobotDriver(wheel_diameter=cfg.WHEEL_DIAMETER_MM, wheelbase=cfg.WHEELBASE_MM, max_angular_velocity=cfg.MAX_ANGULAR_VELOCITY_RPM, encoder_controller=encoder_controller, pid_controller=pid_controller) rd.drive_forward(0.2, 0.5) left_velocities = rd.get_encoder_controller().get_left_encoder( ).get_velocity_records() right_velocities = rd.get_encoder_controller().get_right_encoder( ).get_velocity_records()
import krpc from pid import PIDController throttle_controller = PIDController(0.2, 0.2, 0.3) goal_speed = 200 conn = krpc.connect(name='Hello World') vessel = conn.space_center.active_vessel control = vessel.control ref_frame = vessel.orbit.body.reference_frame flight = vessel.flight(ref_frame) while True: output_throttle = throttle_controller.trigger(goal_speed, flight.speed, 1) control.throttle = output_throttle if output_throttle < -0.1: control.brakes = True else: control.brakes = False print("Speed: {}, Throttle: {}".format(flight.speed, output_throttle))
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