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 __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 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 __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 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 __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_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
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))
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()
def __init__(self, engine):
super(ApogeeSimulator, self).__init__()
self._eng = engine
self.pid = PIDController(TARGET_APOGEE, KP, KI, KD)
