def __init__(self):
self.mode = "m"
self.pitch_hold = False
# self.hdg_hold=False
self.alt_hold = False
self.auto_hold = False
self.speed_hold = False
self.is_connected = 0
self.status = c.OK
self.servos_init = False
self.throttle_updated = False
self.pitch = 0.0
self.roll = 0.0
self.throttle = 0.0
self.heading = 0.0
self.altittude = 0.0
self.speed = 0
self.altPID = pid(0, 0)
self.hdgPID = pid(0, 0)
self.throttlePID = pid(0, 0)
self.elevons = elevons()
self.sensors = sensors()
self.gps = gpsdata()
self.motor = motor_handler(0)
self.camera = camera()
def _reset(self): super()._reset() self.point = None self.canvas.itemconfig(self.point_p, state=tk.HIDDEN) self.pid_s = pid(*self.init_k_s) self.pid_v = pid(*self.init_k_v)
def __init__(self):
# First get an instance of the API endpoint (the connect via web case will be similar)
self.api = local_connect() #from droneapi.lib-->__init__.py,commented by ljx
# Our vehicle (we assume the user is trying to control the first vehicle attached to the GCS)
self.vehicle = self.api.get_vehicles()[0]
self.frame=None
self.timelast=time.time()
#lamda=0.928
self.vx_fopi=fo_pi(1.3079,3.4269,4999)
self.vy_fopi=fo_pi(1.3079,3.4269,4999)
# horizontal velocity pid controller. maximum effect is 10 degree lean
xy_p = balloon_config.config.get_float('general','VEL_XY_P',1.0)
xy_i = balloon_config.config.get_float('general','VEL_XY_I',0.0)
xy_d = balloon_config.config.get_float('general','VEL_XY_D',0.0)
xy_imax = balloon_config.config.get_float('general','VEL_XY_IMAX',10.0)
self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))
# vertical velocity pid controller. maximum effect is 10 degree lean
z_p = balloon_config.config.get_float('general','VEL_Z_P',2.5)
z_i = balloon_config.config.get_float('general','VEL_Z_I',0.0)
z_d = balloon_config.config.get_float('general','VEL_Z_D',0.0)
z_imax = balloon_config.config.get_float('general','VEL_IMAX',10.0)
self.vel_z_pid = pid.pid(z_p, z_i, z_d, math.radians(z_imax))
# velocity controller min and max speed
self.vel_speed_min = balloon_config.config.get_float('general','VEL_SPEED_MIN',1.0)
self.vel_speed_max = balloon_config.config.get_float('general','VEL_SPEED_MAX',4.0)
self.vel_speed_last = 0.0 # last recorded speed
self.vel_accel = balloon_config.config.get_float('general','VEL_ACCEL', 0.5) # maximum acceleration in m/s/s
self.vel_dist_ratio = balloon_config.config.get_float('general','VEL_DIST_RATIO', 0.5)
def __init__(self):
self.mode = "m"
self.pitch_hold = False
# self.hdg_hold=False
self.alt_hold = False
self.auto_hold = False
self.speed_hold = False
self.is_connected = 0
self.status = c.OK
self.servos_init = False
self.throttle_updated = False
self.pitch = 0.0
self.roll = 0.0
self.throttle = 0.0
self.heading = 0.0
self.altittude = 0.0
self.speed = 0
self.altPID=pid(0,0)
self.hdgPID=pid(0,0)
self.throttlePID=pid(0,0)
self.elevons = elevons()
self.sensors = sensors()
self.gps = gpsdata()
self.motor = motor_handler(0)
self.camera = camera()
def __init__(self):
super().__init__()
self.root.bind('<Button-1>', self.__click)
self.point = None
self.init_k_s = [1, 5, 0]
self.init_k_v = [50, 20, 0.1]
self.init_v = 3.0
self.pid_s = pid(*self.init_k_s)
self.pid_v = pid(*self.init_k_v)
self.para_kp_s = tk.StringVar()
self.para_kd_s = tk.StringVar()
self.para_kp_s.set(self.init_k_s[0])
self.para_kd_s.set(self.init_k_s[1])
self.para_kp_v = tk.StringVar()
self.para_kd_v = tk.StringVar()
self.para_ki_v = tk.StringVar()
self.para_kp_v.set(self.init_k_v[0])
self.para_kd_v.set(self.init_k_v[1])
self.para_ki_v.set(self.init_k_v[2])
self.para_v = tk.StringVar()
self.para_v.set(self.init_v)
tk.Label(self.params, text='Kp (s)').grid(row=0, column=2)
tk.Entry(self.params, width=5, textvariable=self.para_kp_s).grid(row=1, column=2)
tk.Label(self.params, text='Kd (s)').grid(row=0, column=3)
tk.Entry(self.params, width=5, textvariable=self.para_kd_s).grid(row=1, column=3)
tk.Label(self.params, text='Kp (v)').grid(row=0, column=4)
tk.Entry(self.params, width=5, textvariable=self.para_kp_v).grid(row=1, column=4)
tk.Label(self.params, text='Kd (v)').grid(row=0, column=5)
tk.Entry(self.params, width=5, textvariable=self.para_kd_v).grid(row=1, column=5)
tk.Label(self.params, text='Ki (v)').grid(row=0, column=6)
tk.Entry(self.params, width=5, textvariable=self.para_ki_v).grid(row=1, column=6)
tk.Label(self.params, text='v').grid(row=0, column=7)
tk.Entry(self.params, width=5, textvariable=self.para_v).grid(row=1, column=7)
self.text_e = self.canvas.create_text((10, 40), anchor='nw')
self.point_p = self.canvas.create_oval([0, 0, 0, 0], fill='red', state=tk.HIDDEN)
def control(gmsg,robot,ball):
mybotpid = pid.pid(x=robot.x,y=robot.y,ball = ball,angle=robot.theta)
if gmsg.status == 1 or gmsg.status == 2:
th = 2*m.atan(gmsg.posetogo.orientation.z)
if abs(robot.x - gmsg.posetogo.position.x)<40 and abs(robot.y - gmsg.posetogo.position.y)<40 and abs(robot.theta - 2*m.atan(gmsg.posetogo.orientation.z))< 1:
if gmsg.status == 2:
print('ball kicked')
robot.kick(ball,1)
gmsg.status = 0
mybotpid.gtg(gmsg.posetogo.position.x,gmsg.posetogo.position.y,robot,ball,thtg=th)
if robot.dribble == 0:
p.collRb(robot,ball)
p.walleffect(robot)
p.walleffect(ball)
if gmsg.status == 0:
mybotpid.gtg(robot.x,robot.y,robot,ball,thtg=robot.theta)
if robot.dribble == 0:
p.collRb(robot,ball)
p.walleffect(robot)
p.walleffect(ball)
mybotpid.gtg(robot.x,robot.y,robot,ball,thtg=robot.theta)
if robot.dribble == 0:
p.collRb(robot,ball)
p.walleffect(robot)
p.walleffect(ball)
gmsg.status = 0
def ex():
UDP_IP = "127.0.0.1"
UDP_PORT = 48001
sock = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))
p = pid.pid(kp=2000.0,
ki=200.0,
kd=-10.0,
tau=0.01,
out_limit_min=-0.03,
out_limit_max=0.05,
t=0.01)
setpoint = 2000.0
while True:
# Receive packet and parse to basic values
data, _addr = sock.recvfrom(1024)
(dataref, value) = parse_raw_packet(data)
if dataref == 'sim/cockpit2/gauges/indicators/altitude_ft_pilot':
measurement = value
yoke_pitch_ratio = p.update(setpoint, measurement)
print('current altitude: ' + str(value) +
' computed yoke pitch ratio: ' + str(yoke_pitch_ratio))
packet = create_raw_packet(
'sim/cockpit2/controls/yoke_pitch_ratio', yoke_pitch_ratio)
sock.sendto(packet, ('192.168.0.94', 49000))
def calculatePowerUpdate(self, position):
# x+=1
# print(position)
# # The "proportional" term should be 0 when we are on the line.
# proportional = position - 2000
#
# # Compute the derivative (change) and integral (sum) of the position.
# derivative = proportional - last_proportional
# integral += proportional
#
# # Remember the last position.
# last_proportional = proportional
#
'''
// Compute the difference between the two motor power settings,
// m1 - m2. If this is a positive number the robot will turn
// to the right. If it is a negative number, the robot will
// turn to the left, and the magnitude of the number determines
// the sharpness of the turn. You can adjust the constants by which
// the proportional, integral, and derivative terms are multiplied to
// improve performance.
'''
# power_difference = proportional/25 + derivative/100 #+ integral/1000;
pidObj = pid.pid()
power_difference = pidObj.calculateDifference(position, self.proportionalCoefficient,
self.derivativeCoefficient,
self.integralCoefficient)
pwmaPower, pwmbPower = self.calculateNewPower( power_difference)
print(position, power_difference)
return pwmaPower, pwmbPower
def __init__(self):
pass
self.comm=comm_layer.Comm_layer(self)
self.cfgName = "temp_reg.config"
self.dataName = "data.csv"
self.config = configparser.ConfigParser()
self.config['Controller']={}
self.config['Server']={}
self.config.read(self.cfgName)
self.ctlConfig=self.config['Controller']
self.serverConfig=self.config['Server']
self.sysinfo = sysinfo.Sysinfo()
self.logfile =self.dataName
self.fh=open(self.logfile, "a+")
#self.logger=dataLogger(self.dataName)
self.stirrer1 = stirrer(13,26)
self.t1 = pt100.PT100()
self.t1.update()
threading.Thread(target=self.t1.run).start()
self.ntc1 = ntc.Ntc('P9_39',beta=3889)
self.ntc2 = ntc.Ntc('P9_37')
self.pid1=pid()
self.pid1.setSetPoint(float(self.ctlConfig.get("SetPoint","20")))
self.pid1.setK_p(float(self.ctlConfig.get("K_p","6")))
self.pid1.setK_i(float(self.ctlConfig.get("K_i","0.01")))
self.pid1.setI_sat_p(float(self.ctlConfig.get("I_sat_p","60")))
self.pid1.setI_sat_n(float(self.ctlConfig.get("I_sat_n","-60")))
self.ramp=float(self.ctlConfig.get("Ramp","0"))
self.setPoint=self.pid1.getSetPoint()
self.ctl1 = relay_ctrl(self.comm,self.pid1,self.t1,self.ctlConfig.get("HeaterPort",16),
self.ctlConfig.get("CoolerPort",17),float(self.ctlConfig.get("CtlPeriod","10")))
self.tilt = tilt2_client.Tilt2()
self.tilt.connect()
self.http1= http_comm(self.serverConfig,self.comm)
self.http1.run()
self.socket_comm = socket_comm.Socket_comm(self.comm)
threading.Thread(target=self.socket_comm.run,daemon=True).start()
def __init__(self, monitor, comedi):
threading.Thread.__init__(self)
self.lock = threading.Lock()
self.time_sleep = 0
self.com = comedi
self.mon = monitor
self.pid = pid()
self.pi = pi()
self.Ts = 0.01
self.lqg = lqg(self.mon)
self.meas_time=dt.datetime.now()
self.offset = 0 # offset mittpunkt bom i volt, bom 2 lördag
self.mon.setOffset(self.offset)
def CallbackPID(msg):
global pub
global lastTime
global pso
# print("PID Callback OK")
velX = msg.velX * 1000
velY = msg.velY * 1000
flag = msg.flag
if flag:
print("Replanned")
pso = PSO(15, 20, 1000, 1, 1, 0.5)
errorInfo.errorX = msg.errorX
errorInfo.errorY = msg.errorY
# print("INitial",velX,velY)
vX, vY = pid(velX, velY, errorInfo, pso)
# vX,vY = velX,velY
# print("Changed",vX,vY)
botAngle = msg.botAngle
# print("BotAngle ", botAngle)
vXBot = vX * cos(botAngle) + vY * sin(botAngle)
vYBot = -vX * sin(botAngle) + vY * cos(botAngle)
# print("Velocity ",vXBot,vYBot)
command_msgs = gr_Robot_Command()
final_msgs = gr_Commands()
command_msgs.id = 0
command_msgs.wheelsspeed = 0
command_msgs.veltangent = vXBot / 1000
command_msgs.velnormal = vYBot / 1000
command_msgs.velangular = 0
command_msgs.kickspeedx = 0
command_msgs.kickspeedz = 0
command_msgs.spinner = False
if (msg.velX == msg.velY and msg.velX == 0):
command_msgs.velnormal = command_msgs.veltangent = 0
# t = rospy.get_rostime()
# currTime = t.secs + t.nsecs/pow(10,9)
# diffT = float(currTime - lastTime)
# command_msgs.nextExpectedX = vXBot*diffT + homePos[0][0];
# command_msgs.nextExpectedY = vYBot*diffT + homePos[0][1];
# final_msgs.timestamp = ros::Time::now().toSec()
final_msgs.isteamyellow = False
final_msgs.robot_commands = command_msgs
# lastTime = currTime
pub.publish(final_msgs)
def __init__(self):
self.cfgName = "temp_reg.config"
self.dataName = "data.csv"
self.config = configparser.ConfigParser()
self.config['Controller'] = {}
self.config['Server'] = {}
self.config.read(self.cfgName)
self.ctlConfig = self.config['Controller']
self.serverConfig = self.config['Server']
self.logfile = self.dataName
self.fh = open(self.logfile, "a+")
#self.logger=dataLogger(self.dataName)
self.stirrer1 = stirrer("P9_14", "P9_12")
self.t1 = pt100.PT100()
self.t1.update()
self.ntc1 = ntc.Ntc('P9_39', beta=3889)
self.ntc2 = ntc.Ntc('P9_37')
self.pid1 = pid()
self.pid1.setSetPoint(float(self.ctlConfig.get("SetPoint", "20")))
self.pid1.setK_p(float(self.ctlConfig.get("K_p", "6")))
self.pid1.setK_i(float(self.ctlConfig.get("K_i", "0.01")))
self.pid1.setI_sat_p(float(self.ctlConfig.get("I_sat_p", "60")))
self.pid1.setI_sat_n(float(self.ctlConfig.get("I_sat_n", "-60")))
self.ramp = float(self.ctlConfig.get("Ramp", "0"))
self.setPoint = self.pid1.getSetPoint()
self.ctl1 = relay_ctrl(self.pid1, self.t1,
self.ctlConfig.get("HeaterPort", "P8_13"),
self.ctlConfig.get("CoolerPort", "P9_42"),
float(self.ctlConfig.get("CtlPeriod", "10")))
print("tilt start", flush=True)
self.tilt = tilt2_client.Tilt2()
self.tilt.connect()
print("http start", flush=True)
self.http1 = http_comm(self)
self.ctl1.setServer(self.http1)
self.http1.run()
def test_controller():
control = pid.pid( Kpid = ( 2.0, 1.0, 2.0 ), now = 0. )
assert near( control.loop( 1.0, 1.0, now = 1. ), 0.0000 )
assert near( control.loop( 1.0, 1.0, now = 2. ), 0.0000 )
assert near( control.loop( 1.0, 1.1, now = 3. ), -0.5000 )
assert near( control.loop( 1.0, 1.1, now = 4. ), -0.4000 )
assert near( control.loop( 1.0, 1.1, now = 5. ), -0.5000 )
assert near( control.loop( 1.0, 1.05,now = 6. ), -0.3500 )
assert near( control.loop( 1.0, 1.05,now = 7. ), -0.5000 )
assert near( control.loop( 1.0, 1.01,now = 8. ), -0.3500 )
assert near( control.loop( 1.0, 1.0, now = 9. ), -0.3900 )
assert near( control.loop( 1.0, 1.0, now =10. ), -0.4100 )
assert near( control.loop( 1.0, 1.0, now =11. ), -0.4100 )
assert near( control.loop( 1.0, 1.0, now =12. ), -0.4100 )
assert near( control.loop( 1.0, 1.0, now =13. ), -0.4100 )
assert near( control.loop( 1.0, 1.0, now =14. ), -0.4100 )
def posepub(xtg, ytg, x, y, ango, bx, by, ang, k):
pub = rospy.Publisher('robot1n1/pose', Pose, queue_size=10)
pubball = rospy.Publisher('ballpose', Pose, queue_size=10)
pose = Pose()
bpose = Pose()
k = k
rate = rospy.Rate(60)
pg.init()
ball = p.ball(x=bx, y=by)
mybot = p.robot(x=x, y=y, yaw=ango, ball=ball)
mybotpid = pid.pid(x=x, y=y, ball=ball, angle=ango)
ko = 0
while not rospy.is_shutdown():
for event in pg.event.get():
if event.type == pg.QUIT:
sys.exit()
mybotpid.gtg(xtg, ytg, mybot, ball, thtg=ang)
if mybot.dribble == 0:
p.collRb(mybot, ball)
bpose.position.x = ball.x
bpose.position.y = ball.y
p.walleffect(mybot)
p.walleffect(ball)
pose.position.x = mybot.x
pose.position.y = mybot.y
pose.orientation.z = m.tan(mybot.theta / 2)
pose.orientation.w = 1
bpose.orientation.w = 1
pub.publish(pose)
pubball.publish(bpose)
if (p.dist(mybot.x, mybot.y, xtg, ytg) < 10
and abs(mybot.theta - angle) < 0.1 and ball.speed <= 3
and ko == 0):
if k == 1 and mybot.dribble == 1:
mybot.kick(ball, 5)
ko = 1
#print ball.speed
if (p.dist(mybot.x, mybot.y, xtg, ytg) < 10
and abs(mybot.theta - angle) < 0.1 and ball.speed <= 3
and ko == 1):
return mybot.x, mybot.y, mybot.theta, ball.x, ball.y
oldx = mybot.x
oldy = mybot.y
rate.sleep()
def __init__(
self,
symbol, # eg. '$'
label, # eg. 'USD'
commodities={}, # The definition of the backing commodities
basket={}, # Reference basket, specifying # Units and Proportion of value
multiplier=1, # How many units of currency does 'basket' represent
K=0.5, # Initial credit/wealth ratio
Lk=(0.0, math.nan), # Allowed range of K (math.nan means no limit)
damping=3.0, # Amplify corrective movement by this factor (too much: oscillation)
window=7 * 24 * 60 *
60, # Default to 1 week sliding average to filter currency value
now=time.time()): # Initial time (default to seconds)
""" Establish the fundamentals and initial conditions of the currency. It will always be
valued based on the initial proportional relationship between the commodities. """
self.symbol = symbol
self.label = label
self.commodities = commodities.copy()
self.basket = basket.copy()
self.multiplier = multiplier
self.window = window
# Remember the latest commodity prices and total basket cost; used for computing how much
# credit can be issued for pledges of any commodities.
self.price = {}
self.total = 0.
# Create the PID loop, and pre-load the integral to produce the initial K. If there is 0
# error (P term) and 0 error rate of change (D term), then only the I term influences the
# output. So, if the next update() supplies prices that show that the value of the currency
# is 1.0, then the error term will be 0, P and D will remain 0, and I will not change, K
# will remain at the current value.
# P I D
Kpid = (damping, 0.1, damping / 2.)
self.stabilizer = pid.pid(Kpid=Kpid, Finp=window, Lout=Lk, now=now)
self.stabilizer.I = K / self.stabilizer.Kpid[1]
# time value K
self.trend = [(now, 1.0, K)]
def __init__(self):
rospy.init_node('follow_face', anonymous=True)
# real_drone = bool(rospy.get_param('~real_drone', 'false'))
rospy.Subscriber("/face_tracker/bbox", rlist, self.maintain_coords)
self.timer = rospy.Timer(rospy.Duration(2), self.timer_callback, True)
self.servo_down_pub = rospy.Publisher('servo_down',
UInt16,
queue_size=10)
self.servo_up_pub = rospy.Publisher('servo_up', UInt16, queue_size=10)
# servo angles
self.up = 90
self.down = 110
# set default servo angles
self.servo_down_pub.publish(self.down)
self.servo_up_pub.publish(self.up)
# init pid
self.pid = pid()
def testOnLine(self):
pidObj = pid.pid()
self.assertEqual(pidObj.calculateDifference(2000, 25, 100, 1000), 0)
def __init__(self):
# First get an instance of the API endpoint (the connect via web case will be similar)
self.api = local_connect()
# Our vehicle (we assume the user is trying to control the virst vehicle attached to the GCS)
self.vehicle = self.api.get_vehicles()[0]
# initialised flag
self.home_initialised = False
# timer to intermittently check for home position
self.last_home_check = time.time()
# historical attitude
self.att_hist = AttitudeHistory(self.vehicle, 2.0)
self.attitude_delay = 0.0 # expected delay between image and attitude
# search variables
self.search_state = 0 # 0 = not search, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw
self.search_start_heading = None # initial heading of vehicle when search began
self.search_target_heading = None # the vehicle's current target heading (updated as vehicle spins during search)
self.search_heading_change = None # heading change (in radians) performed so far during search
self.search_balloon_pos = None # position (as an offset from home) of closest balloon (so far) during search
self.search_balloon_heading = None # earth-frame heading (in radians) from vehicle to closest balloon
self.search_balloon_pitch_top = None # earth-frame pitch (in radians) from vehicle to closest balloon
self.search_balloon_distance = None # distance (in meters) from vehicle to closest balloon
self.targeting_start_time = 0 # time vehicle first pointed towards final target (used with delay_time below)
self.targeting_delay_time = balloon_config.config.get_float('general','SEARCH_TARGET_DELAY',2.0) # time vehicle waits after pointing towards ballon and before heading towards it
self.search_yaw_speed = balloon_config.config.get_float('general','SEARCH_YAW_SPEED',5.0)
# vehicle mission
self.mission_cmds = None
self.mission_alt_min = 0 # min altitude from NAV_GUIDED mission command (we ignore balloons below this altitude). "0" means no limit
self.mission_alt_max = 0 # max altitude from NAV_GUIDED mission command (we ignore balloons above this altitude). "0" means no limit
self.mission_distance_max = 0 # max distance from NAV_GUIDED mission command (we ignore balloons further than this distance). "0" means no limit
# we are not in control of vehicle
self.controlling_vehicle = False
# vehicle position captured at time camera image was captured
self.vehicle_pos = None
# balloon direction and position estimate from latest call to analyse_image
self.balloon_found = False
self.balloon_pitch = None
self.balloon_pitch_top = None # earth-frame pitch (in radians) from vehicle to top of closest balloon
self.balloon_heading = None
self.balloon_distance = None
self.balloon_pos = None # last estimated position as an offset from home
# time of the last target update sent to the flight controller
self.guided_last_update = time.time()
# latest velocity target sent to flight controller
self.guided_target_vel = None
# time the target balloon was last spotted
self.last_spotted_time = 0
# if we lose sight of a balloon for this many seconds we will consider it lost and give up on the search
self.lost_sight_timeout = 3
# The module only prints log messages unless the vehicle is in GUIDED mode (for testing).
# Once this seems to work reasonablly well change self.debug to False and then it will
# actually _enter_ guided mode when it thinks it sees a balloon
self.debug = balloon_config.config.get_boolean('general','debug',True)
# use the simulator to generate fake balloon images
self.use_simulator = balloon_config.config.get_boolean('general','simulate',False)
# start background image grabber
if not self.use_simulator:
balloon_video.start_background_capture()
# initialise video writer
self.writer = None
# horizontal velocity pid controller. maximum effect is 10 degree lean
xy_p = balloon_config.config.get_float('general','VEL_XY_P',1.0)
xy_i = balloon_config.config.get_float('general','VEL_XY_I',0.0)
xy_d = balloon_config.config.get_float('general','VEL_XY_D',0.0)
xy_imax = balloon_config.config.get_float('general','VEL_XY_IMAX',10.0)
self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))
# vertical velocity pid controller. maximum effect is 10 degree lean
z_p = balloon_config.config.get_float('general','VEL_Z_P',2.5)
z_i = balloon_config.config.get_float('general','VEL_Z_I',0.0)
z_d = balloon_config.config.get_float('general','VEL_Z_D',0.0)
z_imax = balloon_config.config.get_float('general','VEL_IMAX',10.0)
self.vel_z_pid = pid.pid(z_p, z_i, z_d, math.radians(z_imax))
# velocity controller min and max speed
self.vel_speed_min = balloon_config.config.get_float('general','VEL_SPEED_MIN',1.0)
self.vel_speed_max = balloon_config.config.get_float('general','VEL_SPEED_MAX',4.0)
self.vel_speed_last = 0.0 # last recorded speed
self.vel_accel = balloon_config.config.get_float('general','VEL_ACCEL', 0.5) # maximum acceleration in m/s/s
self.vel_dist_ratio = balloon_config.config.get_float('general','VEL_DIST_RATIO', 0.5)
# pitch angle to hit balloon at. negative means come down from above
self.vel_pitch_target = math.radians(balloon_config.config.get_float('general','VEL_PITCH_TARGET',-5.0))
# velocity controller update rate
self.vel_update_rate = balloon_config.config.get_float('general','VEL_UPDATE_RATE_SEC',0.2)
def test(inp):
ALPHA = 0.02
test.output = (SAMPLE_TIME_S * inp + test.output) / (1.0 +
ALPHA * SAMPLE_TIME_S)
return test.output
test.output = 0.0
if __name__ == "__main__":
p = pid.pid(kp=2.0,
ki=0.5,
kd=-0.25,
tau=0.01,
out_limit_min=-10.0,
out_limit_max=10.0,
t=0.01)
setpoint = 1.0
print("Time (s)\tSystem Output\tControllerOutput")
t = 0.0
while t < SIMULATION_TIME_MAX:
# Get measurement from system
measurement = test(p.output)
# Compute new control signal
p.update(setpoint, measurement)
def __init__(self):
# connect to vehicle with dronekit
self.vehicle = self.get_vehicle_with_dronekit()
# initialised flag
self.home_initialised = False
# timer to intermittently check for home position
self.last_home_check = time.time()
# historical attitude
self.att_hist = AttitudeHistory(self.vehicle, 2.0)
self.attitude_delay = 0.0 # expected delay between image and attitude
# search variables
self.search_state = 0 # 0 = not search, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw
self.search_start_heading = None # initial heading of vehicle when search began
self.search_target_heading = None # the vehicle's current target heading (updated as vehicle spins during search)
self.search_heading_change = None # heading change (in radians) performed so far during search
self.search_balloon_pos = None # position (as an offset from home) of closest balloon (so far) during search
self.search_balloon_heading = None # earth-frame heading (in radians) from vehicle to closest balloon
self.search_balloon_pitch_top = None # earth-frame pitch (in radians) from vehicle to closest balloon
self.search_balloon_distance = None # distance (in meters) from vehicle to closest balloon
self.targeting_start_time = 0 # time vehicle first pointed towards final target (used with delay_time below)
self.targeting_delay_time = balloon_config.config.get_float(
'general', 'SEARCH_TARGET_DELAY', 2.0
) # time vehicle waits after pointing towards ballon and before heading towards it
self.search_yaw_speed = balloon_config.config.get_float(
'general', 'SEARCH_YAW_SPEED', 5.0)
# vehicle mission
self.mission_cmds = None
self.mission_alt_min = 0 # min altitude from NAV_GUIDED mission command (we ignore balloons below this altitude). "0" means no limit
self.mission_alt_max = 0 # max altitude from NAV_GUIDED mission command (we ignore balloons above this altitude). "0" means no limit
self.mission_distance_max = 0 # max distance from NAV_GUIDED mission command (we ignore balloons further than this distance). "0" means no limit
# we are not in control of vehicle
self.controlling_vehicle = False
# vehicle position captured at time camera image was captured
self.vehicle_pos = None
# balloon direction and position estimate from latest call to analyse_image
self.balloon_found = False
self.balloon_pitch = None
self.balloon_pitch_top = None # earth-frame pitch (in radians) from vehicle to top of closest balloon
self.balloon_heading = None
self.balloon_distance = None
self.balloon_pos = None # last estimated position as an offset from home
# time of the last target update sent to the flight controller
self.guided_last_update = time.time()
# latest velocity target sent to flight controller
self.guided_target_vel = None
# time the target balloon was last spotted
self.last_spotted_time = 0
# if we lose sight of a balloon for this many seconds we will consider it lost and give up on the search
self.lost_sight_timeout = 3
# The module only prints log messages unless the vehicle is in GUIDED mode (for testing).
# Once this seems to work reasonablly well change self.debug to False and then it will
# actually _enter_ guided mode when it thinks it sees a balloon
self.debug = balloon_config.config.get_boolean('general', 'debug',
True)
# use the simulator to generate fake balloon images
self.use_simulator = balloon_config.config.get_boolean(
'general', 'simulate', False)
# start background image grabber
if not self.use_simulator:
balloon_video.start_background_capture()
# initialise video writer
self.writer = None
# horizontal velocity pid controller. maximum effect is 10 degree lean
xy_p = balloon_config.config.get_float('general', 'VEL_XY_P', 1.0)
xy_i = balloon_config.config.get_float('general', 'VEL_XY_I', 0.0)
xy_d = balloon_config.config.get_float('general', 'VEL_XY_D', 0.0)
xy_imax = balloon_config.config.get_float('general', 'VEL_XY_IMAX',
10.0)
self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))
# vertical velocity pid controller. maximum effect is 10 degree lean
z_p = balloon_config.config.get_float('general', 'VEL_Z_P', 2.5)
z_i = balloon_config.config.get_float('general', 'VEL_Z_I', 0.0)
z_d = balloon_config.config.get_float('general', 'VEL_Z_D', 0.0)
z_imax = balloon_config.config.get_float('general', 'VEL_IMAX', 10.0)
self.vel_z_pid = pid.pid(z_p, z_i, z_d, math.radians(z_imax))
# velocity controller min and max speed
self.vel_speed_min = balloon_config.config.get_float(
'general', 'VEL_SPEED_MIN', 1.0)
self.vel_speed_max = balloon_config.config.get_float(
'general', 'VEL_SPEED_MAX', 4.0)
self.vel_speed_last = 0.0 # last recorded speed
self.vel_accel = balloon_config.config.get_float(
'general', 'VEL_ACCEL', 0.5) # maximum acceleration in m/s/s
self.vel_dist_ratio = balloon_config.config.get_float(
'general', 'VEL_DIST_RATIO', 0.5)
# pitch angle to hit balloon at. negative means come down from above
self.vel_pitch_target = math.radians(
balloon_config.config.get_float('general', 'VEL_PITCH_TARGET',
-5.0))
# velocity controller update rate
self.vel_update_rate = balloon_config.config.get_float(
'general', 'VEL_UPDATE_RATE_SEC', 0.2)
# stats
self.num_frames_analysed = 0
self.stats_start_time = 0
def setup_pid(control_rate, kp, ki, kd):
pid = p.pid()
pid.pid_set_frequency(control_rate)
# pid_pos_x.set_derivative_term_low_pass_frequency(control_rate/2)
pid.pid_set_gains(kp, ki, kd)
return pid
def testCalculateDifference(self):
pidObj = pid.pid()
print(pidObj.calculateDifference(1000, 25, 100, 1000))
def __init__(self):
self.x_PID = pid(0.070, 0.025, 0.010, 15)
self.y_PID = pid(0.070, 0.025, 0.010, 15)
self.x_corr = 0
self.y_corr = 0
self.target_angle = [0,0]
global sock, pv
try:
msg = 'READ %d' % channel
sock.sendall(msg)
pv = float(sock.recv(64)) * 6.25
except socket.error, (value, message):
sock.close()
print "Error socket receiving. " + message
connected = False
# Socket
ip = '127.0.0.1'
port = 20081
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((ip, port))
print "Connected with Simutank!\n"
except socket.error, (value, message):
sock.close()
print "Error opening socket. " + message
while True:
integral_ = pid.integral(error, sampleTime, integral_, ki)
mv = pid.pid(error, errorPrevious, sampleTime, integral_, kp, ki, kd)
setMV()
getPV()
errorPrevious = error
error = setPoint - pv
time.sleep(sampleTime)
pitch_init = pitch_init+180 if pitch_init <= 0 else pitch_init-180
roll_init = bno055.readEuler().angle_y
# Write to file
with open("/Web/www/autre.json", "r") as f:
autre_file = ujson.load(f)
with open("/Web/www/autre.json", "w") as f:
autre_file[2]["data"]["target"] = pitch_init
autre_file[1]["data"]["target"] = roll_init
ujson.dump(autre_file, f)
# Airspeed
speed = airspeed(bmp180)
speed_init = 10
# PID controllers
pitch_pid = pid(0, 0, 0)
roll_pid = pid(0, 0, 0)
speed_pid = pid(0, 0, 0)
# Read configuration
pid_counter = 0
#config.readConfig(pitch_pid, roll_pid, speed_pid)
# Setup mode object
mode = modes.mode(receiver, speed, bno055, speed_pid, pitch_pid, roll_pid,
pitch_servo, roll_servo, throttle_servo, yaw_servo)
# --------------------------------------------------------------------------
# Main loop
# --------------------------------------------------------------------------
global sock,pv
try:
msg = 'READ %d' % channel
sock.sendall(msg)
pv = float(sock.recv(64))*6.25
except socket.error, (value,message):
sock.close()
print "Error socket receiving. " + message
connected = False
# Socket
ip = '127.0.0.1'
port = 20081
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((ip, port))
print "Connected with Simutank!\n"
except socket.error, (value,message):
sock.close()
print "Error opening socket. " + message
while True:
integral_ = pid.integral(error,sampleTime,integral_,ki)
mv = pid.pid(error,errorPrevious,sampleTime,integral_,kp,ki,kd)
setMV()
getPV()
errorPrevious = error
error = setPoint-pv
time.sleep(sampleTime)
def update_controls(self):
######################################################
# RETRIEVE SIMULATOR FEEDBACK
######################################################
x = self._current_x
y = self._current_y
yaw = self._current_yaw
v = self._current_speed
self.update_desired_motion()
v_desired = self._desired_speed
yaw_desired = self._desired_yaw
cross_error = self._lookahead_cross_error
t = self._current_timestamp
waypoints = self._waypoints
throttle_output = 0
steer_output = 0
brake_output = 0
######################################################
######################################################
# MODULE 7: DECLARE USAGE VARIABLES HERE
######################################################
######################################################
"""
Use 'self.vars.create_var(<variable name>, <default value>)'
to create a persistent variable (not destroyed at each iteration).
This means that the value can be stored for use in the next
iteration of the control loop.
Example: Creation of 'v_previous', default value to be 0
self.vars.create_var('v_previous', 0.0)
Example: Setting 'v_previous' to be 1.0
self.vars.v_previous = 1.0
Example: Accessing the value from 'v_previous' to be used
throttle_output = 0.5 * self.vars.v_previous
"""
# Init global function
if not self._init_param:
# Throttle Self - Tuning and Model Reference vars
self.vars.create_var('v_previous', np.zeros(2))
self.vars.create_var('v_desired_previous', 0.0)
self.vars.create_var('u_previous', 0.0)
self.vars.create_var('throttle_output_previous', np.zeros(2))
# self.vars.create_var('b0', INIT_GUESS_THROTTLE[0, 0])
# self.vars.create_var('b1', INIT_GUESS_THROTTLE[0, 1])
# self.vars.create_var('b2', INIT_GUESS_THROTTLE[0, 2])
# self.vars.create_var('a1', INIT_GUESS_THROTTLE[1, 1])
# self.vars.create_var('a2', INIT_GUESS_THROTTLE[1, 2])
self.vars.create_var('plant', INIT_GUESS_THROTTLE)
self.vars.create_var('P_k_1', np.eye(4, dtype=float))
# # Throttle controller vars
# self.vars.create_var('throttle_pid_previous', 0.0)
# self.vars.create_var('throttle_e_previous', 0.0)
# self.vars.create_var('throttle_e_previous_2', 0.0)
# Brake controller vars
self.vars.create_var('brake_previous', 0.0)
self.vars.create_var('brake_e_previous', np.zeros(2))
self.vars.create_var('last_brake', False)
# Stanley controller vars
self.vars.create_var('e_yaw_previous', np.zeros(2))
self.vars.create_var('cross_error_previous', np.zeros(2))
self.vars.create_var('u_yaw_previous', 0.0)
self.vars.create_var('u_cross_previous', 0.0)
self._init_param = True
# Skip the first frame to store previous values properly
if self._start_control_loop:
"""
Controller iteration code block.
Controller Feedback Variables:
x : Current X position (meters)
y : Current Y position (meters)
yaw : Current yaw pose (radians)
v : Current forward speed (meters per second)
t : Current time (seconds)
v_desired : Current desired speed (meters per second)
(Computed as the speed to track at the
closest waypoint to the vehicle.)
waypoints : Current waypoints to track
(Includes speed to track at each x,y
location.)
Format: [[x0, y0, v0],
[x1, y1, v1],
...
[xn, yn, vn]]
Example:
waypoints[2][1]:
Returns the 3rd waypoint's y position
waypoints[5]:
Returns [x5, y5, v5] (6th waypoint)
Controller Output Variables:
throttle_output : Throttle output (0 to 1)
steer_output : Steer output (-1.22 rad to 1.22 rad)
brake_output : Brake output (0 to 1)
"""
######################################################
######################################################
# MODULE 7: IMPLEMENTATION OF LONGITUDINAL CONTROLLER HERE
######################################################
######################################################
"""
Implement a longitudinal controller here. Remember that you can
access the persistent variables declared above here. For
example, can treat self.vars.v_previous like a "global variable".
"""
# Change these outputs with the longitudinal controller. Note that
# brake_output is optional and is not required to pass the
# assignment, as the car will naturally slow down over time.
# Throttle controller based on Model Predictive Control
# model = longitudinal_model.longitudinal_model()
# throttle_controller = mpc.mpc(model, T_SAMP, v_desired)
# # estimator = mhe.mhe(model, T_SAMP)
# # estimator.y0 = v
# # x0 = estimator.make_step(y0)
# x0 = np.dot(np.linalg.pinv(model.C), v)
# x0 = np.append(x0, [[v]], axis=0)
# throttle_controller.x0 = x0
# throttle_controller.set_initial_guess()
# u0 = throttle_controller.make_step(x0)
# throttle_output = throttle_controller.u0['u']
# # Throttle controller is Complementary of PID and Feed Forward Controller
# # Feed Forward Controller
# # Relationship between Car's speed and Throttle var is linear regressed as below equation
# # speed = FF_A * throttle_var + FF_B
# FF_A = 20.3713
# FF_B = -4.9084
# throttle_FF_output = (v_desired - FF_B) / FF_A
# # PID Controller
# throttle_P = 1.5
# throttle_I = 8
# throttle_D = 2
# throttle_PID_controller = pid.pid(T_SAMP, throttle_P, throttle_I, throttle_D)
# throttle_k_1 = self.vars.throttle_pid_previous
# throttle_e_k_1 = self.vars.throttle_e_previous
# throttle_e_k_2 = self.vars.throttle_e_previous_2
# throttle_e_k = v_desired - v
# throttle_PID_output = throttle_PID_controller.make_control(throttle_k_1, throttle_e_k, throttle_e_k_1, throttle_e_k_2)
# self.vars.throttle_pid_previous = throttle_PID_output
# self.vars.throttle_e_previous_2 = self.vars.throttle_e_previous
# self.vars.throttle_e_previous = throttle_e_k
# complement_ratio = 0.5 # Ratio of FF Controller in the Complementary Controller
# throttle_output = complement_ratio * throttle_FF_output + (1 - complement_ratio) * throttle_PID_output
# Throttel Controller based on Self - Tuning and Model Reference
# Estimate Model
# Model estimation excutes if brake was unactive, last command
if not self.vars.last_brake:
phi = np.array([[-self.vars.v_previous[0]],
[-self.vars.v_previous[1]],
[self.vars.throttle_output_previous[0]],
[self.vars.throttle_output_previous[1]]])
theta = self.vars.plant.flatten()
theta = theta[[2, 3, 0, 1]].reshape(4, 1)
estimated_model = model_estimator.model(phi, theta)
estimated_model.make_control(v, self.vars.P_k_1)
theta = estimated_model.get_theta()
self.vars.plant = theta[[2, 3, 0, 1]].reshape((2, 2))
self.vars.P_k_1 = estimated_model.get_P_k_1()
# Model Reference Control
throttle_controller = mrac.mrac(THROTTLE_REF_MODEL,
self.vars.plant)
uc = np.array([v_desired, self.vars.v_desired_previous])
y = np.array([v, self.vars.v_previous[0]])
throttle_output = throttle_controller.make_control(
uc, y, self.vars.u_previous)
self.vars.v_desired_previous = v_desired
self.vars.v_previous[1] = self.vars.v_previous[0]
self.vars.v_previous[0] = v
self.vars.u_previous = throttle_output
self.vars.throttle_output_previous[
1] = self.vars.throttle_output_previous[0]
self.vars.throttle_output_previous[0] = np.fmax(
np.fmin(throttle_output, 1.0), 0.0)
# Brake controller is a PID Controller
if throttle_output < 0:
brake_P = 3.7618
brake_I = 0.9738
brake_D = 0.3308
brake_controller = pid.pid(T_SAMP, brake_P, brake_I, brake_D)
brake_e = np.insert(self.vars.brake_e_previous, 0,
v - v_desired)
brake_output = brake_controller.make_control(
self.vars.brake_previous, brake_e)
self.vars.brake_previous = brake_output
self.vars.brake_e_previous[1] = self.vars.brake_e_previous[0]
self.vars.brake_e_previous[0] = v - v_desired
if brake_output > 0:
self.vars.last_brake = True
else:
self.vars.last_brake = False
else:
self.vars.last_brake = False
brake_output = 0
######################################################
######################################################
# MODULE 7: IMPLEMENTATION OF LATERAL CONTROLLER HERE
######################################################
######################################################
"""
Implement a lateral controller here. Remember that you can
access the persistent variables declared above here. For
example, can treat self.vars.v_previous like a "global variable".
"""
# Change the steer output with the lateral controller.
# Lateral Controller is based on Stanley Controller
steer_yaw_P = 1
steer_yaw_I = 0.15
steer_yaw_D = 0.9
steer_cross_P = 1.281
steer_cross_I = 0.25
steer_cross_D = 1.5
steer_K_s = 10
steer_controller = stanley_controller.stanley_controller(steer_yaw_P, steer_yaw_I, steer_yaw_D,\
steer_cross_P, steer_cross_I, steer_cross_D,\
steer_K_s)
# e_yaw = np.array([yaw_desired - yaw, self.vars.e_yaw_previous])
e_yaw = np.insert(self.vars.e_yaw_previous, 0, yaw_desired - yaw)
error = np.insert(self.vars.cross_error_previous, 0, cross_error)
# error = np.array([cross_error, self.vars.cross_error_previous])
steer_output = steer_controller.make_control(self.vars.u_yaw_previous,\
self.vars.u_cross_previous,\
e_yaw, v, error)
self.vars.u_yaw_previous, self.vars.u_cross_previous = steer_controller.get_u_previous(
)
self.vars.e_yaw_previous[1] = self.vars.e_yaw_previous[0]
self.vars.e_yaw_previous[0] = yaw_desired - yaw
self.vars.cross_error_previous[1] = self.vars.cross_error_previous[
0]
self.vars.cross_error_previous[0] = cross_error
######################################################
# SET CONTROLS OUTPUT
######################################################
self.set_throttle(throttle_output) # in percent (0 to 1)
self.set_steer(steer_output) # in rad (-1.22 to 1.22)
self.set_brake(brake_output) # in percent (0 to 1)
######################################################
######################################################
# MODULE 7: STORE OLD VALUES HERE (ADD MORE IF NECESSARY)
######################################################
######################################################
"""
def execute(self, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
# Return vx,vy,vw,self.replan,remainingDistance
self.target = target
self.REPLAN = 0
self.homePos = homePos_
self.awayPos = awayPos_
self.kubid = kub_id
#self.FIRST_CALL = int(os.environ.get('fc'+str(self.kubid)))
print(self.FIRST_CALL)
# if not self.prev_target==None:
if isinstance(self.prev_target, Vector2D):
dist = distance_(self.target, self.prev_target)
if(dist>DESTINATION_THRESH):
self.REPLAN = 1
self.prev_target = self.target
# print("in getVelocity, self.FIRST_CALL = ",self.FIRST_CALL)
curPos = Vector2D(int(self.homePos[self.kubid].x),int(self.homePos[self.kubid].y))
distance = sqrt(pow(self.target.x - self.homePos[self.kubid].x,2) + pow(self.target.y - self.homePos[self.kubid].y,2))
if(self.FIRST_CALL):
print("BOT id:{}, in first call, timeIntoLap: {}".format(self.kubid, t-self.start_time))
startPt = point_2d()
startPt.x = self.homePos[self.kubid].x
startPt.y = self.homePos[self.kubid].y
self.findPath(startPt, self.target, avoid_ball)
#os.environ['fc'+str(self.kubid)]='0'
self.FIRST_CALL = 0
else:
print("Bot id:{}, not first call, timeIntoLap: {}".format(self.kubid,t-self.start_time))
if distance < 1.5*BOT_BALL_THRESH:
return [0,0,0,0,0]
# print("ex = ",self.expectedTraverseTime)
# print("t = ",t," start = ",start)
remainingDistance = 0
# print("ex = ",self.expectedTraverseTime)
# print("t = ",t," start = ",start)
if (t-self.start_time< self.expectedTraverseTime):
if self.v.trapezoid(t-self.start_time,curPos):
index = self.v.GetExpectedPositionIndex()
if index == -1:
vX,vY,eX,eY = self.v.sendVelocity(self.v.getVelocity(),self.v.motionAngle[index],index)
vX,vY = 0,0
else:
remainingDistance = self.v.GetPathLength(startIndex=index)
vX,vY,eX,eY = self.v.sendVelocity(self.v.getVelocity(),self.v.motionAngle[index],index)
else:
# print(t-self.start_time, self.expectedTraverseTime)
if self.expectedTraverseTime == 'self.REPLAN':
self.REPLAN = 1
# print("Motion Not Possible")
vX,vY,eX,eY = 0,0,0,0
flag = 1
else:
# print("TimeOUT, self.REPLANNING")
vX,vY,eX,eY = 0,0,0,0
self.errorInfo.errorIX = 0.0
self.errorInfo.errorIY = 0.0
self.errorInfo.lastErrorX = 0.0
self.errorInfo.lastErrorY = 0.0
startPt = point_2d()
startPt.x = self.homePos[self.kubid].x
startPt.y = self.homePos[self.kubid].y
self.findPath(startPt,self.target, avoid_ball)
errorMag = sqrt(pow(eX,2) + pow(eY,2))
should_replan = self.shouldReplan()
if(should_replan == True):
self.v.velocity = 0
# print("self.v.velocity now = ",self.v.velocity)
# print("entering if, should_replan = ", should_replan)
if should_replan or \
(errorMag > 350 and distance > 1.5* BOT_BALL_THRESH) or \
self.REPLAN == 1:
# print("______________here____________________")
# print("condition 1",should_replan)
# print("error magnitude", errorMag)
# print("distance threshold",distance > 1.5* BOT_BALL_THRESH)
# print("condition 2",errorMag > 350 and distance > 1.5* BOT_BALL_THRESH)
# print("condition 3",self.REPLAN)
# print("Should self.Replan",should_replan)
# print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH)
self.REPLAN = 1
startPt = point_2d()
startPt.x = self.homePos[self.kubid].x
startPt.y = self.homePos[self.kubid].y
self.findPath(startPt,self.target, avoid_ball)
return [0,0,0, self.REPLAN,0]
else:
self.errorInfo.errorX = eX
self.errorInfo.errorY = eY
vX,vY = pid(vX,vY,self.errorInfo,self.pso)
botAngle = self.homePos[self.kubid].theta
vXBot = vX*cos(botAngle) + vY*sin(botAngle)
vYBot = -vX*sin(botAngle) + vY*cos(botAngle)
return [vXBot, vYBot, 0, self.REPLAN,remainingDistance]
return [vXBot, vYBot, 0, self.REPLAN]
phi = -math.pi # face backwards initially
# this is the base velocity of the robot
base_velocity = 1.0
# this is the actual velocity, which may be a percentage of base_velocity
v = base_velocity
# shorthand for goal
g = goal_position
# time step for simulation
dt = 0.01
# create some pid controllers, one for the robot, and one for the camera
dir_pid = pid.pid(2.0, 0, 0)
if SHOW_PATH:
pts = points(pos=[], size=.5, color=color.red, retain=50)
pts.append(x)
seen_obstacles = set()
environment = {
'BASE_VELOCITY' : base_velocity,
'ROBOT_RADIUS' : ROBOT_RADIUS,
'CLEAR_PATH' : clear_path,
'PHI' : 0,
'TARGET' : target,
}
def Get_Vel(start, t, kub_id, target, homePos_, awayPos_, avoid_ball=False):
global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid
REPLAN = 0
homePos = homePos_
awayPos = awayPos_
kubid = kub_id
curPos = Vector2D(int(homePos[kubid].x), int(homePos[kubid].y))
distance = sqrt(
pow(target.x - homePos[kubid].x, 2) +
pow(target.y - homePos[kubid].y, 2))
if (FIRST_CALL):
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
FIRST_CALL = 0
if distance < 1.5 * BOT_BALL_THRESH:
return [0, 0, 0, 0]
# print("ex = ",expectedTraverseTime)
# print("t = ",t," start = ",start)
# print("t - start = ",t-start)
if (t - start < expectedTraverseTime):
if v.trapezoid(t - start, curPos):
index = v.GetExpectedPositionIndex()
if index == -1:
vX, vY, eX, eY = v.sendVelocity(v.getVelocity(),
v.motionAngle[index], index)
vX, vY = 0, 0
else:
vX, vY, eX, eY = v.sendVelocity(v.getVelocity(),
v.motionAngle[index], index)
else:
# print(t-start, expectedTraverseTime)
if expectedTraverseTime == 'REPLAN':
REPLAN = 1
# print("Motion Not Possible")
vX, vY, eX, eY = 0, 0, 0, 0
flag = 1
else:
# print("TimeOUT, REPLANNING")
vX, vY, eX, eY = 0, 0, 0, 0
errorInfo.errorIX = 0.0
errorInfo.errorIY = 0.0
errorInfo.lastErrorX = 0.0
errorInfo.lastErrorY = 0.0
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
errorMag = sqrt(pow(eX, 2) + pow(eY, 2))
if shouldReplan() or \
(errorMag > 350 and distance > 2* BOT_BALL_THRESH) or \
REPLAN == 1:
# print("Should Replan",shouldReplan())
# print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH)
REPLAN = 1
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
return [0, 0, 0, REPLAN]
else:
errorInfo.errorX = eX
errorInfo.errorY = eY
vX, vY = pid(vX, vY, errorInfo, pso)
botAngle = homePos[kubid].theta
vXBot = vX * cos(botAngle) + vY * sin(botAngle)
vYBot = -vX * sin(botAngle) + vY * cos(botAngle)
return [vXBot, vYBot, 0, REPLAN]
from dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal, Command
from pymavlink import mavutil
#Common Library Imports
from flight_assist import send_velocity
from position_vector import PositionVector
import pid
import sim
#Python Imports
import math
import time
import argparse
#Global Variables
x_pid = pid.pid(0.1, 0.005, 0.1, 50)
y_pid = pid.pid(0.1, 0.005, 0.1, 50)
hfov = 60
hres = 640
vfov = 60
vres = 480
x_pre = 0
y_pre = 0
def pixels_per_meter(fov, res, alt):
return ((alt * math.tan(math.radians(fov / 2))) / (res / 2))
def land(vehicle, target, attitude, location):
if (vehicle.location.global_relative_frame.alt <= 2.7):
def move_to_waypoint(self, waypoint):
"""Get the drone to actually move to a waypoint.
Takes the waypoint and the current pose of controls pid
It calls pid until the current pose is equal to the waypoint
with a cetatin error tolerence.
Args:
waypoint (numpy.array): contains the waypoint to travel to
"""
# checking if using aruco mapping or odometry for localisation
if self.aruco_mapping:
current_pose = self.camera_pose.as_waypoints()
else:
current_pose = self.moniter_transform()
# dict for PID
state = dict()
# initialisation of various variables for PID
state['lastError'] = np.array([0., 0., 0., 0.])
state['integral'] = np.array([0., 0., 0., 0.])
state['derivative'] = np.array([0., 0., 0., 0.])
# set pid gains
xy_pid = [2, 0.0, 0.]
# xy_pid = [0.2, 0.00, 0.1]
state['p'] = np.array([xy_pid[0], xy_pid[0], 0.3, 1.0], dtype=float)
state['i'] = np.array([xy_pid[1], xy_pid[1], 0.0025, 0.0], dtype=float)
state['d'] = np.array([xy_pid[2], xy_pid[2], 0.15, 0.0], dtype=float)
# to avoid huge jump for the first iteration
state['last_time'] = time.time()
last_twist = np.zeros(4)
marker_not_detected_count = 0
# not controlling yaw since drone was broken
# stay in the loop until staisfactory error range attained
while not np.allclose(current_pose[0:-1], waypoint[0:-1], atol=0.1):
# check if using aruco_mapping or odometry
if self.aruco_mapping:
current_pose = self.camera_pose.as_waypoints()
pid_twist, state = pid(current_pose, waypoint, state)
# if no aruco is being detected
if (current_pose == np.array([0., 0., 0., 0.])).all():
marker_not_detected_count += 1
# if the feed is stuck
if (last_twist == np.array([
pid_twist.linear.x, pid_twist.linear.y,
pid_twist.linear.z, pid_twist.angular.z
])).all():
marker_not_detected_count += 1
# if we are sure it is stuck or no aruco found
if marker_not_detected_count > 2:
self.pub.publish(self.empty_twist)
marker_not_detected_count = 0
print('feed stuck!!!')
else:
self.pub.publish(pid_twist)
# store last twist values to check if feed is stuck or not
last_twist[0] = pid_twist.linear.x
last_twist[1] = pid_twist.linear.y
last_twist[2] = pid_twist.linear.z
last_twist[3] = pid_twist.angular.z
else:
current_pose = self.moniter_transform()
pid_twist, state = pid(current_pose, waypoint, state)
self.pub.publish(pid_twist)
print(current_pose)
# publish the feedback
self._feedback.difference = waypoint - current_pose
self._as.publish_feedback(self._feedback)
def __init__(self,
name,
pin0=18,
pin1=23,
pin2=24,
pin3=25,
simulation=True):
#GPIO: 18 23 24 25
#pin : 12 16 18 22
self.logger = logging.getLogger('myQ.quadcptr')
self.name = name
self.simulation = simulation
self.version = 1
self.motor = [motor('M' + str(i), 0) for i in xrange(4)]
self.motor[0] = motor('M0',
pin0,
kv=1000,
WMin=0,
WMax=100,
simulation=self.simulation)
self.motor[1] = motor('M1',
pin1,
kv=1000,
WMin=0,
WMax=100,
simulation=self.simulation)
self.motor[2] = motor('M2',
pin2,
kv=1000,
WMin=0,
WMax=100,
simulation=self.simulation)
self.motor[3] = motor('M3',
pin3,
kv=1000,
WMin=0,
WMax=100,
simulation=self.simulation)
self.sensor = sensor(simulation=self.simulation)
self.pidR = pid()
self.pidP = pid()
self.pidY = pid()
self.pidR_rate = pid()
self.pidP_rate = pid()
self.pidY_rate = pid()
self.ip = '192.168.1.1'
self.netscan = netscan(self.ip)
self.webserver = webserver(self)
self.display = display(self)
self.rc = rc(self.display.screen)
self.imulog = False
self.savelog = False
self.calibIMU = False
self.debuglev = 0
self.netscanning = False
#for quadricopter phisics calculations- not used yet
self.prop = prop(9, 4.7, 1)
self.voltage = 12 # [V]
self.mass = 2 # [Kg]
self.barLenght = 0.23 # [mm]
self.barMass = 0.15 # [kg]
self.datalog = ''
self.initLog()
def Get_Vel(start, t, kub_id, target, homePos_, awayPos_, avoid_ball=False):
# Return vx,vy,vw,replan,remainingDistance
global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid, prev_target
REPLAN = 0
homePos = homePos_
awayPos = awayPos_
kubid = kub_id
# if not prev_target==None:
if isinstance(prev_target, Vector2D):
dist = distance_(target, prev_target)
if (dist > DESTINATION_THRESH):
REPLAN = 1
prev_target = target
# print("in getVelocity, FIRST_CALL = ",FIRST_CALL)
curPos = Vector2D(int(homePos[kubid].x), int(homePos[kubid].y))
distance = sqrt(
pow(target.x - homePos[kubid].x, 2) +
pow(target.y - homePos[kubid].y, 2))
if (FIRST_CALL):
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
FIRST_CALL = 0
if distance < 1.5 * BOT_BALL_THRESH:
return [0, 0, 0, 0, 0]
# print("ex = ",expectedTraverseTime)
# print("t = ",t," start = ",start)
remainingDistance = 0
# print("ex = ",expectedTraverseTime)
# print("t = ",t," start = ",start)
if (t - start < expectedTraverseTime):
if v.trapezoid(t - start, curPos):
index = v.GetExpectedPositionIndex()
if index == -1:
vX, vY, eX, eY = v.sendVelocity(v.getVelocity(),
v.motionAngle[index], index)
vX, vY = 0, 0
else:
remainingDistance = v.GetPathLength(startIndex=index)
vX, vY, eX, eY = v.sendVelocity(v.getVelocity(),
v.motionAngle[index], index)
else:
# print(t-start, expectedTraverseTime)
if expectedTraverseTime == 'REPLAN':
REPLAN = 1
# print("Motion Not Possible")
vX, vY, eX, eY = 0, 0, 0, 0
flag = 1
else:
# print("TimeOUT, REPLANNING")
vX, vY, eX, eY = 0, 0, 0, 0
errorInfo.errorIX = 0.0
errorInfo.errorIY = 0.0
errorInfo.lastErrorX = 0.0
errorInfo.lastErrorY = 0.0
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
errorMag = sqrt(pow(eX, 2) + pow(eY, 2))
should_replan = shouldReplan()
if (should_replan == True):
v.velocity = 0
# print("v.velocity now = ",v.velocity)
# print("entering if, should_replan = ", should_replan)
if should_replan or \
(errorMag > 350 and distance > 1.5* BOT_BALL_THRESH) or \
REPLAN == 1:
# print("______________here____________________")
# print("condition 1",should_replan)
# print("error magnitude", errorMag)
# print("distance threshold",distance > 1.5* BOT_BALL_THRESH)
# print("condition 2",errorMag > 350 and distance > 1.5* BOT_BALL_THRESH)
# print("condition 3",REPLAN)
# print("Should Replan",should_replan)
# print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH)
REPLAN = 1
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
return [0, 0, 0, REPLAN, 0]
else:
errorInfo.errorX = eX
errorInfo.errorY = eY
vX, vY = pid(vX, vY, errorInfo, pso)
botAngle = homePos[kubid].theta
vXBot = vX * cos(botAngle) + vY * sin(botAngle)
vYBot = -vX * sin(botAngle) + vY * cos(botAngle)
return [vXBot, vYBot, 0, REPLAN, remainingDistance]
return [vXBot, vYBot, 0, REPLAN]
last_twist = np.zeros(4)
marker_not_detected_count = 0
# continue PID indefinatly until stopped of node exits
while True:
# check if using aruco_mapping.
if aruco_mapping:
# get the marker to be the new set array
set_array = marker_pose.as_waypoints()
# offest x axis by 1.5 to maintain 1.5m distance
# from drone.
set_array[0] += 1.5
# get current pose
current_pose = global_pose.as_waypoints()
# run one iteration of PID
pid_twist, state = pid(current_pose, set_array, state)
# if no aruco is being detected
if (current_pose == np.array([0., 0., 0., 0.])).all():
marker_not_detected_count += 1
# if the feed is stuck
if (last_twist == np.array([
pid_twist.linear.x, pid_twist.linear.y,
pid_twist.linear.z, pid_twist.angular.z
])).all():
marker_not_detected_count += 1
# if we are sure it is stuck or no aruco found
if marker_not_detected_count > 2:
pub.publish(twist)
class omu3(object):
"""docstring fs 3omu."""
pidX = pid()
pidY = pid()
pidTh = pid()
adjustmentX = 0
adjustmentY = 0
adjustmentTh = 0
enlargement_vx = 0
enlargement_vy = 0
v1 = 0
v2 = 0
v3 = 0
r = 0.20
sqrt3 = np.sqrt(3)
xsta = 0
ysta = 0
alfasta = 0
def __init__(self):
#//+++++PID GAIN SET+++++//
self.pidX.set_gain(1, 0, 0.3)
self.pidY.set_gain(1, 0, 0.3)
self.pidTh.set_gain(0.3, 0, 0.1)
#//++++++++++++++++++++++//
pass
#sups 3omu, self).__init__()
def inverse_kinematics_model(self, vx, vy,
theta): #theta := ref, alfa := state
#4-omuni no inverse_kinematics_model
self.enlargement_vx = vx * np.cos(self.alfasta) + vy * np.sin(
self.alfasta)
self.enlargement_vy = -1 * vx * np.sin(self.alfasta) + vy * np.cos(
self.alfasta)
self.v1 = 1 / 2 * self.enlargement_vx - self.sqrt3 / 2 * self.enlargement_vy + self.r * theta
self.v2 = -1 * self.enlargement_vx - 0 * self.enlargement_vy + self.r * theta
self.v3 = 1 / 2 * self.enlargement_vx + self.sqrt3 / 2 * self.enlargement_vy + self.r * theta
def get_v1(self):
return self.v1
def get_v2(self):
return self.v2
def get_v3(self):
return self.v3
def set_state(self, xsta, ysta, alfasta):
self.xsta = xsta
self.ysta = ysta
self.alfasta = alfasta
def using_model(self, xref, yref, thref, index):
self.pidX.set_present(self.xsta)
self.pidY.set_present(self.ysta)
self.pidTh.set_present(self.alfasta)
self.adjustmentX = self.pidX.run_pid(xref)
self.adjustmentY = self.pidY.run_pid(yref)
self.adjustmentTh = self.pidTh.run_pid(thref)
self.inverse_kinematics_model(
self.adjustmentX, self.adjustmentY,
self.adjustmentTh) #theta to alfa no tigai ga wakaranai.
import time, config, pid, irc, db, log
pid = pid.pid()
irc = irc.irc()
db = db.db()
log = log.log()
db.get_raffle_status()
if pid.oktorun:
try:
while True:
time.sleep(1)
# Output
out = db.get_next_output(time.time())
if out:
sendStr = 'PRIVMSG '+ out[1] +' :'+ out[2] +'\n'
print(sendStr)
irc.sendmsg(sendStr)
log.logmsg(sendStr, False)
db.delete_output(out[0])
try:
msg = irc.getmsg()
print(msg)
# Reconnect if disconnected
if len(msg) == 0:
irc.connect()
# Prevent Timeout
def __init__(self):
#load config file
sc_config.config.get_file('Smart_Camera')
#get camera specs
self.camera_index = sc_config.config.get_integer('camera','camera_index',0)
self.camera_width = sc_config.config.get_integer('camera', 'camera_width', 640)
self.camera_height = sc_config.config.get_integer('camera', 'camera_height', 480)
self.camera_hfov = sc_config.config.get_float('camera', 'horizontal-fov', 72.42)
self.camera_vfov = sc_config.config.get_float('camera', 'vertical-fov', 43.3)
#use simulator
#self.simulator = sc_config.config.get_boolean('simulator','use_simulator',True)
self.simulator = sc_config.config.get_boolean('simulator','use_simulator',True)
#how many times to attempt a land before giving up
self.search_attempts = sc_config.config.get_integer('general','search_attempts', 5)
#The timeout between losing the target and starting a climb/scan
self.settle_time = sc_config.config.get_integer('general','settle_time', 1.5)
#how high to climb in meters to complete a scan routine
self.climb_altitude = sc_config.config.get_integer('general','climb_altitude', 20)
#the max horizontal speed sent to autopilot
self.vel_speed_max = sc_config.config.get_float('general', 'vel_speed_max', 5)
#P term of the horizontal distance to velocity controller
#Pid P canshu 0.15
self.dist_to_vel = sc_config.config.get_float('general', 'dist_to_vel', 0.15)
#Descent velocity
self.descent_rate = sc_config.config.get_float('general','descent_rate', 0.5)
#roll/pitch value that is considered stable
self.stable_attitude = sc_config.config.get_float('general', 'stable_attitude', 0.18)
#Climb rate when executing a search
self.climb_rate = sc_config.config.get_float('general','climb_rate', -0.5)
#The height at a climb is started if no target is detected
self.abort_height = sc_config.config.get_integer('general', 'abort_height', 10)
#when we have lock on target, only descend if within this radius
self.descent_radius = sc_config.config.get_float('general', 'descent_radius', 2.0)
#The height at which we lock the position on xy axis
self.landing_area_min_alt = sc_config.config.get_integer('general', 'landing_area_min_alt', 0.5)
#The radius of the cylinder surrounding the landing pad
self.landing_area_radius = sc_config.config.get_integer('general', 'landing_area_radius', 20)
#Whether the landing program can be reset after it is disabled
self.allow_reset = sc_config.config.get_boolean('general', 'allow_reset', True)
#Run the program no matter what mode or location; Useful for debug purposes
self.always_run = sc_config.config.get_boolean('general', 'always_run', True)
#whether the companion computer has control of the autopilot or not
self.in_control = False
#how many frames have been captured
self.frame_count = 0
self.last_time = 0
self.cur_time = 0
# horizontal velocity pid controller. maximum effect is 10 degree lean
xy_p = sc_config.config.get_float('general','VEL_XY_P',10.0)
xy_i = sc_config.config.get_float('general','VEL_XY_I',0.0)
xy_d = sc_config.config.get_float('general','VEL_XY_D',0.0)
xy_imax = sc_config.config.get_float('general','VEL_XY_IMAX',10.0)
self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))
#Reset state machine
self.initialize_landing()
#debugging:
self.kill_camera = False
def __init__(self):
# connect to vehicle with dronekit
self.vehicle = self.get_vehicle_with_dronekit()
# mode 0 = balloon finder ; mode 1 = Precision Land & Landing on moving platform ; mode 2 = detection & following mode
# mode 3 = Precision Land without timout ; mode 4 = Fire Detetction
self.mission_on_guide_mode = balloon_config.config.get_integer('general','mode_on_guide',4)
self.camera_width = balloon_config.config.get_integer('camera','width',640)
self.camera_height = balloon_config.config.get_integer('camera','height',480)
self.camera_hfov = balloon_config.config.get_float('camera', 'horizontal-fov', 72.42)
self.camera_vfov = balloon_config.config.get_float('camera', 'vertical-fov', 43.3)
# initialised flag
self.home_initialised = False
# timer to intermittently check for home position
self.last_home_check = time.time()
# historical attitude
self.att_hist = AttitudeHistory(self.vehicle, 2.0)
self.attitude_delay = 0.0 # expected delay between image and attitude
# search variables
self.search_state = 0 # 0 = not search, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw
self.search_start_heading = None # initial heading of vehicle when search began
self.search_target_heading = None # the vehicle's current target heading (updated as vehicle spins during search)
self.search_heading_change = None # heading change (in radians) performed so far during search
self.search_balloon_pos = None # position (as an offset from home) of closest balloon (so far) during search
self.search_balloon_heading = None # earth-frame heading (in radians) from vehicle to closest balloon
self.search_balloon_pitch_top = None # earth-frame pitch (in radians) from vehicle to closest balloon
self.search_balloon_distance = None # distance (in meters) from vehicle to closest balloon
self.targeting_start_time = 0 # time vehicle first pointed towards final target (used with delay_time below)
self.targeting_delay_time = balloon_config.config.get_float('general','SEARCH_TARGET_DELAY',2.0) # time vehicle waits after pointing towards ballon and before heading towards it
self.search_yaw_speed = balloon_config.config.get_float('general','SEARCH_YAW_SPEED',5.0)
# vehicle mission
self.mission_cmds = None
self.mission_alt_min = 0 # min altitude from NAV_GUIDED mission command (we ignore balloons below this altitude). "0" means no limit
self.mission_alt_max = 0 # max altitude from NAV_GUIDED mission command (we ignore balloons above this altitude). "0" means no limit
self.mission_distance_max = 0 # max distance from NAV_GUIDED mission command (we ignore balloons further than this distance). "0" means no limit
# we are not in control of vehicle
self.controlling_vehicle = False
# vehicle position captured at time camera image was captured
self.vehicle_pos = None
# balloon direction and position estimate from latest call to analyse_image
self.balloon_found = False
self.fire_found = False
self.balloon_pitch = None
self.balloon_pitch_top = None # earth-frame pitch (in radians) from vehicle to top of closest balloon
self.balloon_heading = None
self.balloon_distance = None
self.balloon_pos = None # last estimated position as an offset from home
# time of the last target update sent to the flight controller
self.guided_last_update = time.time()
# latest velocity target sent to flight controller
self.guided_target_vel = None
# time the target balloon was last spotted
self.last_spotted_time = 0
# if we lose sight of a balloon for this many seconds we will consider it lost and give up on the search
self.lost_sight_timeout = 15
# The module only prints log messages unless the vehicle is in GUIDED mode (for testing).
# Once this seems to work reasonablly well change self.debug to False and then it will
# actually _enter_ guided mode when it thinks it sees a balloon
self.debug = balloon_config.config.get_boolean('general','debug',True)
# use the simulator to generate fake balloon images
self.use_simulator = balloon_config.config.get_boolean('general','simulate',False)
# start background image grabber
if not self.use_simulator:
balloon_video.start_background_capture()
# initialise video writer
self.writer = None
# horizontal velocity pid controller. maximum effect is 10 degree lean
xy_p = balloon_config.config.get_float('general','VEL_XY_P',1.0)
xy_i = balloon_config.config.get_float('general','VEL_XY_I',0.0)
xy_d = balloon_config.config.get_float('general','VEL_XY_D',0.0)
xy_imax = balloon_config.config.get_float('general','VEL_XY_IMAX',10.0)
self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))
# vertical velocity pid controller. maximum effect is 10 degree lean
z_p = balloon_config.config.get_float('general','VEL_Z_P',2.5)
z_i = balloon_config.config.get_float('general','VEL_Z_I',0.0)
z_d = balloon_config.config.get_float('general','VEL_Z_D',0.0)
z_imax = balloon_config.config.get_float('general','VEL_IMAX',10.0)
self.vel_z_pid = pid.pid(z_p, z_i, z_d, math.radians(z_imax))
# velocity controller min and max speed
self.vel_speed_min = balloon_config.config.get_float('general','VEL_SPEED_MIN',1.0)
self.vel_speed_max = balloon_config.config.get_float('general','VEL_SPEED_MAX',4.0)
self.vel_speed_last = 0.0 # last recorded speed
self.vel_accel = balloon_config.config.get_float('general','VEL_ACCEL', 0.5) # maximum acceleration in m/s/s
self.vel_dist_ratio = balloon_config.config.get_float('general','VEL_DIST_RATIO', 0.5)
# pitch angle to hit balloon at. negative means come down from above
self.vel_pitch_target = math.radians(balloon_config.config.get_float('general','VEL_PITCH_TARGET',-5.0))
# velocity controller update rate
self.vel_update_rate = balloon_config.config.get_float('general','VEL_UPDATE_RATE_SEC',0.2)
# stats
self.num_frames_analysed = 0
self.stats_start_time = 0
#self.attitude = (0,0,0)
#FROM PRECISION LAND!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
#how many times to attempt a land before giving up
self.search_attempts = balloon_config.config.get_integer('general','search_attempts', 5)
#The timeout between losing the target and starting a climb/scan
self.settle_time = balloon_config.config.get_integer('general','settle_time', 1.5)
#how high to climb in meters to complete a scan routine
self.climb_altitude = balloon_config.config.get_integer('general','climb_altitude', 20)
#the max horizontal speed sent to autopilot
#self.vel_speed_max = balloon_config.config.get_float('general', 'vel_speed_max', 5)
#P term of the horizontal distance to velocity controller
self.dist_to_vel = balloon_config.config.get_float('general', 'dist_to_vel', 0.15)
#Descent velocity
self.descent_rate = balloon_config.config.get_float('general','descent_rate', 0.5)
#roll/pitch valupixee that is considered stable
self.stable_attitude = balloon_config.config.get_float('general', 'stable_attitude', 0.18)
#Climb rate when executing a search
self.climb_rate = balloon_config.config.get_float('general','climb_rate', -2.0)
#The height at a climb is started if no target is detected
self.abort_height = balloon_config.config.get_integer('general', 'abort_height', 10)
#when we have lock on target, only descend if within this radius
self.descent_radius = balloon_config.config.get_float('general', 'descent_radius', 1.0)
#The height at which we lock the position on xy axis, default = 1
self.landing_area_min_alt = balloon_config.config.get_integer('general', 'landing_area_min_alt', 0)
#The radius of the cylinder surrounding the landing pad
self.landing_area_radius = balloon_config.config.get_integer('general', 'landing_area_radius', 20)
#Whether the landing program can be reset after it is disabled
self.allow_reset = balloon_config.config.get_boolean('general', 'allow_reset', True)
#Run the program no matter what mode or location; Useful for debug purposes
self.always_run = balloon_config.config.get_boolean('general', 'always_run', True)
#whether the companion computer has control of the autopilot or not
self.in_control = False
#The timeout between losing the target and starting a climb/scan
self.settle_time = balloon_config.config.get_integer('general','settle_time', 1.5)
#how many frames have been captured
self.frame_count = 0
#Reset state machine
self.initialize_landing()
# Variable sendiri
self.lanjut_cmd = 1
self.arm = True
self.a = False
self.b = 0
self.c = False
self.relay(0) #servo
self.relay1(0) #indikator
self.relay2(0) #magnet
self.servo(0)
self.waktu = 0
self.waktu1 = 0
self.drop = 0
motorA = motor(17,27)
motorB = motor(22,10)
gyro=mpu6050()
gyro.setFilter(2)
greenLed = led(24)
yellowLed = led(25)
greenLed.on()
yellowLed.off()
# regulator settings
Ta = 0.05
Time =120 / Ta
regulator = pid()
regulator.setKp(20)
#regulator.setKi(0.2)
#regulator.setKd(3)
regulator.setTa(4*Ta)
regulator.setMinMax(-100,100)
regulator.setTarget(-1.2)
pwmActive = False
angle = 0
plotDataiFile = open("plot.dat", "w")
def blueButtonPressed(channel):
print "blue Button pressed"
sleep(0.5)
while GPIO.input(18) != GPIO.LOW:
