def __init__(self, kp=1.0, ki=0.0, kd=0.0, pinA=4, pinB=21):
self.pid = PID(kp, ki, kd)
self.motor = Motor_encoder(pinA, pinB)
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.z_sp = 2 # z-position set point
self.z_ref_filt = 0 # z ref filtered
self.z_mv = 0 # z-position measured value
self.pid_z = PID() # pid instance for z control
self.vz_sp = 0 # vz velocity set_point
self.vz_mv = 0 # vz velocity measured value
self.pid_vz = PID() # pid instance for z-velocity control
#########################################################
#########################################################
# Add parameters for z controller
self.pid_z.set_kp(10)
self.pid_z.set_ki(0.2)
self.pid_z.set_kd(10)
# Add parameters for vz controller
self.pid_vz.set_kp(1) #87.2)
self.pid_vz.set_ki(0.0)
self.pid_vz.set_kd(0) #10.89)
#########################################################
#########################################################
self.pid_z.set_lim_high(5) # max vertical ascent speed
self.pid_z.set_lim_low(-5) # max vertical descent speed
self.pid_vz.set_lim_high(350) # max velocity of a motor
self.pid_vz.set_lim_low(-350) # min velocity of a motor
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.gm_attitude_ctl = 0 # flag indicates if attitude control is turned on
self.gm_attitude_ctl = rospy.get_param('~gm_attitude_ctl', 0)
self.t_old = 0
rospy.Subscriber('pose', PoseStamped, self.pose_cb)
rospy.Subscriber('velocity', TwistStamped, self.vel_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('pid_vz',
PIDController,
queue_size=1)
self.mot_ref_pub = rospy.Publisher('mot_vel_ref',
Float32,
queue_size=1)
self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed',
Actuators,
queue_size=1)
self.pub_gm_mot = rospy.Publisher('collectiveThrust',
GmStatus,
queue_size=1)
self.cfg_server = Server(MavZCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(10)
self.t_start = rospy.Time.now()
def set_controllers(self):
#Rough estimate for PID controller
self.pid_throttle = PID(0.35, 0.0, 0.0, 0.0, 1.0)
self.pid_brake = PID(0.5, 0.0, 0.0, 0.0, 1.0)
#Low pass filter for steering cmd
self.lowpass_steer = LowPassFilter(0.2, 1.0)
THRESHOLD = (
52, 0, 127, -128, 127, -41
) # Grayscale threshold for dark things...(75, 0, 127, -128, 127, -62)(71, 0, 127, -128, 54, -18)
import sensor, image, time
from pyb import LED
import car
from pid import PID
from pyb import Pin
rho_pid = PID(p=0.5, i=0.01)
theta_pid = PID(p=0.09, i=0.01)
enable = 'False'
exposure_us = 1000
LED(1).on()
LED(2).on()
LED(3).on()
pin1 = Pin('P1', Pin.IN, Pin.PULL_UP)
pin2 = Pin('P2', Pin.IN, Pin.PULL_UP)
pin3 = Pin('P3', Pin.IN, Pin.PULL_UP)
old_L = 0
old_R = 0
clock = time.clock()
sensor.reset()
#sensor.set_vflip(True)
#sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(
sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
sensor.set_gainceiling(2)
sensor.set_auto_exposure(False, exposure_us=2500) #baoguanglv
sensor.set_contrast(+3)
import sensor, image, time
import car
from pid import PID
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
clock = time.clock()
green_threshold = (37, 67, -128, -25, -128, 71) #(76, 96, -110, -30, 8, 66)
size_threshold = 2000
x_pid = PID(p=0.5, i=1, imax=100)
h_pid = PID(p=0.05, i=0.1, imax=50)
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
while(True):
clock.tick()
img = sensor.snapshot()
blobs = img.find_blobs([green_threshold])
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[5]-img.width()/2
h_error = max_blob[2]*max_blob[3]-size_threshold
print("x error: ", x_error)
'''
for b in blobs:
def __init__(self, particle=Particle(), pid=PID()):
self.particle = particle
self.pid = pid
self.reset()
delta_x2 = 1e25
if math.isinf(delta_y2):
delta_y2 = 1e25
distance = math.sqrt(delta_x2 + delta_y2)
self.noise_path.append([self.x, self.y])
self.test_path.append([self.test_x, self.test_y])
self.pid.calculatePID(distance, delta_theta)
self.dynamics(self.pid.v, self.pid.g, fault)
self.noise_path = np.asarray(self.noise_path)
self.test_path = np.asarray(self.test_path)
plt.plot(self.noise_path[:, 0],
self.noise_path[:, 1],
color='red',
linewidth=2)
plt.plot(self.test_path[:, 0],
self.test_path[:, 1],
color='blue',
linewidth=2)
plt.xlabel('x')
plt.ylabel('y')
plt.title('UGV Path')
plt.show()
if __name__ == '__main__':
rover = Rover()
rover.pid = PID([0.1, 0.0, 0.0, 2.0, 0.0, 0.0])
rover.driveAlongPath(0)
red = 0
green = 0
blue = 0
brightness = 16
# Set white balance manually
cam.awb_mode = 'off'
cam.awb_gains = (Fraction(357, 256), Fraction(173, 128))
# Set initial target/output values
target = 6500
output = 6500
# Create PID control structure
pid = PID(0.5, 0, 0, target)
def update_target(x):
global target, pid
target = x + 2000
pid.changeSetpoint(target)
def update_red(x):
global red
red = x
def update_green(x):
global green
import _thread #调试pid用
from pid import PID
import mycar
car = mycar.car()
u1 = UART(1, 115200)
pidL = PID(p=0.064, i=0, d=0)
tt = 120.01
lock = _thread.allocate_lock()
def cpid():
lock.acquire()
#写入countL
car.countL = car.countR = 0
time.sleep_ms(100)
#print(car.countL)
u1.write('REAL_VALUE,' + str(car.countL) + '\r\n')
#get count
countL = car.countL
errorL = tt - car.countL
outputL = pidL.get_pid(errorL, 1)
#print(countL,errorL,outputL)
car.r.pulse_width_percent(car.lv + outputL)
lock.release()
def threadwhile():
while (1):
cpid()
def __init__(self, name, crazyflie):
QThread.__init__(self)
self.topic_name = name
self.cf = crazyflie
self.cf_physical_params = CF_parameters()
# Import the PID gains (from the firmware)
self.cf_pid_gains = CF_pid_params()
# Out from the PIDs, values of
# r, p, y, thrust
self.desired_rpy = np.zeros(3)
# Comes from the external position controller
self.desired_thrust = 0.0
self.mode = ""
self.setPoint = np.zeros(4)
self.stop_flag = True
######################
# Position
######################
self.x_pid = PID(self.cf_pid_gains.KP_X, self.cf_pid_gains.KI_X,
self.cf_pid_gains.KD_X, self.cf_pid_gains.INT_MAX_X,
self.cf_pid_gains.X_DT)
self.y_pid = PID(self.cf_pid_gains.KP_Y, self.cf_pid_gains.KI_Y,
self.cf_pid_gains.KD_Y, self.cf_pid_gains.INT_MAX_Y,
self.cf_pid_gains.Y_DT)
self.z_pid = PID(self.cf_pid_gains.KP_Z, self.cf_pid_gains.KI_Z,
self.cf_pid_gains.KD_Z, self.cf_pid_gains.INT_MAX_Z,
self.cf_pid_gains.Z_DT)
self.desired_pos = np.zeros(3)
######################
# Linear velocities
######################
self.vx_pid = PID(self.cf_pid_gains.KP_VX, self.cf_pid_gains.KI_VX,
self.cf_pid_gains.KD_VX,
self.cf_pid_gains.INT_MAX_VX,
self.cf_pid_gains.VX_DT)
self.vy_pid = PID(self.cf_pid_gains.KP_VY, self.cf_pid_gains.KI_VY,
self.cf_pid_gains.KD_VY,
self.cf_pid_gains.INT_MAX_VY,
self.cf_pid_gains.VY_DT)
self.vz_pid = PID(self.cf_pid_gains.KP_VZ, self.cf_pid_gains.KI_VZ,
self.cf_pid_gains.KD_VZ,
self.cf_pid_gains.INT_MAX_VZ,
self.cf_pid_gains.VZ_DT)
self.desired_lin_vel = np.zeros(3)
######################
# Angular velocities
######################
self.wx_pid = PID(self.cf_pid_gains.KP_WX, self.cf_pid_gains.KI_WX,
self.cf_pid_gains.KD_WX,
self.cf_pid_gains.INT_MAX_WX,
self.cf_pid_gains.WX_DT)
self.wy_pid = PID(self.cf_pid_gains.KP_WY, self.cf_pid_gains.KI_WY,
self.cf_pid_gains.KD_WY,
self.cf_pid_gains.INT_MAX_WY,
self.cf_pid_gains.WY_DT)
self.wz_pid = PID(self.cf_pid_gains.KP_WZ, self.cf_pid_gains.KI_WZ,
self.cf_pid_gains.KD_WZ,
self.cf_pid_gains.INT_MAX_WZ,
self.cf_pid_gains.WZ_DT)
self.desired_ang_vel = np.zeros(3)
######################
# Attitudes
######################
self.roll_pid = PID(self.cf_pid_gains.KP_ROLL,
self.cf_pid_gains.KI_ROLL,
self.cf_pid_gains.KD_ROLL,
self.cf_pid_gains.INT_MAX_ROLL,
self.cf_pid_gains.ROLL_DT)
self.pitch_pid = PID(self.cf_pid_gains.KP_PITCH,
self.cf_pid_gains.KI_PITCH,
self.cf_pid_gains.KD_PITCH,
self.cf_pid_gains.INT_MAX_PITCH,
self.cf_pid_gains.PITCH_DT)
self.yaw_pid = PID(self.cf_pid_gains.KP_YAW, self.cf_pid_gains.KI_YAW,
self.cf_pid_gains.KD_YAW,
self.cf_pid_gains.INT_MAX_YAW,
self.cf_pid_gains.YAW_DT)
self.desired_att = np.zeros(3)
#instantiate IMU #TODO see how to import C interface to python imu = IMU() #MyKalman=KalmanFilter(....) #instantiate motors and put them together in a list motor1 = Motor(1) motor2 = Motor(2) motor3 = Motor(3) motor4 = Motor(4) motors = [motor1, motor2, motor3, motor4] # instantiate PID controllers and init them rollPID = PID(0.9, 0.2, 0.3) # Kp, Kd, Ki pitchPID = PID(0.9, 0.2, 0.3) yawPID = PID(0.06, 0.02, 0.01) #zPID=PID(.....) #xposPID=PID(.....) #yposPID=PID(.....) #init pitch, roll and yaw: roll = 0 pitch = 0 yaw = 0 print "------------------------" print " stabilize loop " print "------------------------" ############################
def __init__(self):
#Raio obtido
self.obtainedRadiusToTarget = 0.0
#Angulo Obtido em Relacao ao Target
self.obtainedAngleToTarget = 0.0
#Angulo Obtido em Relacao ao Robo
self.obtainedAngleToRobot = 0.0
#Distancia obtida
self.obtainedDistanceToRobot = 0.0
#Distancia obtida ao alien
self.obtainedDistanceToAlien = 0.0
#angulo obtido ao alien
self.obtainedAngleToAlien = 0.0
#nova dimensao
self.obtainedDistanceFromTargetToRobot = 0.0
#novo controle
self.circularCoefToTarget = 0.0
#novo controle
self.propDistCoefToRobot = 0.0
#novo controle
self.linearCoefTargetToRobot = 0.0
#novo controle
self.propAngularCoefToRobot = 0.0
#novo controle
self.linearCoefToTarget = 0.0
#self
self.proximityCoefToRobot = 0.0
#self
self.actualTime = 0.0
#self
self.previousTime = 0.0
#self
self.approachRadius = 0.0
#self
#self.vRobot = LinAng()
#localizacao x,y , com base no frame centralizado em mim
self.target = LinAng()
self.robot = LinAng()
self.alien = LinAng()
#substituida por linear velocity, angular_velocity
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity
self.realLinearVelocity = 0.0
self.realAngularVelocity = 0.0
#velocidade tangencial/linear da roda direita e esquerda #m/s
self.rightLinearVelocity = 0.0
self.leftLinearVelocity = 0.0
#rotacao da roda direita e esquerda #r.p.s
self.rightWheelRotation = 0.0
self.leftWheelRotation = 0.0
#Target detectado?
self.hasTarget = False
#Robo detectado?
self.hasRobot = False
#Alien detectado?
self.hasAlien = False
#Status
self.status = 0
#pids
self.pidLinear = PID(3.0, 4.0, 1.2)
self.pidLinear.setPoint(sp.desired_radius)
self.pidAngular = PID(3.0, 4.0, 1.2)
self.pidAngular.setPoint(sp.desired_angle_to_target)
#mutable circumnavigation appRadiusCoef
self.desired_radius = sp.desired_radius
#added in 12dez2018
self.light_factor = 0.0
# '>>> '))
if ptu_offset_mode == 1:
#Command PTU to point at sun
ptu.ephem_point(ep,imu=imu,target='sun',init=False)
if ptu_offset_mode == 2:
#Command PTU to point at moon
ptu.ephem_point(ep,imu=imu,target='moon',init=False)
else:
track_mode = default_track_mode
filter_mode = default_filter_mode
ptu_offset_mode = default_ptu_offset_mode
#Initiate PID control loop
pid_x= PID(step_size=params.ptu_step_size) #'eighth' #pid_x will control azimuth ptu motor (assuming orientation of ss is correct)
pid_x.SetKp(kp_x)
pid_x.SetKi(ki_x)
pid_x.SetKd(kd_x)
pid_y= PID(step_size=params.ptu_step_size) #pid_y will control azimuth ptu motor (assuming orientation of ss is correct)
pid_y.SetKp(kp_y)
pid_y.SetKi(ki_y)
pid_y.SetKd(kd_y)
#Set ptu=None if not using tracking to ensure PTU is not moved after initial offset
if track_mode == 4:
ptu.ptu.close()
ptu=None
print('Not tracking, so disconnecting from the PTU for safe measure')
def execute_next(self): action = self.actions.pop(0) direction = None if action == "MoveF" or action == "MoveB": current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) current_yaw = euler[2] if current_yaw > (-math.pi /4.0) and current_yaw < (math.pi / 4.0): print "Case 1" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 1.0 #direction = 'x' #incr y co-ordinate elif current_yaw > (math.pi / 4.0 ) and current_yaw < (3.0 * math.pi / 4.0): print "Case 2" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y += 1.0 #direction = 'y' #decr x co elif current_yaw > (-3.0*math.pi /4.0) and current_yaw < (-math.pi /4.0): print "Case 3" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y -= 1.0 #direction = '-y' else: print "Case 4" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 1.0 #direction = '-x' PID(target_pose,"linear",self.id).publish_velocity() elif action == "TurnCW" or action == "TurnCCW": current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) yaw = euler[2] if action == "TurnCW": target_yaw = yaw - ( math.pi / 2.0) if target_yaw < -math.pi: target_yaw += (math.pi * 2) else: target_yaw = yaw + ( math.pi / 2.0) if target_yaw >= (math.pi ): target_yaw -= (math.pi * 2 ) target_pose = Pose() target_pose.position = current_pose.position target_quat = Quaternion(*tf.transformations.quaternion_from_euler(euler[0],euler[1],target_yaw)) target_pose.orientation = target_quat print target_pose.orientation PID(target_pose,"rotational",self.id).publish_velocity() else: print "Invalid action" exit(-1) if len(self.actions) == 0: self.status_publisher.publish(self.free)
from pid import PID
from radio import Radio
from servos import Servos
# r = Radio('/dev/ttyUSB0')
# while True:
# if r.bytes_available() > 16:
# data = r.read()
# print(data.decode())
# r.close()
if __name__ == '__main__':
imu = IMU()
servos = Servos()
pid_roll = PID(600., 0., 0., control_range=[-250, 250])
pid_pitch = PID(-600., 0., 0., control_range=[-250, 250])
pid_yaw = PID(10., 0., 0)
pids = [pid_roll, pid_pitch, pid_yaw]
rpy = np.zeros(3)
sp_rpy = np.zeros(3)
cmd = np.array([1000, 1000, 1500, 1500])
throttle = 1100
tt = t = time.time()
running = True
while running:
t = time.time()
dt = t - tt
## Grabs Temperature of GPU/CPU
#
# @return temperature of GPU/CPU
def grab_temp():
val = 0
with open("/sys/class/thermal/thermal_zone0/temp", "r") as file:
val = int(file.readline()) / 1000
return val
# Initial loop setup
ploop = PID(1,
1,
.02,
Integrator_max=100,
Integrator_min=0,
Set_Point=TEMP_TARGET)
cycle = 1
fan = Motor(FAN_PIN, DUMMY_PIN)
fan.forward(cycle)
last_duty_cycle = 0
last_temp = 0
while True:
sleep(1)
temp = grab_temp()
cycle_change = ploop.update(temp)
cycle = (100 - int(cycle_change)
) / 100 # Since fan.forward() wants a number between 0 and 1
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband,
decel_limit, accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel, max_steer_angle):
# TODO: Implement
rospy.logwarn(
"[Controller Init] vehicle_mass: {0}".format(vehicle_mass))
rospy.logwarn("[Controller Init] decel_limit: {0}".format(decel_limit))
rospy.logwarn("[Controller Init] accel_limit: {0}".format(accel_limit))
rospy.logwarn(
"[Controller Init] wheel_radius: {0}".format(wheel_radius))
min_speed = 0.1
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
#original
#original
#kp = 0.3
#ki = 0.1
#kd = 0.
#mn = 0. #Minimum throttle value
#mx = 0.2#Maximum throttle value
#self.throttle_controller = PID(kp,ki,kd,mn,mx)
#0530, add steering and throttle controller
###################
# Throttle
###################
#highway ok
#testing lot ok, need use 10kms
throttle_kp = 0.3
throttle_ki = 0.1
throttle_kd = 0.005
#testing lot ok, need use 10kms
#throttle_kp = 0.8
#throttle_ki = 0.002
#throttle_kd = 0.3
#not good in testing log
#throttle_kp = 0.8
#throttle_ki = 0.00
#throttle_kd = 0.06
min_throttle = 0.0 # Minimum throttle value
#max_throttle = 0.2 # Maximum throttle value (should be OK for simulation, not for Carla!)
#max_throttle = 0.5*accel_limit
max_throttle = accel_limit
#self.throttle_controller =PID(2.0, 0.4, 0.1, min_throttle, max_throttle)
self.throttle_controller = PID(throttle_kp, throttle_ki, throttle_kd,
min_throttle, max_throttle)
###################
# Steering
###################
#highway ok
#testing lot ok, need use 10kms
steer_kp = 0.4
steer_ki = 0.1
steer_kd = 0.005
#testing lot ok, need use 10kms
#steer_kp = 1.15
#steer_ki = 0.001
#steer_kd = 0.1
#not good in testing log
#steer_kp = 0.5
#steer_ki = 0.00
#steer_kd = 0.2
min_steer = -max_steer_angle
max_steer = max_steer_angle
self.steering_controller = PID(steer_kp, steer_ki, steer_kd, min_steer,
max_steer)
#is for filter out the high frequency noise of volcity
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = 0.02 #Sample time
self.vel_lpf = LowPassFilter(tau, ts)
# catch the input
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.wheel_base = wheel_base
self.steer_ratio = steer_ratio
self.max_lat_accel = max_lat_accel
self.max_steer_angle = max_steer_angle
self.last_time = rospy.get_time()
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
# X controller
self.x_sp = 0.0
self.x_mv = 0.0
self.pid_x = PID()
self.vx_sp = 0.0
self.vx_mv = 0.0
self.pid_vx = PID()
# X controller
self.y_sp = 0.0
self.y_mv = 0.0
self.pid_y = PID()
self.vy_sp = 0.0
self.vy_mv = 0.0
self.pid_vy = PID()
# Z controller
self.z_sp = 0.0 # z-position set point
self.z_ref_filt = 0 # z ref filtered
self.z_mv = 0 # z-position measured value
self.pid_z = PID() # pid instance for z control
self.vz_sp = 0 # vz velocity set_point
self.vz_mv = 0 # vz velocity measured value
self.pid_vz = PID() # pid instance for z-velocity control
#########################################################
#########################################################
# Add parameters for x controller
self.pid_x.set_kp(0.65)# 0.05
self.pid_x.set_ki(0.0)
self.pid_x.set_kd(0.0) # 0.07
self.pid_x.set_lim_high(500) # max vertical ascent speed
self.pid_x.set_lim_low(-500) # max vertical descent speed
# Add parameters for vx controller
self.pid_vx.set_kp(0.11) # 0.11
self.pid_vx.set_ki(0.0)
self.pid_vx.set_kd(0)
self.pid_vx.set_lim_high(500) # max velocity of a motor
self.pid_vx.set_lim_low(-500) # min velocity of a motor
# Add parameters for y controller
self.pid_y.set_kp(0.65)
self.pid_y.set_ki(0.0) #0.05
self.pid_y.set_kd(0.0) #0.03
self.pid_y.set_lim_high(500) # max vertical ascent speed
self.pid_y.set_lim_low(-500) # max vertical descent speed
# Add parameters for vy controller
self.pid_vy.set_kp(0.11) # 0.11
self.pid_vy.set_ki(0.0)
self.pid_vy.set_kd(0)
self.pid_vy.set_lim_high(500) # max velocity of a motor
self.pid_vy.set_lim_low(-500) # min velocity of a motor
# Add parameters for z controller
self.pid_z.set_kp(100)
self.pid_z.set_ki(1)
self.pid_z.set_kd(100)
self.pid_z.set_lim_high(500) # max vertical ascent speed
self.pid_z.set_lim_low(-500) # max vertical descent speed
# Add parameters for vz controller
self.pid_vz.set_kp(1)#87.2)
self.pid_vz.set_ki(0.0)
self.pid_vz.set_kd(0)#10.89)
self.pid_vz.set_lim_high(500) # max velocity of a motor
self.pid_vz.set_lim_low(-500) # min velocity of a motor
#########################################################
#########################################################
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.t_old = 0
rospy.Subscriber('pose', PoseStamped, self.pose_cb)
rospy.Subscriber('odometry', Odometry, self.vel_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
rospy.Subscriber('reset_controllers', Empty, self.reset_controllers_cb)
self.pub_pid_x = rospy.Publisher('pid_x', PIDController, queue_size=1)
self.pub_pid_vx = rospy.Publisher('pid_vx', PIDController, queue_size=1)
self.pub_pid_y = rospy.Publisher('pid_y', PIDController, queue_size=1)
self.pub_pid_vy = rospy.Publisher('pid_vy', PIDController, queue_size=1)
self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('pid_vz', PIDController, queue_size=1)
self.mot_ref_pub = rospy.Publisher('mot_vel_ref', Float64, queue_size=1)
self.euler_ref_pub = rospy.Publisher('euler_ref', Vector3, queue_size=1)
#self.pub_gm_mot = rospy.Publisher('collectiveThrust', GmStatus, queue_size=1)
self.cfg_server = Server(VpcMmuavPositionCtlParamsConfig, self.cfg_callback)
self.rate = rospy.get_param('~rate', 100)
self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz
self.t_start = rospy.Time.now()
import RPi.GPIO as GPIO
import time
import threading
from encoderThread import Encoder
from pid import PID
encoder = Encoder(21, 20) #port nums for encoder
pid = PID(0.001, 0, 0, 2000) # p , i , d , setpoint
pid.setMinMax(-1.0, 1.0)
class PWMMotorControl(threading.Thread):
def __init__(self, port):
threading.Thread.__init__(self)
self.port = port
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.port, GPIO.OUT)
self.motor = GPIO.PWM(self.port, 50)
self.speed = 0
self.pwmSpeed = 0
self._stop = threading.Event()
self.controlEnabled = True
def run(self):
self.motor.start(0)
while self.controlEnabled:
if self.stopped():
self.motor.ChangeDutyCycle(0)
return
self.motor.ChangeDutyCycle(self.pwmSpeed)
def __init__(self):
"""Constructor initializes all needed variables"""
self.mass = 0.5 # kg --> mass of the quadcopter
self.Ix = 0.00389 # kg m^2 --> Quadrotor moment of inertia in body x direction
self.Iy = 0.00389 # kg m^2 --> Quadrotor moment of inertia in body y direction
self.Iz = 0.0078 # kg m^2 --> Quadrotor moment of inertia in body z direction
self.Tm = 0.0125 # s --> Time constant of a motor
self.bf = 8.548e-6 # kg m --> Thrust constant of a motor
self.bm = 0.016 # m --> Moment constant of a motor
self.l = 0.12905 # m --> The distance of a motor from a center of mass
self.gravity = 9.81 # m/s^2 --> Gravity value
self.sleep_sec = 2
self.hover_speed = math.sqrt(4.905 / self.bf / 4)
self.first_measurement = False
self.controller_info = rospy.get_param("~verbose", False)
self.wind_controller = rospy.get_param("~wind", False)
self.hoover = rospy.get_param("~hoover", False)
self.odom_subscriber = rospy.Subscriber("bebop/odometry", Odometry,
self.odometry_callback)
self.pose_subscriber = rospy.Subscriber("bebop/pos_ref", Vector3,
self.setpoint_cb)
self.odom_gt_subscriber = rospy.Subscriber("bebop/odometry_gt",
Odometry,
self.odometry_gt_callback)
self.angle_subscriber = rospy.Subscriber("bebop/angle_ref", Vector3,
self.angle_cb)
self.velocity_subscriber = rospy.Subscriber("bebop/lin_vel_pub",
Vector3, self.linvel_cb)
# initialize publishers
self.motor_pub = rospy.Publisher('/gazebo/command/motor_speed',
Actuators,
queue_size=10)
self.error_pub = rospy.Publisher('/bebop/pos_error',
Float64,
queue_size=10)
self.error_vel_pub = rospy.Publisher('/bebop/vel_error',
Float64,
queue_size=10)
self.motor_pub = rospy.Publisher('/gazebo/command/motor_speed',
Actuators,
queue_size=10)
self.actuator_msg = Actuators()
# define vector for measured and setopint values
if self.hoover == True:
self.pose_sp = Vector3(0., 0., 1.)
else:
self.pose_sp = Vector3(0., 0., 0.)
self.euler_sp = Vector3(0., 0., 0.)
self.euler_mv = Vector3(0., 0., 0.)
self.euler_rate_mv = Vector3(0., 0., 0.)
self.t_old = 0
# define PID for height control
self.z_mv = 0
# Crontroller rate
self.controller_rate = 50
self.rate = rospy.Rate(self.controller_rate)
# define PID for height rate control
self.vz_sp = 0 # vz velocity set point
self.vz_mv = 0 # vz velocity measured value
# Height controller
self.pid_vz = PID(195.8, 0, 1.958, 300, -300)
# Position loop
if self.wind_controller:
# TODO: Tune paramters
print("LaunchBebop.init() - Wind parameters active")
self.pid_z = PID(4, 0.05, 0.1, 10, -10)
self.pid_x = PID(0.5, 0.06, 0.03, 0.35, -0.35)
self.pid_y = PID(0.5, 0.06, 0.03, 0.35, -0.35)
else:
print("LaunchBebop.init() - Non Wind parameters active")
self.pid_z = PID(4, 0.05, 0.1, 10, -10)
self.pid_x = PID(0.25, 0.05, 0.055, 0.18, -0.18)
self.pid_y = PID(0.25, 0.05, 0.055, 0.18, -0.18)
# outer_loops
self.pitch_PID = PID(4.44309, 0.1, 0.2, 100, -100)
self.roll_PID = PID(4.44309, 0.1, 0.2, 100, -100)
self.yaw_PID = PID(25, 0, 0.0, 150, -150)
# inner_loops
self.pitch_rate_PID = PID(16.61, 0, 0, 100, -100)
self.roll_rate_PID = PID(16.61, 0, 0, 100, -100)
# Pre-filter constants
self.filt_const_x = 0.5
self.filt_const_y = 0.5
self.filt_const_z = 0.1
# Post filter values
self.z_filt_sp = 0
self.y_filt_sp = 0
self.x_filt_sp = 0
# Define magic thrust number :-)
self.magic_number = 0.908
# Velocity refs at start
self.linvel_x = 0
self.linvel_y = 0
self.linvel_z = 0
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband,
decel_limit, accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel, max_steer_angle):
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
# Parameters for the throttle control PID controller #
kp = 0.3
#kp = 1.0
#kp = 1.5
#kp = 0.5
#kp = 0.2
#kp = 0.05
#kp = 0.02
#kp = 0.03
#kp = 0.045
#kp = 0.035
#kp = 0.03
#kp = 0.027
#kp = 0.02
#kp = 0.5
#kp = 0.035
ki = 0.1
#ki = 0.3
#ki = 0.0
#ki = 0.02
#ki = 0.005
#ki = 0.01
#ki = 0.005
#ki = 0.008
#ki = 0.007
#ki = 0.006
#ki = 0.0055
#ki = 0.005
#ki = 0.003
#ki = 0.004
#ki = 0.0045
#ki = 0.003
#ki = 0.0
kd = 0.0
#kd = 0.04
#kd = 0.0
#kd = 0.005
#kd = 0.001
#kd = 0.0005
#kd = 0.0
mn = 0.0 # Minimum throttle value #
mx = 0.2 # Maximum throttle value #
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# Low pass filter to reduce the effect of the noise of the incoming velocity #
tau = 0.5 # 1/(2pi*tau)_ = cutoff frequency #
ts = 0.02 # [s] Sample time #
self.vel_lpf = LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
gains = self.step_tune()
timer -= 1
# sleep(1)
if gains:
return gains
else:
return None
if __name__ == '__main__':
test_vessel = Vessel(dat_file='dat', duty=50)
tuner = AutoTune(vessel=test_vessel)
pid_gain = tuner.tune()
if pid_gain:
pid_obj = PID(
P=pid_gain['Kp'],
I=pid_gain['Ki'],
D=pid_gain['Kd'],
on_error=False,
output_min=0,
output_max=100,
)
pickle.dump(pid_obj, open(os.path.join(CONFIG_PATH, 'PID.p'), 'wb'))
else:
print('PID gains not found', file=sys.stderr)
import sensor, image, time
from pid import PID
from pyb import Servo
pan_servo = Servo(1)
tilt_servo = Servo(2)
red_threshold = (13, 49, 18, 61, 6, 47)
pan_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
tilt_pid = PID(p=0.05, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
#pan_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
#tilt_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
def find_max(blobs):
max_size = 0
for blob in blobs:
if blob[2] * blob[3] > max_size:
max_blob = blob
max_size = blob[2] * blob[3]
return max_blob
import sensor, image, time, random import car, hand import math from pid import PID sensor.reset() # Initialize the camera sensor. sensor.set_pixformat(sensor.RGB565) # use RGB565. sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. sensor.skip_frames(60) # Let new settings take affect. sensor.set_auto_whitebal(False) # turn this off. clock = time.clock() # Tracks FPS. #x_pid = PID(p=1, i=6,d=0.5, imax=100) x_pid = PID(p=1, i=3, d=0.5, imax=100) y_pid = PID(p=0.1, i=0.1, d=0.1, imax=40) ''' red_threshold = (46, 100, 40, 111, -11, 64) #red_threshold = (46, 77, 53, 111, -11, 64) green_threshold = (32, 92, -69, -34, -1, 39) #green_threshold = (32, 100, -77, -24, -24, 22) blue_threshold = (32, 77, -12, 27, -69, -25) black_threshold = (0, 25, -17, 15, -18, 15) ''' red_threshold = (43, 74, 27, 77, -8, 50) green_threshold = (48, 70, -78, -40, 35, 60) #green_threshold = (32, 100, -77, -24, -24, 22) blue_threshold = (34, 100, -55, 3, -51, -6) black_threshold = (0, 25, -17, 15, -18, 15) #ball_threshold = red_threshold ball_threshold = blue_threshold
def set_controllers(self):
#PID Controllers for throttle and brake
self.pid_throttle = PID(0.35,0.0,0.0,0.0,1.0)
self.pid_brake = PID(0.3,0.0,0.0,0.0,1.0)
#Low pass filter to smooth out the steering
self.lowpass_steer = LowPassFilter(0.2,1.0)
