예제 #1
0
 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()
예제 #2
0
 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()
예제 #3
0
    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]
예제 #4
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
예제 #5
0
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()
예제 #6
0
    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)
예제 #7
0
    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()))
예제 #8
0
    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)
예제 #9
0
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)
예제 #10
0
파일: pidDrive.py 프로젝트: Cifer1/PIDstuff
    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()
예제 #11
0
파일: pidDrive.py 프로젝트: Cifer1/PIDstuff
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()
예제 #12
0
                                                       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()
예제 #13
0
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)
예제 #14
0
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)
예제 #15
0
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
예제 #16
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)
예제 #17
0


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)
예제 #18
0
# 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)
예제 #19
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()
예제 #20
0
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)
예제 #21
0
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)