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: