def reset(self):
self.xerror = 0
self.yerror = 0
self.controllerX = pid.PID()
self.controllerY = pid.PID()
self.controllerX.SetKp(25.0)
self.controllerX.SetKi(0.0)
self.controllerY.SetKp(25.0)
self.controllerY.SetKi(0.0)
self.x_correction = 0
self.y_correction = 0
self.prev_gray = None
self.tracks = []
self.draw_trails = True
self.xerror = 0
self.yerror = 0
self.last_command = 0
self.frame = None
self.queue = Queue(maxsize=1)
def __init__(self):
global hertz
global tolerance
self.odom = None
self.tolerance = tolerance #allowed imprecision for reaching a waypoint
self.current_goal = None #current goal from list of goals
self.vel_curr = 0.0 #current velocity
self.angle_curr = 0.0 #current wheel angle
self.kill = False
self.vel_angle = VelAngle()
#util classes
self.waypoints = waypoint_handler.WaypointHandler(self.tolerance)
self.angle_pid_controller = pid.PID(1,0,0,0,0)
self.vel_pid_controller = pid.PID(1,0,0,0,0)
#publishers
self.vel_angle_p = rospy.Publisher('/vel_angle', VelAngle, queue_size=10)
self.xyz_waypoint_pub = rospy.Publisher('/xyz_waypoints', PointStamped, queue_size=10, latch = True)
#subscribers
self.odom_s = rospy.Subscriber('/pose_and_speed', Odometry, self.odom_callback, queue_size=10)
self.point_cloud_s = rospy.Subscriber('/2d_point_cloud', PointCloud2, self.point_cloud_callback, queue_size=10)
self.killswitch_s = rospy.Subscriber('/stop', EmergencyStop, self.killswitch_callback, queue_size=10)
self.waypoints_s = rospy.Subscriber('/waypoints', WaypointsArray, self.waypoints_callback, queue_size=10)
rospy.init_node('the_overmind', anonymous=False)
self.control()
def __init__(self,
max_brake,
max_throttle,
max_steer_angle,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
vehicle_mass,
fuel_capacity,
brake_deadband,
wheel_radius):
self.throttle_pid = pid.PID(kp=3, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle)
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle)
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = yaw_controller.YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.wheel_radius = wheel_radius
self.max_throttle = max_throttle
self.max_brake = max_brake
self.max_brake_torque = (vehicle_mass + fuel_capacity*GAS_DENSITY) * wheel_radius * max_brake
self.clk = rospy.get_time()
self.count = 0
def __init__(self,
max_brake,
max_throttle,
max_steer_angle,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
wheel_radius,
vehicle_mass,
fuel_capacity):
self.throttle_pid = pid.PID(kp=1.5, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle)
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle)
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = yaw_controller.YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
self.clk = rospy.get_time()
self.max_acc = max_throttle
self.max_dec = max_brake
self.wheel_radius = wheel_radius
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.max_brk_trq = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius * self.max_dec # -1809 N*m
def __init__(self, *args, **kwargs):
# Timesteps
self.dt = 0.1 # 10Hz
self.prevtime = rospy.get_time()
# TODO: Pull PID Gains out to Launch Files
# TODO: Tune Coefficients
# Initialize Gains on PID Controllers
self.PIDCont_Thr = pid.PID(kp=1.0,ki=0.04,kd=0.1,mn= 0.0,mx=1.0)
self.PIDCont_Brk = pid.PID(kp=200.0,ki=0.00,kd=0.0,mn= 0.0,mx=10000.0)
if STEER_CTE:
# cross track error based steering
self.PIDCont_Str = pid.PID(kp=0.25,ki=0.075,kd=1.5,mn=-0.5,mx=0.5)
else:
# angular velocity error based steering
self.PIDCont_Str = pid.PID(kp=4.0,ki=0.5,kd=0.05,mn=-0.5,mx=0.5)
self.YawCont_Str = yc.YawController(wheel_base=2.8498, steer_ratio=14.8, min_speed=10.0, max_lat_accel=3.0, max_steer_angle=8.0)
# Initialize Low Pass Filters
self.LPFilt_Thr = lowpass.LowPassFilter(tau=0.0,ts=self.dt)
self.LPFilt_Brk = lowpass.LowPassFilter(tau=0.0,ts=self.dt)
if STEER_CTE:
# cross track error based steering
self.LPFilt_Str = lowpass.LowPassFilter(tau=0.7,ts=self.dt)
else:
# angular velocity error based steering
self.LPFilt_Str = lowpass.LowPassFilter(tau=0.5,ts=self.dt)
self.first = True # first pass flag
pass
def __init__(self, p, workers, target_hand_count, target_priv_node_count,
pub_delete_odds, hand_delete_odds, hand_expand_odds,
pub_expand_odds, adjust_every):
self.workers = workers
self.p = p
self.target_hand_count = target_hand_count
self.target_priv_node_count = target_priv_node_count
self.next_adjust = adjust_every
self.adjust_every = adjust_every
self.pub_delete_odds = pub_delete_odds
self.hand_delete_odds = hand_delete_odds
self.hand_expand_odds = hand_expand_odds
self.pub_expand_odds = pub_expand_odds
self.rng = []
for i in xrange(64):
self.rng.append(lzp.get_rng())
self.run = True
if self.target_hand_count > 0:
self.hc_pid = pid.PID(1.0, 0.0, 30.0)
self.hc_pid.setPoint(self.target_hand_count)
else:
self.hc_pid = None
if self.target_priv_node_count > 0:
self.nc_pid = pid.PID(1.0, 0.0, 10.0)
self.nc_pid.setPoint(self.target_priv_node_count)
else:
self.nc_pid = None
self.work_thread = threading.Thread(target=self.work_loop)
self.work_thread.daemon = True
self.work_thread.start()
self.adjust_retval = np.zeros(3, dtype=np.uint64)
def __init__(self, decel_limit, accel_limit, max_steer_angle,
max_lat_accel, min_speed, wheel_base, steer_ratio,
vehicle_mass, wheel_radius, max_throttle, max_brake):
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.max_steer_angle = max_steer_angle
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
# self.throttle_pid = pid.PID(kp = 0.6, ki = 0.004, kd = 0.2, mn=decel_limit, mx=accel_limit)
# self.throttle_pid = pid.PID(kp = 40.0, ki = 0.0, kd = 0.7, mn=max_brake, mx=max_throttle) # percents cte
self.throttle_pid = pid.PID(kp=1.5,
ki=0.0,
kd=0.01,
mn=max_brake,
mx=max_throttle) # cte is m/s
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5,
ki=0.0,
kd=0.2,
mn=-max_steer_angle,
mx=max_steer_angle) # ki = 0.04
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.clk = rospy.get_time()
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._lg_stab = LogConfig(name="Logger", period_in_ms=10)
self._lg_stab.add_variable('acc.x', "float")
self._lg_stab.add_variable('acc.y', "float")
self._lg_stab.add_variable('acc.zw', "float")
self._lg_stab.add_variable('gyro.x', "float")
self._lg_stab.add_variable('gyro.y', "float")
self._lg_stab.add_variable('gyro.z', "float")
# PID for Z velocity??
# self._lg_stab.add_variable('acc.z', "float")
# self._lg_stab.add_variable("", "float")
self._cf.open_link(link_uri)
print("Connecting to %s" % link_uri)
self._acc_x = 0.0
self._acc_y = 0.0
self._acc_zw = 0.0
self._gyro_x = 0.0
self._gyro_y = 0.0
self._gyro_z = 0.0
# self._acc_z = 0.0
# self._vel_z = 0.0
# ROLL/PITCH
maxangle = 0.25
kpangle = 2.0
kiangle = 0.0
kdangle = 0.1
self._acc_pid_x = pid.PID(0, 0, kpangle, kiangle, kdangle, -maxangle,
maxangle)
self._acc_pid_y = pid.PID(0, 0, kpangle, kiangle, kdangle, -maxangle,
maxangle)
self._acc_pid_z = pid.PID(0, 0, 10, 0.0005, 0.1, 1 / 6, 2)
self._gyro_x_pid = pid.PID(0, 0, kpangle, kiangle, kdangle, -maxangle,
maxangle)
self._gyro_y_pid = pid.PID(0, 0, kpangle, kiangle, kdangle, -maxangle,
maxangle)
# self._gyro_z_pid = pid.PID(
# 0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
self._is_connected = True
# self._acc_log = []
self.exit = False
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._lg_stab = LogConfig(name="Logger", period_in_ms=10)
self._lg_stab.add_variable('acc.x', "float")
self._lg_stab.add_variable('acc.y', "float")
self._lg_stab.add_variable('acc.zw', "float")
#self._lg_stab.add_variable('stabilizer.roll', "float")
#self._lg_stab.add_variable('stabilizer.pitch', 'float')
#PID for Z velocity??
#self._lg_stab.add_variable('acc.z', "float")
#self._lg_stab.add_variable("", "float")
self._cf.open_link(link_uri)
print("Connecting to %s" % link_uri)
self._acc_x = 0.0
self._acc_y = 0.0
self._acc_zw = 0.0
#self._actual_roll = 0.0
#self._actual_pitch = 0.0
#self._acc_z = 0.0
#self._vel_z = 0.0
#ROLL/PITCH
maxangle = 5
kpangle = 3.5
kiangle = 0.002
kdangle = 1
self._acc_pid_x = pid.PID(0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
self._acc_pid_y = pid.PID(0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
self._acc_pid_z = pid.PID(0, 0, 2, 0.018, 2, 1/6, 2)
#self._pitch_pid = pid.PID(0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
#self._roll_pid = pid.PID(0, 0, kpangle, kiangle, kdangle, -maxangle, maxangle)
self._is_connected = True
#self._acc_log = []
self.exit = False
def __init__(self, parser):
"""Initialize the flying code."""
self.pose = pose.Pose()
self.cmd = None
self.marker_id = 0
self.flying = False
self.armed = False
self.image_count = 0
self.height_pid = pid.PID(False)
self.height_pid.set_set_point(
20.0) # We want to get to height of gate.
self.height_pid.set_output_limits(-1.0, 1.0)
self.height_pid.set_initial_output(-1.0)
self.height_pid.set_tunings(0.02, 0.0, 0.0, True)
self.missed_data = 0
parser.add_argument('-i',
'--id',
help='marker ID to find',
required=False,
type=int,
default=2)
parser.add_argument('-t',
'--ttyname',
help='Serial tty to transmitter.',
required=False,
default='/dev/ttyACM0')
def test_without_when(self):
mocker = Mocker()
mock_time = mocker.replace('time.time')
mock_time()
mocker.result(1.0)
mock_time()
mocker.result(2.0)
mock_time()
mocker.result(3.0)
mock_time()
mocker.result(4.0)
mock_time()
mocker.result(5.0)
mocker.replay()
controller = pid.PID(P = 0.5, I = 0.5, D = 0.5,
setpoint = 0, initial = 12)
self.assertEqual(controller.calculate_response(6), -3)
self.assertEqual(controller.calculate_response(3), -4.5)
self.assertEqual(controller.calculate_response(-1.5), -0.75)
self.assertEqual(controller.calculate_response(-2.25), -1.125)
mocker.restore()
mocker.verify()
def __init__(self, dry_mass, alt, fuel, thrust, fuel_consumption, vel, state, target_alt, time_steps, gravity=9.8): self.dry_mass = dry_mass self.alt = alt self.fuel = fuel self.thrust = thrust self.vel = vel self.acc = 0 self.throttle = 0 self.fuel_consumption = 10 self.state = state self.p_state = state self.state_n = 0 self.target_alt = target_alt self.pid = pid.PID(0.25, 0.0005, 1.8) self.pid(self.target_alt-self.alt, time_steps) self.time_steps = time_steps self.g = gravity self.time = 0 self.k = 0 self.estimated_touchdown = 0 self.data_keys = [ 'alt', 'fuel', 'vel', 'acc', 'throttle', 'ed', 't/w', 'state_n', 'state', 'k', 'desired_throttle', 'pid_p', 'pid_i', 'pid_d', 'desired_acceleration', 'ΔV', 'estimated touchdown' ]
def main():
i2c = busio.I2C(board.SCL, board.SDA)
# wir verwenden den 1115
ads = ADS.ADS1115(i2c)
# Channel 0 auslesen
chan_0 = AnalogIn(ads, ADS.P0)
# GPIO.setmode(GPIO.BOARD)
GPIO.setup(HEATER_PIN, GPIO.OUT)
pid_regulator = pid.PID(1.5, 0.2, 0.6)
pid_regulator.setPoint(TARGET_TEMPERATURE)
print("{:>5}\t{:>5}\t{:>5}\t{}".format('raw0', 'v0', 'temp', 'pid'))
while True:
heater_voltage = chan_0.voltage
temperature = (heater_voltage - 1.25) / 0.05
pid_value = pid_regulator.update(temperature)
pid_value = min(100, max(0, pid_value))
print("{:>5}\t{:>5.3f}\t{:>5.3f}\t{:>5.3f}".format(
chan_0.value, heater_voltage, temperature, pid_value))
regulate(pid_value)
def __init__(self, P = -35, I = -0.02 , D = 0, frecuencia = 1000, ciclo = 100, salida = 13):
self.corriendo = True
# Inicio del PID con las constantes
# y ajuste de su setpoint
self.pid = pid.PID(P, I, D)
self.pid.setPoint(-500)
# Revisar si ya se establecio
# enumeracion en la Pi
if GPIO.getmode() is None or GPIO.getmode() is GPIO.BOARD:
GPIO.setmode(GPIO.BCM)
# Ajustar pines de salida para el
# puente h
GPIO.setup(12, GPIO.OUT)
GPIO.setup(16, GPIO.OUT)
GPIO.output(12, GPIO.HIGH)
GPIO.output(16, GPIO.LOW)
# Ajustar salida al pin establecido
GPIO.setup(salida, GPIO.OUT)
# Inicar PWM
self.PWM = GPIO.PWM(salida, frecuencia)
self.PWM.start(ciclo)
def __init__(self, vehicle_mass, fuel_capacity, accel_limit, decel_limit,
wheel_base, wheel_radius, steer_ratio, max_lat_accel,
max_steer_angle, brake_deadband):
# TODO: Implement
self.velocity_controller = pid.PID(kp=K_P,
ki=K_I,
kd=K_D,
mn=decel_limit,
mx=accel_limit)
self.yaw_controller = yaw_controller.YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=1.,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.yaw_filter = lowpass.LowPassFilter(YAW_TAU, YAW_TS)
# need to calculate torque
self.total_mass = vehicle_mass + (fuel_capacity * GAS_DENSITY)
self.wheel_radius = wheel_radius
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
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):
# TODO: Implement
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_lat_accel = max_lat_accel
self.max_steer_angle = max_steer_angle
self.yaw_controller = YawController(wheel_base,
steer_ratio,
MIN_SPEED,
max_lat_accel,
max_steer_angle)
self.throttle_controller = pid.PID(kp=0.5, ki=0.002, kd=0.25,
mn=-self.max_steer_angle, mx=self.max_steer_angle)
self.timestamp = rospy.get_time()
def __init__(self):
self.AtDeg = 7.0
self.maxAtDeg = 7.0
self.minAtDeg = -7.0
self.minVERTICALSPEED = -10.0
self.maxVERTICALSPEED = 15.0
self.workVERTICALSPEED = 15
self.maxRoll = 35
self.maxDeg = 45
self.tALTITUDE = 1000.0
self.tsleep = 0.3
self.dVERTICALSPEED = 1.2
self.KVERTSPEED = 60 #для пустого 60 для макс массы 90
self.MaxTakeoffMass = 89100 #0.8312985571587127 ПГ от массы снаряженного пустого
self.mass0 = 48600
self.landingspeed0 = 70
self.Klandingspeed = 1.8
self.TrottleKp = 0.35 #0.220
self.TrottleKi = 0.0003
self.TrottleKd = 0.4
self.TrottlePid = pid.AviaPID(self.TrottleKp, self.TrottleKi,
self.TrottleKd)
self.headingKp = 0.075
self.headingKi = 0.000001
self.headingKd = 0.150
self.headingPid = pid.PID(self.headingKp, self.headingKi,
self.headingKd, -7, 7)
self.old_met = vessel.met
def __init__(self, parmams):
self.vehicle_mass = parmams['vehicle_mass']
self.fuel_capacity = parmams['fuel_capacity']
self.brake_deadband = parmams['brake_deadband']
self.decel_limit = parmams['decel_limit']
self.accel_limit = parmams['accel_limit']
self.wheel_radius = parmams['wheel_radius']
self.wheel_base = parmams['wheel_base']
self.steer_ratio = parmams['steer_ratio']
self.max_lat_accel = parmams['max_lat_accel']
self.max_steer_angle = parmams['max_steer_angle']
self.pid_accel = PID.PID(0.3,
0.05,
0.3,
mn=self.decel_limit,
mx=self.accel_limit)
self.yaw_cont = yaw_controller.YawController(self.wheel_base,
self.steer_ratio, 0.1,
self.max_lat_accel,
self.max_steer_angle)
self.vel_LP = lowpass.LowPassFilter(0.5, 0.02)
def __init__(self, ramp=None):
#threading.Thread.__init__(self)
self.error = ''
self.comms = {}
self.comms['temperature'] = DateDataPullClient('rasppi83', 'mgw_temp', exception_on_old_data=True)
try:
#self.comms['temperature sample'] = DateDataPullClient('rasppi98', 'omicron_TPD_sample_temp', exception_on_old_data=True, port=9002, timeout=0.1)
self.comms['temperature sample'] = DateDataPullClient('10.54.7.193', 'omicron_TPD_sample_temp', exception_on_old_data=True, port=9002, timeout=0.1)
except socket.timeout:
self.comms['temperature sample'] = None
self.error = 'socket'
#self.comms['pressure'] = DateDataPullClient('rasppi98', 'omicron_pvci_pull')
self.values = {}
self.values['pid'] = 0
self.values['setpoint'] = None
self.values['temperature'] = None
self.values['old_temperature'] = -9998
self.values['time'] = 0
self.start_values = {}
self.start_values['time'] = -9999
self.start_values['temperature'] = -9999
self.zero = 0 # Offset for PID control
self.pid = PID.PID(pid_p=0.2, pid_i=0.05, pid_d=0)
self.quit = False
#self.wait_time = 0.05
if ramp <= 0:
raise ValueError('Ramp parameter \"{}\" not larger than zero'.format(ramp))
self.ramp = ramp
self.get_temperature()
def do_coast(self, target_altitude):
"""Coast until out of the atmosphere, keeping apoapsis at `altitude`
Parameters:
`altitude`: the target altitude to hold
"""
self.logger.info("Coasting out of atmosphere to apoapsis of %s",
target_altitude)
self.vessel.auto_pilot.target_pitch = 0
coast_pid = pid.PID(P=0.3, I=0, D=0)
coast_pid.setpoint(target_altitude)
# Until we exit the planets atmosphere
atmosphere_depth = self.vessel.orbit.body.atmosphere_depth
with self.conn.stream(getattr, self.vessel.flight(),
"mean_altitude") as altitude, self.conn.stream(
getattr, self.vessel.orbit,
"apoapsis_altitude") as apoapsis:
while altitude() < atmosphere_depth:
self.vessel.control.throttle = self.limit(
self.vessel.control.throttle +
coast_pid.update(apoapsis()))
self.auto_stage()
time.sleep(0.1)
def __init__(self):
"""
Setup of the ROS node. Publishing computed commands happens at 100Hz.
"""
self.reached_start = False
self.reached_goal = False
self.last_dof = None
self.target_index = 0
self.step_size = 1
self.time_points = None
# ----- Controller Setup ----- #
# stores maximum COMMANDED joint velocity
self.max_cmd = MAX_CMD_VEL * np.eye(7)
# stores current COMMANDED joint velocity
self.cmd = np.eye(7)
# P, I, D gains
p_gain = 60.0
i_gain = 0
d_gain = 20.0
self.P = p_gain * np.eye(7)
self.I = i_gain * np.eye(7)
self.D = d_gain * np.eye(7)
self.controller = pid.PID(self.P, self.I, self.D, 0, 0)
self.joint_sub = None
self.is_shutdown = False
def __init__(self, reconfig_server, rate=100.0, interpolation='minjerk'):
self._dyn = reconfig_server
self._fjt_ns = "position_joint_trajectory_controller/follow_joint_trajectory"
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._arm = franka_interface.ArmInterface(synchronous_pub=True)
self._enable = franka_interface.RobotEnable()
self._interpolation = interpolation
# Verify joint control mode
self._server.start()
self._alive = True
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic
# reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._stopped_velocity = 0.0001
self._goal_error = dict()
self._path_thresh = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._arm.joint_names():
self._pid[joint] = pid.PID()
# Create our spline coefficients
self._coeff = [None] * len(self._arm.joint_names())
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):
# TODO: Implement
self.yaw_controller = yaw_controller.YawController(
wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.
# min and max throttle
mn = 0.
mx = 0.2
self.throttle_controller = pid.PID(kp, ki, kd, mn, mx)
# 1/(2pi*tau) = cutoff freq
tau = 0.5
# sample time
ts = .02
self.vel_lpf = lowpass.LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
pass
def __init__(self):
"""
Set up the node. There is no main loop, all publishing
will happen in response to scan callbacks.
"""
rospy.init_node('pid_chaser')
rospy.Subscriber('/scan', LaserScan, self.scan_callback, queue_size=1)
self.vel_pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist)
self.p_pub = rospy.Publisher('p_error', Float32)
self.i_pub = rospy.Publisher('i_error', Float32)
self.d_pub = rospy.Publisher('d_error', Float32)
# These should probably come from the parameter server as
# well...
self.target_distance = 1.25
self.max_vel = .2
self.twist = Twist()
#INSERT CODE HERE TO READ PARAMETERS
# ZEROS NEED TO BE REPLACED WITH CORRECT PARAMETER VALUES.
self.controller = pid.PID(0, 0, 0, -.1, .1)
rospy.spin()
def test_pid(P=0.2, I=0.0, D=0.0, L=100):
"""Self-test PID class
.. note::
...
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1/i))
if i>9:
pid.SetPoint = 1
time.sleep(0.02)
---
"""
pid = PID.PID(P, I, D)
pid.SetPoint = 0.0
pid.setSampleTime(0.01)
END = L
feedback = 0
feedback_list = []
time_list = []
setpoint_list = []
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1 / i))
if i > 9:
pid.SetPoint = 1
time.sleep(0.02)
feedback_list.append(feedback)
setpoint_list.append(pid.SetPoint)
time_list.append(i)
time_sm = np.array(time_list)
time_smooth = np.linspace(time_sm.min(), time_sm.max(), L - 1)
# feedback_smooth = spline(time_list, feedback_list, time_smooth)
# Using make_interp_spline to create BSpline
#helper_x3 = make_interp_spline(time_list, feedback_list)
#feedback_smooth = helper_x3(time_smooth)
plt.plot(time_smooth, feedback_list) #feedback_smooth)
plt.plot(time_list, setpoint_list)
plt.xlim((0, L))
plt.ylim((min(feedback_list) - 0.5, max(feedback_list) + 0.5))
plt.xlabel('time (s)')
plt.ylabel('PID (PV)')
plt.title('TEST PID')
plt.ylim((1 - 0.5, 1 + 0.5))
plt.grid(True)
plt.show()
def __init__(self, vehicle_mass, wheel_radius, accel_limit, decel_limit, max_steer_angle):
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
self.accel_limit = accel_limit
self.decel_limit = decel_limit
self.max_steer_angle = max_steer_angle
self.steer_pid = pid.PID(kp=0.15, ki=0, kd=0.99, mn=-max_steer_angle, mx=max_steer_angle)
self.timestamp = rospy.get_time()
def track_blobs():
global rawBlobs, pub_command, pub_stop
Z_MAX = 0.4 # maximum speed
zero_count = 0
while(True):
rospy.sleep(0.001)
command = zero()
command.linear.x = 0.35 # update values; .7 = too fast
mergedBlobs = mergeBlobs()
trackingBlob = None
# print mergedBlobs.keys()
if "greenline" in mergedBlobs.keys() and len(mergedBlobs["greenline"]) > 0:
trackingBlob = mergedBlobs["greenline"][0]
if trackingBlob is None:
if zero_count < 1000:
zero_count += 1
print zero_count
else:
pub_stop.publish(Empty())
continue
zero_count = 0
# pid error (Proportional-Integral-Derivative (PID) Controller)
p = 0.009 # update values
i = 0 # can leave this as zero
d = 0 # update values; .5 = crazy turn
controller = pid.PID(p, i, d)
controller.start()
center = rawBlobs.image_width//2 # the center of the image
centerOffset = center - trackingBlob.x # the offset that the ball need to travel
cor = controller.correction(centerOffset) # added, right angular speed you want
# print cor
# print "Tracking Blob Object Attr: ", trackingBlob.name, "<<" # added AS
command.angular.z = cor
# if centerOffset > 20: # if the offset is bigger than 20
# command.angular.z = corr
# print([command.angular.z, centerOffset/rawBlobs.image_width])
# elif centerOffset < -20: # if the offset is smaller than -20
# command.angular.z = corr
# command.angular.z = max(-Z_MAX, speed) # turn right and follow the ball
# print([command.angular.z, centerOffset/rawBlobs.image_width])
pub_command.publish(command) # publish the twist command to the kuboki node
def __init__(self, front: motor.Motor, left: motor.Motor,
right: motor.Motor, back: motor.Motor):
self.front = front
self.left = left
self.right = right
self.back = back
self.pitch_pid = pid.PID([1, 1, 1])
self.roll_pid = pid.PID([1, 1, 1])
self.yaw_pid = pid.PID([1, 1, 1])
self.current_pitch = 0.0
self.current_roll = 0.0
self.current_yaw = 0.0
self.target_pitch = 0.0
self.target_roll = 0.0
self.target_yaw = 0.0
self.throttle = 0.0
self.throttle_squared = 0.0
def __init__(self, cfmonitor):
self._cfmonitor = cfmonitor
self._pid = pid.PID(kp=10000.0, ki=5000.0, kd=15000.0,
integ_max=50000.0,
out_min=-4000, out_max=4000)
self._target_pressure = 100.0
self._thrust_center = 40000
self._auto = False
self._pid.CreateWindow('thrust')
def test_with_when(self):
controller = pid.PID(P = 0.5, I = 0.5, D = 0.5,
setpoint = 0, initial = 12,
when = 1)
self.assertEqual(controller.calculate_response(6, 2), -3)
self.assertEqual(controller.calculate_response(3, 3), -4.5)
self.assertEqual(controller.calculate_response(-1.5, 4), -0.75)
self.assertEqual(controller.calculate_response(-2.25, 5), -1.125)
