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]
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.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)
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)
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()
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
# 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)
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()
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)
