def __init__(self):
rospy.Subscriber('/scan', LaserScan, self.laser_callback, queue_size=10)
self.publisher = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=10)
self.rangeStarted = False
self.drive_msg = AckermannDriveStamped()
self.pid = PIDController(rospy.Time.now(), PID_KP,PID_KI,PID_KD)
self.publish()
def __init__(self):
self.px = 0
self.py = 0
self.vx = 0
self.vy = 0
self.radius = 30
self.color = (10,100,230)
self.pid_controller = PIDController()
def __init__(self):
self.startpos = [psm.BAM1.pos(), psm.BAM2.pos(), psm.BBM1.pos()]
self.controllers = [
PIDController(Scalar=1),
PIDController(Scalar=1),
PIDController(Scalar=1)
]
self.setpoints = [0, 0, 0]
class BlockFinder(object):
def __init__(self,r, m):
self.pixycam = Pixy()
self.rPID = PIDController(Kp=0.2, Ki=0.05, Kd=0.5, Scalar=0.0002)
self.dPID = PIDController(Kp=1, Ki=0.01, Kd=0.15, Scalar=0.08)
self.r = r#Mapper(psm.BAM1, psm.BAM2, psm.BBS1, psm, True)
self.m = m#DynManager(psm.BAM1, psm.BAM2, 8.7)
def getBlocks(self):
return self.pixycam.getBlocks()
def step(self):
return self.drive()
def drive(self):
ret = self.getBlocks()
largestsize = 0
largestblock = None
for i in ret:
if i.signature!=1:
continue
size = i.width * i.height
if size > largestsize:
largestsize = size
largestblock = i
if largestblock == None:
self.m.setSpeed(0)
self.m.step()
return
#print largestblock.x,largestblock.y
x_diff = 160 - largestblock.x
# blockheight = (25.4*160)/largestblock.height
blockheight = largestblock.height
#print x_diff
print "blockheight", blockheight
output = self.rPID.step(x_diff)
if abs(x_diff) < 30:
print "done turning"
output2 = self.dPID.step(130 - blockheight)
if (abs(130-blockheight) < 30):
print "at target"
return True
print "speed", output2
self.m.setSpeed(output2)
self.m.setTurn(0)
self.m.step()
else:
if (abs(130-blockheight) < 30):
print "at target"
return True
self.m.setSpeed(output)
self.m.setTurn(-1)
m.step()
return False
class twoWallFollow:
def __init__(self):
rospy.Subscriber('/scan', LaserScan, self.laser_callback, queue_size=10)
self.publisher = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=10)
self.rangeStarted = False
self.drive_msg = AckermannDriveStamped()
self.pid = PIDController(rospy.Time.now(), PID_KP,PID_KI,PID_KD)
self.publish()
def laser_callback(self, msg):
ranges = map(lambda x: int(x>1.5), msg.ranges)
ranges= ranges[180:901]
self.rangeStarted = False
#print ranges
currStart = 0
currEnd = 0
currLen = 0
maxLen = 0
maxEnd = 0
maxStart = 0
for i in range(len(ranges)):
if ranges[i]:
#print "empty"
if self.rangeStarted:
pass
else:
currStart = i
#print currStart
#print "started Range at" + str(currStart)
self.rangeStarted = True
else:
#print self.rangeStarted
if self.rangeStarted:
currEnd = i
#print currEnd
currLen = currEnd - currStart
#print currLen
if currLen > maxLen:
print "this"
maxLen = currLen
maxStart = currStart
maxEnd = currEnd
self.rangeStarted = False
else:
continue
targetPoint = (maxEnd + maxStart)/2 + 180 if maxEnd != 0 else 540
print targetPoint
print maxLen
error = (targetPoint * msg.angle_increment) + msg.angle_min
steer_angle = self.pid.update(error, rospy.Time.now())
print steer_angle
self.drive_msg = AckermannDriveStamped()
self.drive_msg.drive.speed = 2.0 if maxLen >= 115 else -1.0
self.drive_msg.drive.steering_angle = steer_angle
def publish(self):
while not rospy.is_shutdown():
self.publisher.publish(self.drive_msg)
rospy.Rate(8).sleep()
def __init__(self):
self.error = 0 # initializes the running error counter
self.d_des = 0.5 # takes desired dist from wall
self.scan1 = []
self.scan2 = []
self.start = True
self.pidVal = 0
rospy.init_node("wall_PID", anonymous=False)
self.sideFlag = 0
self.pid = PIDController(rospy.Time.now().to_sec(), -1.0, 0.0,
0.0) #0.00000001,0.12)
#init the pub/subs
rospy.Subscriber("/side", Int32, self.sideChange)
rospy.Subscriber("/scan", LaserScan, self.callback)
self.angle_pub = rospy.Publisher("/steer_angle_follow",
Float32,
queue_size=1)
def __init__(self):
self.node_name = rospy.get_name()
self.lane_reading = None
self.pub_counter = 0
# Initialize PID parameters
P_d = -0.8
I_d = -0.02
D_d = -0.01
P_theta = -0.8
I_theta = -0.02
D_theta = -0.01
# Initialize PID controller
self.pid_d = PIDController(P_d, I_d, D_d)
self.pid_theta = PIDController(P_theta, I_theta, D_theta)
# Setup parameters
self.setGains()
# Publicaiton
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1)
self.pub_debug = rospy.Publisher("~debug", Pose2D, queue_size=1)
# Subscriptions
self.sub_lane_reading = rospy.Subscriber("~lane_pose",
LanePose,
self.cbPose,
queue_size=1)
# safe shutdown
rospy.on_shutdown(self.custom_shutdown)
# timer
self.gains_timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.getGains_event)
rospy.loginfo("[%s] Initialized " % (rospy.get_name()))
def __init__(self):
self.error = 0 # initializes the running error counter
self.d_des = 0.5 # takes desired dist from wall
self.scan1 = []
self.scan2 = []
self.start = True
self.pidVal = 0
rospy.init_node("wall_PID", anonymous=False)
self.sideFlag = 0
self.pid = PIDController(rospy.Time.now().to_sec(), -1.0,0.0, 0.0)#0.00000001,0.12)
#init the pub/subs
rospy.Subscriber("/side", Int32, self.sideChange)
rospy.Subscriber("/scan", LaserScan, self.callback)
self.angle_pub = rospy.Publisher("/steer_angle_follow", Float32, queue_size = 1)
class Follower:
def __init__(self):
self.px = 0
self.py = 0
self.vx = 0
self.vy = 0
self.radius = 30
self.color = (10,100,230)
self.pid_controller = PIDController()
def update(self,time_passed,mouse_position):
# the mouse's position is the setpoint
# the follower's position is the current value
current_value = (self.px,self.py)
self.vx, self.vy = self.pid_controller.get_u(mouse_position,current_value)
self.px = int(self.px + self.vx*time_passed)
self.py = int(self.py + self.vy*time_passed)
def render(self,screen):
pygame.draw.circle(screen,self.color,(self.px,self.py),self.radius)
def __init__(self, d_des, speed, dist):
self.drive_cmd = AckermannDriveStamped()
self.drive_cmd.drive.speed = speed
self.error = 0 # initializes the running error counter
self.d_des = d_des # takes desired dist from wall
self.dist = dist
self.scan1 = []
self.scan2 = []
self.sideFlag = 0
rospy.init_node("wall_PID", anonymous=False)
self.pid = PIDController(rospy.Time.now().to_sec(), 0.1,0.0, 0.07)#0.00000001,0.12)
#init the pub/subs
self.drive_pub = rospy.Publisher("/vesc/ackermann_cmd_mux/input/teleop", AckermannDriveStamped, queue_size=5)
rospy.Subscriber("left_or_right", Int32, self.leftRight)
rospy.Subscriber("/scan", LaserScan, self.callback)
# START PUB FUNCTION
self.publisher()
class pidDriver:
def __init__(self, d_des, speed, dist):
self.drive_cmd = AckermannDriveStamped()
self.drive_cmd.drive.speed = speed
self.error = 0 # initializes the running error counter
self.d_des = d_des # takes desired dist from wall
self.dist = dist
self.scan1 = []
self.scan2 = []
self.sideFlag = 0
rospy.init_node("wall_PID", anonymous=False)
self.pid = PIDController(rospy.Time.now().to_sec(), 0.1,0.0, 0.07)#0.00000001,0.12)
#init the pub/subs
self.drive_pub = rospy.Publisher("/vesc/ackermann_cmd_mux/input/teleop", AckermannDriveStamped, queue_size=5)
rospy.Subscriber("left_or_right", Int32, self.leftRight)
rospy.Subscriber("/scan", LaserScan, self.callback)
# START PUB FUNCTION
self.publisher()
def leftRight(self, msg):
self.sideFlag = msg
print str(self.sideFlag)
def callback(self,msg):
self.scan1 = []
self.scan2 = []
if self.sideFlag == 1 or self.sideFlag == 0:
for i in range(175, 186):
self.scan1.append(msg.ranges[i]) # gets 90 degree scans and adds them to scan1
for i in range(215, 226):
self.scan2.append(msg.ranges[i]) # gets 100 degree scans and adds them to scan2
if self.sideFlag == 2:
for i in range(895, 906):
self.scan1.append(msg.ranges[i]) # gets 90 degree scans and adds them to scan1
for i in range(855, 866):
self.scan2.append(msg.ranges[i]) # gets 100 degree scans and adds them to scan2
self.main(self.scan1, self.scan2)
def main(self, scan1, scan2):
self.error = 0
total1 = 0 # total distance for averaging (from scan1)
total2 = 0 # total distance for averaging (from scan2)
meanD_x = 0 # average distance taken from scan1
meanD_y = 0 # average dist taken from scan2
for i in scan1:
total1 += i # adds each element of scan1 to total for averaging
for i in scan2:
total2 += i # same as above but for scan2
meanD_x = total1/len(scan1) # average of scan1
meanD_y = total2/len(scan2) # average of scan2
if meanD_x != 0 and meanD_y != 0: # checks if vals are good
#print ("started trig")
d_actual = driveTrig.findDist(meanD_x, meanD_y, 10)
else:
return
print "d_actual: " + str(d_actual)
self.error = self.d_des - d_actual
print "error: " + str(self.error)
pidVal = self.pid.update(self.error, rospy.Time.now().to_sec())
pidVal = pidVal/abs(pidVal) * min(1.0, abs(pidVal)) if pidVal!=0 else 0
# print pidVal
self.drive_cmd.drive.steering_angle = pidVal
def publisher(self):
while not rospy.is_shutdown():
self.drive_pub.publish(self.drive_cmd)
rospy.Rate(10).sleep()
distanceError, s, t)
self.manager.setSpeed(s)
self.manager.setTurn(t)
self.manager.step()
return False
if __name__ == "__main__":
from PID import PIDController
from MarcMapper import Mapper
from PiStorms import PiStorms
from FloatRangeMotorManager import DynManager
from time import sleep
sleep(1)
psm = PiStorms()
rPID = PIDController(Kp=1, Ki=0, Kd=0.1, stepSec=0.05, Scalar=1.4)
dPID = PIDController(Kp=1, Ki=0, Kd=0.15, stepSec=0.05, Scalar=0.001)
r = Mapper(psm.BAM1, psm.BAM2, psm.BBS1, psm, True)
m = DynManager(psm.BAM1, psm.BAM2, 8.7)
p = PointNavigator(m, r, rPID, dPID, psm.BAS2)
p.setPoint((2000, 0))
while not psm.isKeyPressed():
print(psm.BAS2.distanceUSEV3())
p.step()
sleep(0.02)
p.manager.setSpeed(0)
p.manager.setTurn(0)
p.manager.step()
class pidDriver:
def __init__(self):
self.error = 0 # initializes the running error counter
self.d_des = 0.5 # takes desired dist from wall
self.scan1 = []
self.scan2 = []
self.start = True
self.pidVal = 0
rospy.init_node("wall_PID", anonymous=False)
self.sideFlag = 0
self.pid = PIDController(rospy.Time.now().to_sec(), -1.0,0.0, 0.0)#0.00000001,0.12)
#init the pub/subs
rospy.Subscriber("/side", Int32, self.sideChange)
rospy.Subscriber("/scan", LaserScan, self.callback)
self.angle_pub = rospy.Publisher("/steer_angle_follow", Float32, queue_size = 1)
# START PUB FUNCTION
def flag(self, msg):
self.start = msg.data
def sideChange(self, msg):
self.sideFlag = msg.data
def callback(self,msg):
self.scan1 = []
self.scan2 = []
for i in range(895, 906):
self.scan1.append(msg.ranges[i]) # gets 90 degree scans and adds them to scan1
for i in range(855, 866):
self.scan2.append(msg.ranges[i]) # gets 100 degree scans and adds them to scan2
self.main(self.scan1, self.scan2)
def main(self, scan1, scan2):
self.error = 0
total1 = 0 # total distance for averaging (from scan1)
total2 = 0 # total distance for averaging (from scan2)
meanD_x = 0 # average distance taken from scan1
meanD_y = 0 # average dist taken from scan2
for i in scan1:
total1 += i # adds each element of scan1 to total for averaging
for i in scan2:
total2 += i # same as above but for scan2
meanD_x = total1/len(scan1) if len(scan1) > 0 else 0# average of scan1
meanD_y = total2/len(scan2) if len(scan2) > 0 else 0# average of scan2
if meanD_x != 0 and meanD_y != 0: # checks if vals are good
#print ("started trig")
d_actual = driveTrig.findDist(meanD_x, meanD_y, 10)
else:
return
print "d_actual: " + str(d_actual)
self.error = self.d_des - d_actual
print "error: " + str(self.error)
self.pidVal = self.pid.update(self.error, rospy.Time.now().to_sec())
self.pidVal = self.pidVal/abs(self.pidVal) * min(1.0, abs(self.pidVal)) if self.pidVal!=0 else 0
self.publisher()
def publisher(self):
print("published angle")
self.angle_pub.publish(self.pidVal)
class lane_controller(object):
def __init__(self):
self.node_name = rospy.get_name()
self.lane_reading = None
self.pub_counter = 0
# Initialize PID parameters
P_d = -0.8
I_d = -0.02
D_d = -0.01
P_theta = -0.8
I_theta = -0.02
D_theta = -0.01
# Initialize PID controller
self.pid_d = PIDController(P_d, I_d, D_d)
self.pid_theta = PIDController(P_theta, I_theta, D_theta)
# Setup parameters
self.setGains()
# Publicaiton
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1)
self.pub_debug = rospy.Publisher("~debug", Pose2D, queue_size=1)
# Subscriptions
self.sub_lane_reading = rospy.Subscriber("~lane_pose",
LanePose,
self.cbPose,
queue_size=1)
# safe shutdown
rospy.on_shutdown(self.custom_shutdown)
# timer
self.gains_timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.getGains_event)
rospy.loginfo("[%s] Initialized " % (rospy.get_name()))
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name,
value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def setGains(self):
v_bar = 0.5 # nominal speed, 0.5m/s
k_theta = -2.0
k_d = -(k_theta**2) / (4.0 * v_bar)
theta_thres = math.pi / 6
d_thres = math.fabs(k_theta / k_d) * theta_thres
d_offset = 0.0
self.v_bar = self.setupParameter("~v_bar", v_bar) # Linear velocity
# FIXME: AC aug'17: are these inverted?
self.k_d = self.setupParameter("~k_d", k_theta) # P gain for theta
self.k_theta = self.setupParameter("~k_theta", k_d) # P gain for d
self.d_thres = self.setupParameter("~d_thres",
theta_thres) # Cap for error in d
self.theta_thres = self.setupParameter("~theta_thres",
d_thres) # Maximum desire theta
self.d_offset = self.setupParameter(
"~d_offset",
d_offset) # a configurable offset from the lane position
def getGains_event(self, event):
v_bar = rospy.get_param("~v_bar")
k_d = rospy.get_param("~k_d")
k_theta = rospy.get_param("~k_theta")
d_thres = rospy.get_param("~d_thres")
theta_thres = rospy.get_param("~theta_thres")
d_offset = rospy.get_param("~d_offset")
params_old = (self.v_bar, self.k_d, self.k_theta, self.d_thres,
self.theta_thres, self.d_offset)
params_new = (v_bar, k_d, k_theta, d_thres, theta_thres, d_offset)
if params_old != params_new:
rospy.loginfo("[%s] Gains changed." % (self.node_name))
rospy.loginfo(
"old gains, v_var %f, k_d %f, k_theta %f, theta_thres %f, d_thres %f, d_offset %f"
% (params_old))
rospy.loginfo(
"new gains, v_var %f, k_d %f, k_theta %f, theta_thres %f, d_thres %f, d_offset %f"
% (params_new))
self.v_bar = v_bar
self.k_d = k_d
self.k_theta = k_theta
self.d_thres = d_thres
self.theta_thres = theta_thres
self.d_offset = d_offset
def custom_shutdown(self):
rospy.loginfo("[%s] Shutting down..." % self.node_name)
# Stop listening
self.sub_lane_reading.unregister()
# Send stop command
car_control_msg = Twist2DStamped()
car_control_msg.v = 0.0
car_control_msg.omega = 0.0
self.publishCmd(car_control_msg)
rospy.sleep(0.5) #To make sure that it gets published.
rospy.loginfo("[%s] Shutdown" % self.node_name)
def publishCmd(self, car_cmd_msg):
#wheels_cmd_msg = WheelsCmdStamped()
#wheels_cmd_msg.header.stamp = stamp
#speed_gain = 1.0
#steer_gain = 0.5
#vel_left = (speed_gain*speed - steer_gain*steering)
#vel_right = (speed_gain*speed + steer_gain*steering)
#wheels_cmd_msg.vel_left = np.clip(vel_left,-1.0,1.0)
#wheels_cmd_msg.vel_right = np.clip(vel_right,-1.0,1.0)
self.pub_car_cmd.publish(car_cmd_msg)
#self.pub_wheels_cmd.publish(wheels_cmd_msg)
def cbPose(self, lane_pose_msg):
self.lane_reading = lane_pose_msg
# Calculating the delay image processing took
timestamp_now = rospy.Time.now()
image_delay_stamp = timestamp_now - self.lane_reading.header.stamp
# delay from taking the image until now in seconds
image_delay = image_delay_stamp.secs + image_delay_stamp.nsecs / 1e9
cross_track_err = lane_pose_msg.d - self.d_offset
heading_err = lane_pose_msg.phi
car_control_msg = Twist2DStamped()
car_control_msg.header = lane_pose_msg.header
car_control_msg.v = self.v_bar #*self.speed_gain #Left stick V-axis. Up is positive
if math.fabs(cross_track_err) > self.d_thres:
cross_track_err = cross_track_err / math.fabs(
cross_track_err) * self.d_thres
car_control_msg.omega = self.k_d * cross_track_err + self.k_theta * heading_err #*self.steer_gain #Right stick H-axis. Right is negative
# controller mapping issue
# car_control_msg.steering = -car_control_msg.steering
# print "controls: speed %f, steering %f" % (car_control_msg.speed, car_control_msg.steering)
# self.pub_.publish(car_control_msg)
# Testing PID controller
debug = False
# Debug
if debug:
debug_msg = Pose2D()
# Construct message according to original P control
debug_msg.x = self.v_bar
debug_msg.y = 0
debug_msg.theta = self.k_d * cross_track_err + self.k_theta * heading_err
# Publish original control message
self.pub_debug.publish(debug_msg)
return
# Update controller
ctl_d = self.pid_d.update(cross_track_err)
ctl_theta = self.pid_theta.update(heading_err)
ctl_v = self.v_bar
ctl_omega = self.k_d * ctl_d + self.k_theta * ctl_theta
# Construct message according to PID controller
debug_msg = Pose2D()
debug_msg.x = ctl_v
debug_msg.theta = ctl_omega
self.pub_debug.publish(debug_msg)
# publish car command
car_control_msg = Twist2DStamped()
car_control_msg.header = lane_pose_msg.header
car_control_msg.v = ctl_v
car_control_msg.omega = ctl_omega
self.publishCmd(car_control_msg)
from PID import PIDController
import time
# Set up option parsing to get connection string
import argparse
parser = argparse.ArgumentParser(description='Control Copter and send commands in GUIDED mode ')
parser.add_argument('--connect',
help="Vehicle connection target string. If not specified, SITL automatically started and used.")
args = parser.parse_args()
connection_string = args.connect
sitl = None
#PID
pid_T = PIDController(proportional = 0.15, derivative_time = 10, integral_time=0)
pid_R = PIDController(proportional = 0.15, derivative_time = 10, integral_time=0)
pid_P = PIDController(proportional = 0.15, derivative_time = 10, integral_time=0)
pid_T.vmin, pid_T.vmax = -0.05, 0.05
pid_R.vmin, pid_R.vmax = -1, 1
pid_P.vmin, pid_T.vmax = -1, 1
pid_T.setpoint = 1.0 #aTargetAltitude(m)
TAltitude = pid_T.setpoint
pid_R.setpoint=0 #distance between centre of line and centre of frame
TRoll=pid_R.setpoint
pid_P.setpoint=0 #distance between centre of line and centre of frame
TPitch=pid_P.setpoint
baseThrust = 0.55
baseRoll = 0
basePitch = 0
def __init__(self,r, m):
self.pixycam = Pixy()
self.rPID = PIDController(Kp=0.2, Ki=0.05, Kd=0.5, Scalar=0.0002)
self.dPID = PIDController(Kp=1, Ki=0.01, Kd=0.15, Scalar=0.08)
self.r = r#Mapper(psm.BAM1, psm.BAM2, psm.BBS1, psm, True)
self.m = m#DynManager(psm.BAM1, psm.BAM2, 8.7)
import imutils
from PID import PIDController
#ser = serial.Serial('/dev/ttyACM0',9600)
# multiple cascades: https://github.com/Itseez/opencv/tree/master/data/haarcascades
#https://github.com/Itseez/opencv/blob/master/data/haarcascades/haarcascade_frontalface_default.xml
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
#cap = WebcamVideoStream(src=0).start()
cap = cv2.VideoCapture(0)
val=90
Center_x=0
#-----------------------------------PID------------------------------------------
pid = PIDController(proportional = 0.015, derivative_time = 0, integral_time=0)
pid.vmin, pid.vmax = -10, 10
pid.setpoint = 0.0 #aTargetDifference(m)
TDifference = pid.setpoint
baseAngle = 90
pidout = 0
i=0
while 1:
i+=1
center_x=[]
center_y=[]
ret,img = cap.read()
img = cv2.resize(img, (120, 140))
if i%1 ==0:
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# greenUpper = (64, 255, 255)
# pts = deque(maxlen = bufferSize)
# greenLower = (20, 139, 21)
# greenUpper = (35, 255, 255)
# greenLower = (20, 117, 166)
# greenUpper = (99, 189, 255)
# greenLower = (29, 88, 161)
# greenUpper = (47, 255, 255)
greenLower = (27, 76, 98)
greenUpper = (87, 255, 255)
camera = cv2.VideoCapture(2)
camera.set(cv2.CAP_PROP_FPS, 60)
pid = PIDController(proportional=25.0, derivative_time=1, integral_time=100)
pid.vmin, pid.vmax = -10000, 10000
pid.setpoint = 600
baseThrust = 43000
rollTrim = 1
pitchTrim = 1.5
yawTrim = 0
cflib.crtp.init_drivers(True)
r = RadioDriver()
# cf_urls = r.scan_interface(None)
# print(cf_urls)
# uri = cf_urls[0][0]
cf = Crazyflie()
cf.open_link("radio://0/90/250K")
cf.commander.send_setpoint(0, 0, 0, 0)
if not self.manager.isBusy():
if self.turnState == 0:
self.manager.turnLeft(self.turnSpeed, self.turnDuration)
elif self.turnState == 1:
self.manager.turnRight(self.turnSpeed,
self.turnDuration * 2)
else:
self.manager.turnLeft(self.turnSpeed, self.turnDuration)
self.turnDuration += self.turnAdd
self.turnState = (self.turnState + 1) % 3
else:
print("Follow Mode")
if self.sensor.distanceUSEV3() > self.searchDistance:
self.moveState = 1
self.turnDuration = 1
self.turnState = 0
self.manager.stopAll()
return
self.manager.driveStraight(self.controller.step(lastSensorReading),
1)
psm = PiStorms()
MANAGER = MotorManager(psm.BAM1, psm.BAM2, True)
PID = PIDController()
F = Follower(MANAGER, psm.BAS2, PID)
while not psm.isKeyPressed():
F.step()
F.manager.stopAll()
import argparse
parser = argparse.ArgumentParser(
description='Control Copter and send commands in GUIDED mode ')
parser.add_argument(
'--connect',
help=
"Vehicle connection target string. If not specified, SITL automatically started and used."
)
args = parser.parse_args()
connection_string = args.connect
sitl = None
#PID
pid = PIDController(proportional=0.15, derivative_time=0, integral_time=0)
pid.vmin, pid.vmax = -0.05, 0.05
pid.setpoint = 1, 5 #aTargetAltitude(m)
TAltitude = pid.setpoint
baseVelocity = 0.5
pidout = 0
# Start SITL if no connection string specified
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
# Connect to the Vehicle
print('Connecting to vehicle on: %s' % connection_string)
class pidDriver:
def __init__(self):
self.error = 0 # initializes the running error counter
self.d_des = 0.5 # takes desired dist from wall
self.scan1 = []
self.scan2 = []
self.start = True
self.pidVal = 0
rospy.init_node("wall_PID", anonymous=False)
self.sideFlag = 0
self.pid = PIDController(rospy.Time.now().to_sec(), -1.0, 0.0,
0.0) #0.00000001,0.12)
#init the pub/subs
rospy.Subscriber("/side", Int32, self.sideChange)
rospy.Subscriber("/scan", LaserScan, self.callback)
self.angle_pub = rospy.Publisher("/steer_angle_follow",
Float32,
queue_size=1)
# START PUB FUNCTION
def flag(self, msg):
self.start = msg.data
def sideChange(self, msg):
self.sideFlag = msg.data
def callback(self, msg):
self.scan1 = []
self.scan2 = []
for i in range(895, 906):
self.scan1.append(
msg.ranges[i]) # gets 90 degree scans and adds them to scan1
for i in range(855, 866):
self.scan2.append(
msg.ranges[i]) # gets 100 degree scans and adds them to scan2
self.main(self.scan1, self.scan2)
def main(self, scan1, scan2):
self.error = 0
total1 = 0 # total distance for averaging (from scan1)
total2 = 0 # total distance for averaging (from scan2)
meanD_x = 0 # average distance taken from scan1
meanD_y = 0 # average dist taken from scan2
for i in scan1:
total1 += i # adds each element of scan1 to total for averaging
for i in scan2:
total2 += i # same as above but for scan2
meanD_x = total1 / len(scan1) if len(
scan1) > 0 else 0 # average of scan1
meanD_y = total2 / len(scan2) if len(
scan2) > 0 else 0 # average of scan2
if meanD_x != 0 and meanD_y != 0: # checks if vals are good
#print ("started trig")
d_actual = driveTrig.findDist(meanD_x, meanD_y, 10)
else:
return
print "d_actual: " + str(d_actual)
self.error = self.d_des - d_actual
print "error: " + str(self.error)
self.pidVal = self.pid.update(self.error, rospy.Time.now().to_sec())
self.pidVal = self.pidVal / abs(self.pidVal) * min(
1.0, abs(self.pidVal)) if self.pidVal != 0 else 0
self.publisher()
def publisher(self):
print("published angle")
self.angle_pub.publish(self.pidVal)
