def pid_process(output, p, i, d, objCoord, centerCoord):
# signal trap to handle keyboard interrupt
signal.signal(signal.SIGINT, signal_handler)
# create a PID and initialize it
p = PID(p.value, i.value, d.value)
p.initialize()
# loop indefinitely
while True:
# calculate the error
error = centerCoord.value - objCoord.value
print(centerCoord.value,objCoord.value,error)
# update the value
output.value = p.update(error)
def __init__(self, proportional_gain, integral_gain, differential_gain, stepper_motor, caliper, error_margin,
steppermotor_frequency_limits, settling_time, name, setpoint_offset, interrupt_ignore_time):
"""This class controls a single steppermotor-caliper feedback loop, by moving the load to a given setpoint.
Args:
proportional_gain: P gain of controller
integral_gain: I gain of controller
differential_gain: D gain of controller
stepper_motor: StepperMotor object
caliper: Caliper object
error_margin: The maximum error in absolute terms in mm (so error_margin=10 -> +-10mm)
steppermotor_frequency_limits: tuple with the minimum and maximum allowed steppermotor frequencies
settling_time: the time in seconds that the position reading should stay within the setpoint +- error_margin range to stop
name: name for debugging
setpoints_offset: This is the offset that when given as a setpoint should move the camera to the middle of the first well
interrupt_ignore_time: The time to ignore interrupts for in seconds when temp_disable_interrupts is called
"""
self.pid = PID(p=proportional_gain, i=integral_gain, d=differential_gain) # P I D controller
self.steppermotor = stepper_motor # The stepper motor moving the load
self.caliper = caliper # The caliper providing position feedback.
self.stop_loop_event = threading.Event() # This is set when the control loop stops
self.setpoint = None # Current setpoint
self.error_margin = error_margin
self.step_frequency_min, self.step_frequency_max = steppermotor_frequency_limits
self.name = name
self.settling_time = settling_time
self.setpoint_offset = setpoint_offset
self.interrupt_ignore_time = interrupt_ignore_time
self.start_settling_time = None # timestamp when settling started
self.settling = False # true if within allowed error band
self.captured_data = [] # Stores captured data for visualization and debugging purposes
def __init__(self,
can_bus_stream,
ground_obstacles_stream,
ground_traffic_lights_stream,
waypoints_stream,
control_stream,
name,
flags,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
ground_obstacles_stream.add_callback(self.on_obstacles_update)
ground_traffic_lights_stream.add_callback(
self.on_traffic_lights_update)
waypoints_stream.add_callback(self.on_waypoints_update)
erdos.add_watermark_callback([
can_bus_stream, ground_obstacles_stream,
ground_traffic_lights_stream, waypoints_stream
], [control_stream], self.on_watermark)
self._name = name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._can_bus_msgs = deque()
self._obstacle_msgs = deque()
self._traffic_light_msgs = deque()
self._waypoint_msgs = deque()
def __init__(self,
name,
city_name,
depth_camera_name,
flags,
log_file_name=None,
csv_file_name=None):
super(ERDOSAgentOperator, self).__init__(name)
self._flags = flags
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._map = CarlaMap(city_name)
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
self._world_transform = []
self._depth_imgs = []
self._traffic_lights = []
self._obstacles = []
self._vehicle_pos = None
self._vehicle_acc = None
self._vehicle_speed = None
self._wp_angle = None
self._wp_vector = None
self._wp_angle_speed = None
(self._depth_intrinsic, self._depth_transform,
self._depth_img_size) = self.__setup_camera_tranforms(
name=depth_camera_name, postprocessing='Depth')
def __init__(self, main_window):
#super(Main).__init__()
super().__init__()
#self.class_main=class_main
self.setupUi(self)
self.main_window = main_window
self.btn_read.clicked.connect(
self.readparam) # We define the purpose of the buttons
self.btn_stpm.clicked.connect(self.Stop_m)
self.btn_strtm.clicked.connect(self.Start_m)
self.btn_Stc.toggled.connect(self.monitoring_both)
self.btn_Clear.clicked.connect(self.clear_graph)
self.btn_apply.clicked.connect(self.applyparam)
# ------- INITIALISATION ---------
#self.line_setp1.setText(str(round(float(execution(ser, ":SOUR:PRES?")[0:4]), 3))) # Add' # ' when PACE is not connected, remove '#' when it is
#self.line_slewrate1.setText(str(round((float(execution(ser, ":SOUR:SLEW?")[0:-1]) * 60), 3))) # Add' # ' when PACE is not connected, remove '#' when it is
# -------------PID--------------------------
pid = PID(p=1, i=0.010, d=0.001)
"Pour réaliser le POD, il faut utiliser les coefficients déjà définient, la target correpsond au set-point de la pression que subit le rubis,"
"target = pressure inside the cell"
" PID return alpha, alpha is a coefiecient --> alpha * slew_rate allow us to reach the target pressure inside the cell"
" The closer you are to the target, the smaller alpha is" "alpha ==0 when the target it reached"
"We need to put a set_point for the Pace pressure, it could be very hight beacause the slew-rate will stop when the target will be reached"
"target = (pressure we want inside the cell)*0.95 to avoid going higher than the set point"
def __init__(self,
name,
longitudinal_control_args,
flags,
log_file_name=None,
csv_file_name=None):
""" Initializes the operator to send out control information given
waypoints on its input streams.
Args:
name: The name of the operator.
longitudinal_control_args: A dictionary of arguments to be used to
initialize the longitudinal controller. It needs to contain
three floating point values with the keys K_P, K_D, K_I, where
K_P -- Proportional Term
K_D -- Differential Term
K_I -- Integral Term
flags: A handle to the global flags instance to retrieve the
configuration.
log_file_name: The file to log the required information to.
csv_file_name: The CSV file to log info to.
"""
super(PIDControlOperator, self).__init__(name)
self._flags = flags
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._longitudinal_control_args = longitudinal_control_args
self._pid = PID(
p=longitudinal_control_args['K_P'],
i=longitudinal_control_args['K_I'],
d=longitudinal_control_args['K_D'],
)
self._vehicle_transform = None
self._last_waypoint_msg = None
self._latest_speed = 0
def __init__(self,
waypoints_stream,
can_bus_stream,
control_stream,
name,
flags,
log_file_name=None,
csv_file_name=None):
""" Initializes the operator to send out control information given
waypoints on its input streams.
Args:
name: The name of the operator.
flags: A handle to the global flags instance to retrieve the
configuration.
log_file_name: The file to log the required information to.
csv_file_name: The CSV file to log info to.
"""
waypoints_stream.add_callback(self.on_waypoint)
can_bus_stream.add_callback(self.on_can_bus_update, [control_stream])
self._name = name
self._flags = flags
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
self._vehicle_transform = None
self._last_waypoint_msg = None
self._latest_speed = 0
def setup(self):
# ROS Setup
rospy.init_node('driver')
rospy.Subscriber("driver_commands", String, callback)
# self.tof_pub = rospy.Publisher('tof_readings', String, queue_size=10)
# Device Setup
self.motor1 = Motor(self.tamp, MOTOR1_DIR, MOTOR1_PWM)
self.motor2 = Motor(self.tamp, MOTOR2_DIR, MOTOR2_PWM)
self.encoder1 = Encoder(self.tamp, *ENCODER1, continuous=True)
self.encoder2 = Encoder(self.tamp, *ENCODER2, continuous=True)
# self.encoder1_value = 0
# self.encoder2_value = 0
# TimeOfFlight Sensors
# self.tof1 = TimeOfFlight(self.tamp, TOF1, 1, continuous=False)
# self.tof2 = TimeOfFlight(self.tamp, TOF2, 2, continuous=False)
# self.tof1.enable()
# self.tof2.enable()
# self.tof1_value = 0
# self.tof2_value = 0
# Servo for the Release
self.servo = Servo(self.tamp, SERVO)
self.servo.write(SERVO_CLOSE)
# Controller Params
self.pid = PID(p=P, i=I, d=D)
self.target1 = 0
self.target2 = 0
self.servo_commands = Queue()
def __init__(self):
rospy.init_node('brain')
rospy.Subscriber("ball_info", String, self.ball_info_callback)
rospy.Subscriber("goal_info", String, self.goal_info_callback)
# rospy.Subscriber("tof_readings", String, self.tof_readings_callback)
self.pub = rospy.Publisher('driver_commands', String, queue_size=10)
self.ball_pid = PID(p=BALL_P, i=BALL_I, d=BALL_D)
self.goal_pid = PID(p=GOAL_P, i=GOAL_I, d=GOAL_D)
self.ball_center = 0
self.ball_radius = 0
self.goal_center = 0
self.goal_size = 0
self.state = State.EXPLORE
self.plan = Queue()
# TOF Readings
# self.dist1 = MAX_TOF_DIST
# self.dist2 = MAX_TOF_DIST
# self.danger = 1
self.time = time()
self.index = 0
self.last_commands = [i for i in range(TIMEOUT_THRESHOLD)]
def lost(vstate, vehicle, vs, flightHeight, connection_string):
print vstate
# The vehicle processes images and returns to tracking state if any lock is found.
# Meanwhile it rotates clockwise and maintains height.
target = None # Initialise tuple returned from video stream
frame = None
# Altitude P constant
altlP = 0.3
# Initialise PID constants for altitude control
altlPID = PID(p=altlP, i=0.004, d=0.02)
while vstate == "lost":
# grab the frame from the threaded video stream and return position of line (left/right)
# The last argument (height) is a dummy value. We just want to know if the line was found.
frame, target = detectline3(vs, vehicle, 1.0)
angle = target[0]
offset = target[1]
lineFound = target[2]
if lineFound == True:
vstate = "tracking"
break
else:
# Check that operator has transferred to autopilot using TX switch.
if vehicle.mode.name == "GUIDED":
if connection_string:
height = vehicle.location.global_relative_frame.alt * -1 # This used for SITL
else:
height = vehicle.rangefinder.distance * -1.0 # This used for real flight.
zError = flightHeight - height
vz = altlPID(zError)
send_ned_velocity(0.0, 0.0,
vz) # Stay stationary, but adjust altitude
condition_yaw(6, 1, True) # Rotate clockwise
time.sleep(0.05)
# Show the annotated video frame
show_frame(frame)
return vstate
def __init__(self, pose_stream, waypoints_stream, control_stream, flags):
pose_stream.add_callback(self.on_pose_update)
waypoints_stream.add_callback(self.on_waypoints_update)
erdos.add_watermark_callback([pose_stream, waypoints_stream],
[control_stream], self.on_watermark)
self._flags = flags
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
# Queues in which received messages are stored.
self._waypoint_msgs = deque()
self._pose_msgs = deque()
def __init__(self,
can_bus_stream,
waypoints_stream,
traffic_lights_stream,
obstacles_stream,
point_cloud_stream,
open_drive_stream,
control_stream,
name,
flags,
camera_setup,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
waypoints_stream.add_callback(self.on_waypoints_update)
traffic_lights_stream.add_callback(self.on_traffic_lights_update)
obstacles_stream.add_callback(self.on_obstacles_update)
point_cloud_stream.add_callback(self.on_point_cloud_update)
open_drive_stream.add_callback(self.on_open_drive_map)
erdos.add_watermark_callback([
can_bus_stream, waypoints_stream, traffic_lights_stream,
obstacles_stream, point_cloud_stream
], [control_stream], self.on_watermark)
self._name = name
self._flags = flags
self._camera_setup = camera_setup
self._log_file_name = log_file_name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
if not hasattr(self._flags, 'track'):
# The agent is not used in the Carla challenge. It has access to
# the simulator, and to the town map.
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.debug('Agent running using map')
else:
self._map = None
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
# Queues in which received messages are stored.
self._waypoint_msgs = deque()
self._can_bus_msgs = deque()
self._traffic_lights_msgs = deque()
self._obstacles_msgs = deque()
self._point_clouds = deque()
self._vehicle_labels = {'car', 'bicycle', 'motorcycle', 'bus', 'truck'}
def __init__(self, pose_stream, waypoints_stream, control_stream, flags):
pose_stream.add_callback(self.on_pose_update)
waypoints_stream.add_callback(self.on_waypoints_update)
erdos.add_watermark_callback([pose_stream, waypoints_stream],
[control_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
self._config = global_config
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._pose_msgs = deque()
self._obstacles_msgs = deque()
self._traffic_light_msgs = deque()
self._waypoint_msgs = deque()
self._mpc = None
def __init__(self, name, flags, log_file_name=None, csv_file_name=None):
super(GroundAgentOperator, self).__init__(name)
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._flags = flags
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._can_bus_msgs = deque()
self._pedestrian_msgs = deque()
self._vehicle_msgs = deque()
self._traffic_light_msgs = deque()
self._speed_limit_sign_msgs = deque()
self._waypoint_msgs = deque()
self._lock = threading.Lock()
def callback_PID(self, data):
if data.data == "q":
self.pid_p_val -= 0.01
if data.data == "w":
self.pid_p_val += 0.01
if data.data == "a":
self.pid_i_val -= 0.001
if data.data == "s":
self.pid_i_val += 0.001
if data.data == "z":
self.pid_d_val -= 0.01
if data.data == "x":
self.pid_d_val += 0.01
self.pid_roll = PID(self.pid_p_val, self.pid_i_val, self.pid_d_val)
print("PID: ", self.pid_p_val, " ", self.pid_i_val, " ",
self.pid_d_val)
def __init__(self,
name,
city_name,
flags,
log_file_name=None,
csv_file_name=None):
super(GroundAgentOperator, self).__init__(name)
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._map = CarlaMap(city_name)
self._flags = flags
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._vehicle_pos = None
self._vehicle_acc = None
self._vehicle_speed = None
self._pedestrians = []
self._vehicles = []
self._traffic_lights = []
self._traffic_signs = []
self._wp_angle = None
self._wp_vector = None
self._wp_angle_speed = None
def __init__(self,
name,
flags,
bgr_camera_setup,
log_file_name=None,
csv_file_name=None):
super(PylotAgentOperator, self).__init__(name)
self._flags = flags
self._log_file_name = log_file_name
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._bgr_camera_setup = bgr_camera_setup
self._map = None
if '0.9' in self._flags.carla_version:
from pylot.map.hd_map import HDMap
from pylot.simulation.carla_utils import get_map
if not hasattr(self._flags, 'track'):
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.info('Agent running using map')
elif hasattr(self._flags, 'track'):
from pylot.map.hd_map import HDMap
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
self._waypoint_msgs = deque()
self._can_bus_msgs = deque()
self._traffic_lights_msgs = deque()
self._obstacles_msgs = deque()
self._point_clouds = deque()
self._depth_camera_msgs = deque()
self._vehicle_labels = {'car', 'bicycle', 'motorcycle', 'bus', 'truck'}
self._lock = threading.Lock()
self._last_traffic_light_game_time = -100000
self._last_moving_time = 0
# Num of control commands to override to ensure the agent doesn't get
# stuck.
self._num_control_override = 0
def __init__(self):
self.tvecs = np.ndarray(shape = (1, 3), dtype = float)
self.rvecs = np.ndarray(shape = (1, 3), dtype = float)
self.gray = None
self.thr_cmd = 1700
self.yaw_cmd = 1500
self.targetZ = 0.9
self.neutral_roll = 1500#1431
self.neutral_pitch = 1500# 1495
self.neutral_yaw = 1500
self.neutral_thr = 1759
self.roll_cmd = self.neutral_roll
self.pitch_cmd = self.neutral_pitch
self.roll_counter = 0
self.pitch_counter = 0
self.userNumber = 0
self.pidHoverDown = PID(15, 1.5, 0)
self.trpy_send = Int32MultiArray(data = [self.neutral_thr, self.neutral_roll, self.neutral_pitch, self.neutral_yaw])
self.pidHoverUp = PID(12, 0.5, 0.001) #12, 0.5, 0.001 siegler nicholas metoden
self.pidRoll = PID(6, 0.1, 0) # P=7 or 5
self.pidPitch = PID(6, 0.1, 0) #P=10 or 5
self.pidYaw = PID(2, 0.001, 0) # P=7 or 5
self.pidBlindRoll = PID(5, 0.1, 0.1)
self.pidBlindPitch = PID(5, 0.1, 0.1)
self.old_tvecs = np.ndarray(shape = (1, 3), dtype = float)
self.old_rvecs = np.ndarray(shape = (1, 3), dtype = float)
self.aruco_rate = 0
self.aruco_counter = 0
self.video_init = False
self.dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
self.parameters = aruco.DetectorParameters_create()
with open("/home/mark/catkin_ws/src/three_d_markernode/calibration.yaml") as f:
loadeddict = yaml.load(f)
self.mtxloaded = loadeddict.get('camera_matrix')
self.distloaded = loadeddict.get('dist_coeff')
self.new_cap = False
self.cap_rate = 0
self.debug = True
self.time = 0.0
self.blindY = 0.0
self.blindX = 0.0
def __init__(self, can_bus_stream, obstacles_stream,
ground_obstacles_stream, control_stream, goal, flags):
""" Initializes the operator with the given information.
Args:
goal: The destination pylot.utils.Location used to plan until.
flags: The command line flags passed to the driver.
"""
can_bus_stream.add_callback(self.on_can_bus_update)
obstacles_stream.add_callback(self.on_obstacles_update)
ground_obstacles_stream.add_callback(self.on_ground_obstacles_update)
erdos.add_watermark_callback([can_bus_stream, obstacles_stream],
[control_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
self.config.name + '-csv', self.config.csv_log_file_name)
self._flags = flags
self._goal = goal
# Input retrieved from the various input streams.
self._can_bus_msgs = collections.deque()
self._ground_obstacles_msgs = collections.deque()
self._obstacle_msgs = collections.deque()
# PID Controller
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
# Planning constants.
self.SPEED = self._flags.target_speed
self.DETECTION_DISTANCE = 12
self.GOAL_DISTANCE = self.SPEED
self.SAMPLING_DISTANCE = self.SPEED / 3
self._goal_reached = False
def __init__(self):
self.thrVal = 0
self.rollVal = 0
self.pitchVal = 0
self.yawVal = 0
self.dummy = []
self.xPos = 0
self.yPos = 0
self.xPosCentered = 0 # centered to be in the center of the picture
self.yPosCentered = 0 # dito
self.rotOne = 0 # rotation around z axsis
self.rotTwo = 0 # ratation around y axsis
self.rot_pitch = 0 # ratation around x axsis
self.distance = 0
self.valid = False
self.yImageOffset = 0 #240
self.xImageOffset = 0 #320
self.pid_throttle = PID(0.04, 0.0015, 0.001)
self.pid_roll = PID(0.04, 0.001, 0.0)
self.pid_yaw = PID(0.00001, 0.0001, 0.0)
#self.pid_roll = PID(0.00, 0.0000, 0.0)
self.pid_pitch = PID(0.02, 0.0, 0.05)
self.pidDist = PID(0.006, 0.0001, 0)
self.pidRot = PID(0.006, 0.001, 0)
self.pid_p_val = 0.0
self.pid_i_val = 0.0
self.pid_d_val = 0.0
self.rosMsg = rospy.Subscriber(
'ArUco/data_array',
Float64MultiArray,
self.callback_TMP,
queue_size=1) # Subscriber for data to calculate
self.pidMsg = rospy.Subscriber(
'PID_controls', String, self.callback_PID,
queue_size=1) # Subscriber for data to calculate
def tracking(vstate, vehicle, vs, flightHeight, connection_string):
#In tracking state the vehicle processes images and maintains all data ready to fly autonomously.
#However, it does not actually send velocity vectors unless GUIDED flight mode is selected.
########################### CONFIGURABLE BITS FOR FLIGHT #######################################
# Initialise velocities
# vMax should be between 0.5 and 2.0.
vMax = 0.75 # The forward velocity (due to pitch) in m/s
# Initialise P constant for yaw control
# Yaw controller P constant. Reduce this for more gentle response, increase for more aggressive.
yawP = 0.2 # This is the proportion of the yaw error we attempt to change each time around.
# These can be set to False to increase the frame rate
recordLog = True
########################## END OF CONFIGURABLE BITS ##################################
# framecount is incremented with each image captured and is used to periodically record data.
global framecount
# Display in the terminal console the state we are now in (i.e. tracking)
print vstate
# Initialise PID constants for yaw control
yawPID = PID(p=yawP, i=0.003, d=0.03)
# Altitude P constant
altP = 0.2
# Initialise PID constants for altitude control
altPID = PID(p=altP, i=0.004, d=0.02)
# Safety check on maximum velocity
if not connection_string:
if (vMax < 0.0) or (vMax > 2.0):
print('Maximum velocity vMax out of range - setting to 0.75 m/s')
vMax = 0.75
# The maximum turning angle when the line is at the edge of the image (degrees)
# This value corresponds to the field of view from the centre to the edge of the image.
# It should not be changed!
yawMax = 30.0
# Initialise velocity vectors
# Python does not need variables to be initialised, but (IMHO) it is good practice do do so.
# Also, the first use of a variable sets it's type - so vital to state it's a float (decimal),
# for example, in these cases.
vx = 0.0
vy = 0.0 # Sideways velocity (due to roll) in m/s
vz = 0.0 # Vertical velocity is controlled for us to keep at requested height.
yaw = 0.0
# Open the file to record log data.
f = open("locationdata.txt", "a")
# Now start the loop in which we adjust the height, then
# turn and move towards the detected point on the line.
target = None # Initialise the tuple (it's just an array) returned from video stream
while vstate == "tracking":
framecount = framecount + 1
# First let's make sure the vehicle is at the right height.
# Get height above ground using rangefinder. This is reported in metres beween 0.2 and 7m.
# It is made negative because in we are working in NED space, where negative means up.
if connection_string:
height = vehicle.location.global_relative_frame.alt * -1 # This line for SITL
else:
height = vehicle.rangefinder.distance * -1.0 # Or this for real flight.
# Check actual height and make adjustments as necessary.
# Calculate required z velocity to achieved requested height
#Remember +z is DOWN!
# First calculate the error, then get the correction using a PID controller.
zError = flightHeight - height
vz = altPID(zError)
# This grabs a frame from the video stream and returns position of line (left:-1 / right:+1)
target = detectline1(vs, vehicle)
xPos = target[0] # The value between -1 and 1
lineFound = target[
1] # This is set to True if the line was found (otherwise False, of course)
# Now we work out which way to turn to follow the point on the line.
# The position of the line is held in xPos, from -1 extreme left to +1 extreme right.
# First check that we did find the line last time we got an image (lineFound is True).
if lineFound == True:
# So the line was found. Now set the yaw value as a fraction of the maximum allowed.
# Rememember xPos is a signal from -1 (leftmost) to +1 (rightmost) of the image.
yawError = xPos * yawMax
yaw = yawPID(yawError)
# So only actually send control commands if the vehicle is still in guided mode.
if vehicle.mode.name == "GUIDED":
# The yaw is is currently a number between about -30 to 30 degrees.
# The controller needs a positive number only and to know which direction.
if yaw < 0.0: # So need to turn left - anticlockwise
condition_yaw(abs(yaw), -1) # anticlockwise
else:
condition_yaw(abs(yaw), 1) # clockwise
# Now send the commands tell the vehicle to keep moving forward and adjust for height.
vy = 0.0
vx = vMax
send_ned_velocity(vx, vy, vz)
# Log data and save image every nth frame
n = 10
if framecount % n == 0:
if recordLog == True:
# Append location data to logfile
f.write('\n' + time.strftime("%H_%M_%S_") +
' yawError, ' + str(yawError) + ', Yaw, ' +
str(yaw) + ', Height, ' + str(height) +
', vz, ' + str(vz))
else:
# We didn't find the line and are therefore lost!
# Set the vehicle state flag to "lost" to find the line again.
# This stops the drone in one place and causes it to rotate until it finds the line.
vstate = "lost"
return vstate
def __init__(self, params):
# The parameters for this controller, set by the agent
self.params = params
# PID speed controller
self.pid = PID(p=params['pid_p'], i=params['pid_i'], d=params['pid_d'])
#!/usr/bin/env python
import rospy
from nav_msgs.msg import OccupancyGrid
from omni_msgs.msg import OmniFeedback
from geometry_msgs.msg import PoseStamped
import numpy as np
from pid_controller.pid import PID
import tf2_ros
mapMsg = None
poseMsg = None
pid = PID(p=100, i=50, d=0.0)
pid._target = -0.01
def MapCallback(msg):
global mapMsg
mapMsg = np.asarray(msg.data)
mapMsg = mapMsg.reshape(msg.info.height, msg.info.width)
def PoseCallback(msg):
global poseMsg
poseMsg = msg.pose
def run():
rospy.init_node('phantom_controll', anonymous=False)
rospy.Subscriber('/map', OccupancyGrid, MapCallback)
rospy.Subscriber('/phantom/pose', PoseStamped, PoseCallback)
force_pub = rospy.Publisher('/phantom/force_feedback', OmniFeedback, queue_size=10)
rate = rospy.Rate(100) # 5 hz
def __init__(self, params):
self.params = params
# PID speed controller
self.pid = PID(p=params['pid_p'], i=params['pid_i'], d=params['pid_d'])
from pid_controller.pid import PID pid = PID(p=0.1, i=0.004, d=3.0) output = pid(feedback=get_feedback(pid))
def main():
global last_request
global pixel_pos
global set_mode
rospy.init_node('pixel_to_pos', anonymous=False)
rate = rospy.Rate(20)
mavros.set_namespace('/mavros')
rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, state_callback)
rospy.Subscriber("pixel_coord", Int16MultiArray, callback_vision, queue_size=1)
rospy.Subscriber(mavros.get_topic('local_position', 'pose'), mavros.setpoint.PoseStamped, callback_drone,
queue_size=1)
setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10)
# /mavros/cmd/arming
set_arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
# /mavros/set_mode
set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
while not UAV_state.connected:
rate.sleep()
mavros.command.arming(True)
time.sleep(1)
set_mode(0, 'OFFBOARD')
last_request = rospy.Time.now()
pidHor = PID(0.004, 0.01, 0)
pidVer = PID(0.006, .0001, 0)
may_land = False
while True:
init_drone()
euler = get_drone_euler()
# Marker is too much to the left or right
if pixel_pos[0] < 300 or pixel_pos[0] > 500:
rotate_drone(pidHor, euler)
else:
# Marker is too left or right
if pixel_pos[0] < 380 or pixel_pos[0] > 420:
left_right_adjust_drone(pidVer, euler)
# Marker is too high or low
if pixel_pos[1] < 380 or pixel_pos[1] > 420:
up_down_adjust_drone(pidVer, euler)
lower_dist = 350
higher_dist = 450
if pixel_pos[0] > lower_dist and pixel_pos[0] < higher_dist and pixel_pos[1] > lower_dist and pixel_pos[1] < higher_dist:
if not may_land:
land_drone(euler)
if current_pose.pose.position.z < 0.5:
may_land = True
if may_land:
print("Going down")
current_pose.pose.position.z = 0
setpoint_local_pub.publish(current_pose)
rate.sleep()
return 0
def tracking(vstate, vehicle, vs, flightHeight, connection_string):
#In tracking state the vehicle processes images and maintains all data ready to fly autonomously.
#However, it does not actually send velocity vectors unless GUIDED flight mode is selected.
########################### CONFIGURABLE BITS #######################################
# Initialise velocities
# vMax should be between 0.5 and 2.0.
vMax = 1.0 # The forward velocity (due to pitch) in m/s
# Initialise P constant for yaw control
# Yaw controller P constant. Reduce this for more gentle response, increase for more aggressive.
yawP = 0.25 # This is the proportion of the yaw error we attempt to change each time around.
# We have some idea of how much to one side of the line we are.
# This can be corrected by 'sideslipping' the vehicle, so introducing a sideways velocity.
# This is the largest value permitted, in m/s. The bigger this is, the more the vehicle
# sideslip to try to get back over the line. Maximum suggested is 1 m/s.
vyMax = 0.3
vyP = 0.4 # Sideslip (vy) controller P constant
# These can be set to False to increase the frame rate
recordLog = False
saveImages = False
annotateFrame = True
########################## END OF CONFIGURABLE BITS ##################################
# framecount is incremented with each image captured and is used to periodically record data.
global framecount
# Display in the terminal console the state we are now in (i.e. tracking)
print vstate
# Initialise PID constants for yaw control
yawPID = PID(p=yawP, i=0.003, d=0.03)
# Altitude P constant
altP = 0.3
# Initialise PID constants for altitude control
altPID = PID(p=altP, i=0.004, d=0.02)
# Initialise PID constants for sideslip velocity (vy) control
vyPID = PID(p=vyP, i=0.004, d=0.02)
# Safety check on maximum velocity
if not connection_string:
if (vMax < 0.0) or (vMax > 2.0):
print('Maximum velocity vMax out of range - setting to 0.75 m/s')
vMax = 0.75
# Initialise velocity vectors
# Python does not need variables to be initialised, but (IMHO) it is good practice do do so.
# Also, the first use of a variable sets it's type - so vital to state it's a float (decimal),
# for example, in these cases.
vy = 0.0 # Sideways velocity (due to roll) in m/s
vz = 0.0 # Vertical velocity is controlled for us to keep at requested height.
yaw = 0.0
# Open the file to record key data.
f = open("locationdata.txt", "a")
# The vehicle process images and maintains all data ready to fly autonomously.
# However, it does not actually send velocity vector commands unless GUIDED
# flight mode is selected.
target = None # Initialise tuple returned from video stream
frame = None
while vstate == "tracking":
framecount = framecount + 1
# First let's make sure the vehicle is at the right height.
# Get height above ground using rangefinder. This is reported in metres beween 0.2 and 7m.
if connection_string:
height = vehicle.location.global_relative_frame.alt * -1 # This used for SITL
else:
height = vehicle.rangefinder.distance * -1.0 # This used for real flight.
#print height
# Check actual height and make adjustments as necessary.
# Calculate required z velocity to achieved requested height
#Remember +z is DOWN!
# First calculate the error, then get the correction using a simple P controller and deadband,
# or a full PID controller.
zError = flightHeight - height
vz = altPID(zError)
# grab the frame from the threaded video stream and return position of line (left/right)
frame, target = detectline3(vs, vehicle, height)
bearing = target[0] # In degrees, left is -ve, right +ve
offset = target[
1] # A value from -1 to 1, indicating how far to the side the line is.
lineFound = target[
2] # True if the line was found ok. False if the line was not found.
#print bearing, offset
# Now we work out which way to turn to follow the line.
# First check that we did find the line last time we got an image (lineFound is True).
if lineFound == True:
# Here we set the yaw to match the bearing of the line - we are trying to fly along it!
# Now set the yaw value as a fraction of the maximum allowed.
# First calculate the error, then get the correction using a PID controller.
yawError = bearing
yaw = yawPID(yawError)
# The position of the line is held in 'offset', from -1 extreme left to +1 extreme right.
# Set sideslip to try to get above the line.
# First calculate the error, then get the correction using a simple P controller and deadband,
# or a full PID controller.
vyError = offset * vyMax
vy = vyPID(vyError)
# So only actually send control commands if the vehicle is still in guided mode.
if vehicle.mode.name == "GUIDED":
if bearing < 0.0: # So need to turn left - anticlockwise
condition_yaw(abs(yaw), -1) # anticlockwise
else:
condition_yaw(abs(yaw), 1) # clockwise
# Now send the velocities to the vehicle.
vx = vMax
#send_ned_velocity(0.0, 0.0, vz)
send_ned_velocity(vx, vy, vz)
if annotateFrame == True:
annotate_frame(frame, vstate, height, bearing, offset, vx,
vy, vz)
# Log data and save image every nth frame
n = 10
if framecount % n == 0:
if recordLog == True:
# Append location data to logfile
f.write('\n' + time.strftime("%H_%M_%S_") +
' yawError, ' + str(yawError) + ', Yaw, ' +
str(yaw) + ', Height, ' + str(height) +
', vz, ' + str(vz))
if saveImages == True:
cv2.imwrite(
'images/' + (time.strftime("%H_%M_%S_")) +
str(framecount) + '.jpg', frame)
else:
# We didn't find the line and are therefore lost!
# Set the vehicle state flag to "lost" to find the line again.
# This stops the vehicle in one place and causes it to rotate until it finds the line.
vstate = "lost"
# Show the annotated video frame
show_frame(frame)
return vstate
serGDS = serialComGetPortGWGDS1072AU(
portList) #OPENS A PORT TO A GDS 1072AU DEVICE IF AVAILABLE
serialComConfigurePort2018MAY16a(serGDS) #CONFIGURES THE PORT
del portList[portList.index(
serGDS.port)] #REMOVES THE OPENED PORT FROM THE LIST
serAFG = serialComGetPortGWAFG2005(
portList) #CHECK ME!! DOES THE PORTS FOR THE GDS WORK AFTER THIS
del portList[portList.index(
serAFG.port)] #REMOVES THE OPENED PORT FROM THE LIST
freq_PWM = 90 # HZ
pi = pigpio.pi() #CREATES OBJECT FOR HARDWARE PWM
pid = PID(1.0, 1.0, 1.0) #creats a PID object
##serARD=serial.Serial(portList[0],9600,timeout=0.5)
##
##serARD.flushInput()
##serARD.flushOutput()
##serARD.readlines()
##
##print("WAITING FOR VOLUMEACTUATOR")
##while serARD.inWaiting()==0:
## pass
##
##bla=serARD.readline()
##print(bla.rstrip())
print("CONFIGURING THE GDS")
serGDS.flushInput()
for i in params.keys():
val = drone.get_param(params[i])
if val is not None:
print "found existing value for param", params[i], val
i = val
else:
print "creating new param", params[i], i
drone.create_param(params[i], i)
v_s_switch = True
# Initialize two separate PID controllers for two axis.
# Controller's target is to bring delta angle between image center and object's centroid to zero.
# Controller is not aware of current position of gimbal.
# Since position feedback from gimbal is not available run controller at lower speed than the gimbal can move. Then assume that current position of gimbal is last commanded state.
pidc_X = PID(p=kp, i=ki, d=kd)
pidc_X.target = 0
pidc_Y = PID(p=kp, i=ki, d=kd)
pidc_Y.target = 0
# Initialize gimbal to (0,0,0) angles.
drone.gimbal_set(0.0, 0.0, 0.0)
time.sleep(5.0)
# initialize a rospy.Timer to force controller rate.
control_loop = rospy.Timer(period=rospy.Duration(loop_rate),
callback=control_loop_callback,
oneshot=False)
# subscribe to param update msg
param__update_sub = rospy.Subscriber(
"/" + drone.namespace + "/parameter_updated", String,
# initialize PWM board
pwm = PWM()
# arm the motors
pwm.arm()
# initialize Pressure Sensor
pressure = PressureSensor()
# calibrate Pressure Sensor
pressure.calibrate()
imu = IMU()
sleep(1)
depthPID = PID(p=145, i=0.1, d=1)
depthPID.target = 0.9
headingPID = PID(p=0.75, i=0.01, d=0)
init_heading, _, _ = imu.get_required(0)
headingPID.target = init_heading - 15
count = 0
start = time()
while True:
try:
if not controller.connected():
depth = pressure.get_depth()
heading, roll, accy = imu.get_required(
ref_heading=headingPID.target)
depthCorrection = -int(depthPID(feedback=depth))
headingCorrection = -int(headingPID(feedback=heading))
