def pid_loop(state):
i = 0
pidhist = config.pid_hist_len * [0.]
temphist = config.temp_hist_len * [0.]
temperr = config.temp_hist_len * [0]
temp = 25.
lastsettemp = state['brewtemp']
lasttime = time()
sensor = MAX31855(SPI(board.SCK, MOSI=board.MOSI, MISO=board.MISO),
DigitalInOut(board.D5))
pid = PID(Kp=config.pidc_kp,
Ki=config.pidc_ki,
Kd=config.pidc_kd,
setpoint=state['brewtemp'],
sample_time=config.time_sample,
proportional_on_measurement=False,
output_limits=(-config.boundary, config.boundary))
while True:
try:
temp = sensor.temperature
del temperr[0]
temperr.append(0)
del temphist[0]
temphist.append(temp)
except RuntimeError:
del temperr[0]
temperr.append(1)
if sum(temperr) >= 5 * config.temp_hist_len:
print("Temperature sensor error!")
call(["killall", "python3"])
avgtemp = sum(temphist) / config.temp_hist_len
if avgtemp <= 0.9 * state['brewtemp']:
pid.tunings = (config.pidc_kp, config.pidc_ki, config.pidc_kd)
else:
pid.tunings = (config.pidw_kp, config.pidw_ki, config.pidw_kd)
if state['brewtemp'] != lastsettemp:
pid.setpoint = state['brewtemp']
lastsettemp = state['brewtemp']
pidout = pid(avgtemp)
pidhist.append(pidout)
del pidhist[0]
avgpid = sum(pidhist) / config.pid_hist_len
state['i'] = i
state['temp'] = temp
state['pterm'], state['iterm'], state['dterm'] = pid.components
state['avgtemp'] = round(avgtemp, 2)
state['pidval'] = round(pidout, 2)
state['avgpid'] = round(avgpid, 2)
sleeptime = lasttime + config.time_sample - time()
sleep(max(sleeptime, 0.))
i += 1
lasttime = time()
with SyncCrazyflie(URI) as scf:
# Define motion commander to crazyflie as mc:
cf2 = CF(scf, available)
cf2_psi = 0 # Get current yaw-angle of cf
cf2_bat = cf2.vbat # Get current battery level of cf
cf2_blevel = cf2.blevel
cf2_pwm = cf2.m1_pwm
cf2_temp = cf2.temp
cf2_phi = 0
cf2_theta = 0
marker_psi = 0 # Set marker angle to zero initially
pid = PID(0, 0, 0, setpoint=marker_psi) # Initialize PID
pid_x = PID(0, 0, 0, setpoint=0) # init PID for roll
# Define pid parameters
pid.tunings = (Kp_psi, Kd_psi, Ki_psi)
pid_x.tunings = (Kp_x, Kd_x, Ki_x)
pid_x.sample_time = 0.05 # update pid every 50 ms
pid.output_limits = (-60, 60)
pid_x.output_limits = (-20, 20)
pid.proportional_on_measurment = False
pid_x.proportional_on_measurment = False
if cf2_bat >= low_battery:
cf2.takeoff() # CF takes off from ground
print("Taking off ! Battery level: " + str(cf2_bat))
time.sleep(1.5) # Let the CF hover for 1.5s
t_init = time.time()
endTime = t_init + 30000 # Let the CF do its thing for some time
def mainThread(debug=False):
""" Controls the robot including joystick input, computer vision, line following, etc.
Arguments:
debug: (optional) log debugging data
"""
DC = ControllerUtils.DriveController(flip=[1, 0, 1, 0, 0, 0, 0, 0])
# Get Controller
gamepad = None
while (not gamepad) and execute['mainThread']:
time.sleep(5)
try:
gamepad = ControllerUtils.identifyController()
except Exception as e:
print(e)
newestImage = []
newestSensorState = {
'imu': {
'calibration': {
'sys': 2,
'gyro': 3,
'accel': 0,
'mag': 0
},
'gyro': {
'x': 0,
'y': 0,
'z': 0
},
'vel': {
'x': -0.01,
'y': 0.0,
'z': -0.29
}
},
'temp': 25
}
# Initalize PID controllers
Kp = 1
Kd = 0.1
Ki = 0.05
xRotPID = PID(Kp, Kd, Ki, setpoint=0)
yRotPID = PID(Kp, Kd, Ki, setpoint=0)
zRotPID = PID(Kp, Kd, Ki, setpoint=0)
mode = "user-control"
override = False
print("mode:", mode)
print("override:", override)
lastMsgTime = time.time()
minTime = 1.0 / 10.0
while execute['mainThread']:
while not mainQueue.empty():
recvMsg = mainQueue.get()
if recvMsg['tag'] == 'cam':
#newestImage = CommunicationUtils.decodeImage(recvMsg['data'])
pass
elif recvMsg['tag'] == 'sensor':
newestSensorState = recvMsg['data']
# Get Joystick Input
event = gamepad.read_one()
if event:
if (ControllerUtils.isOverrideCode(event, action="down")):
override = True
print("override:", override)
elif (ControllerUtils.isOverrideCode(event, action="up")
and override):
override = False
print("override:", override)
if (ControllerUtils.isStopCode(event)):
handlePacket(CommunicationUtils.packet("stateChange", "close"))
time.sleep(1)
stopAllThreads()
elif (ControllerUtils.isZeroMotorCode(event)):
handlePacket(
CommunicationUtils.packet("motorData",
DC.zeroMotors(),
metadata="drivetrain"))
elif (ControllerUtils.isStabilizeCode(event)):
if (mode != "stabilize"):
# Reset PID controllers
xRotPID.reset()
yRotPID.reset()
zRotPID.reset()
xRotPID.tunings = (Kp, Ki, Kd)
yRotPID.tunings = (Kp, Ki, Kd)
zRotPID.tunings = (Kp, Ki, Kd)
# Assuming the robot has been correctly calibrated, (0,0,0) should be upright
xRotPID.setpoint = 0
yRotPID.setpoint = 0
zRotPID.setpoint = 0
mode = "stabilize"
print("mode:", mode)
else:
mode = "user-control"
print("mode:", mode)
if (mode == "user-control" or override):
DC.updateState(event)
speeds = DC.calcThrust()
if (time.time() - lastMsgTime > minTime):
handlePacket(
CommunicationUtils.packet("motorData",
speeds,
metadata="drivetrain"))
lastMsgTime = time.time()
elif (mode == "stabilize" and not override):
xTgt = xRotPID(newestSensorState["imu"]["gyro"]["x"])
yTgt = yRotPID(newestSensorState["imu"]["gyro"]["y"])
zTgt = zRotPID(newestSensorState["imu"]["gyro"]["z"])
speeds = DC.calcPIDRot(xTgt, yTgt, zTgt)
if (time.time() - lastMsgTime > minTime):
handlePacket(
CommunicationUtils.packet("motorData",
speeds,
metadata="drivetrain"))
lastMsgTime = time.time()
if (robot_heading_to_target - robot_heading
) < math.radians(-180): # something might be wrong here
robot_heading_to_target += math.radians(
360) # something might be wrong here
else:
if (robot_heading_to_target - robot_heading
) > math.radians(180): # something might be wrong here
robot_heading_to_target -= math.radians(
360) # something might be wrong here
robot_distance_to_target = distance(robot_center_position,
current_target_position)
pid_left_right.tunings = (
0.3, 0.001, 0.01
) # tuning depends on the robot configuration, vision delay, etc
if math.fabs(robot_heading_to_target -
robot_heading) > math.radians(5): # I-term anti windup
pid_left_right.Ki = 0
pid_left_right.output_limits = (
-0.3, 0.3
) # tuning depends on the robot configuration, vision delay, etc
pid_left_right.sample_time = 0.01 # update every 0.01 seconds
pid_left_right.setpoint = robot_heading_to_target
ch1 = pid_left_right(robot_heading) * 100 + 100 # steering
pid_speed.tunings = (0.01, 0.0001, 0
) # depends on the robot configuration
if robot_distance_to_target > 5: # I-term anti windup
pid_speed.Ki = 0
sub_thread = threading.Thread(target=controller.thread_function,
daemon=True)
sub_thread.start()
pid = PID(controller.p, controller.i, controller.d, controller.
desired_pitch) #create the pid according to boat desired pitch
pid.sample_time = 0.05
pid.output_limits = (
parameters.PID_PITCH_MIN_CORRECTION(),
parameters.PID_PITCH_MAX_CORRECTION()
) #limit the range of action to aprox (-25,25) degrees
while True: #EXECUTE CONSTANTLY BUT PID ONLY ACTS IF pid.sample_time SECONDS HAVE PASSED
try:
pid.tunings = (controller.p, controller.i, controller.d)
print("Current ctes: ", controller.p, "#", controller.i, "#",
controller.d)
controller.pitch = controller.get_pitch_info()
# if controller.pitch != controller.desired_pitch:
print("Current pitch: ", controller.pitch)
current_time = time.time()
dt = current_time - last_time
change = pid(controller.pitch)
pitch = controller.send_pitch_corrections(change)
last_time = current_time
#plot_debugging_data(x,y,desired_value)
import json
import pyvesc
from pyvesc.messages import GetValues, SetRPM, SetCurrent, SetRotorPositionMode, GetRotorPosition,SetDutyCycle,GetValues
import serial
import time
from simple_pid import PID
import time
import paho.mqtt.client as mqtt
pid = PID(1, 0.1, 0.05, setpoint=1)
pid.sample_time = 0.001 # update every 0.01 seconds
pid.setpoint = 0
normal_tuning=(700,1000,0.000000001)
#break_tuning=(100,500,0)
pid.tunings = normal_tuning #(700, 1000, 0.000000001)
pid.output_limits = (-100000, 100000) # output value will be between 0 and 10
aca=0
motor_speed=0
motor_tach=0
force_break=0
# Set your serial port here (either /dev/ttyX or COMX)
serialport = '/dev/ttyACM2'
out_json={"MOTOR":"RIGTH","SPEED":0,"TACHOMETER":0}
def on_connect(client, userdata, flags, rc):
print("Connected with result code {0}".format(str(rc))) # Print result of connection attempt
client.subscribe("actions") # Subscribe to the topic “digitest/test1”, receive any messages published on it
print("setting up Serial")
time.sleep(1)
print("Getting Data")
while noData == True:
Serial()
print("DATA", dataList)
# PID Gubbins
LTPI, RTPI = 50, 50 # Ticks per Interval, initial setpoint
#tunings = (2, 1.0, 0.01) # Fast but permissibly erratic
#tunings = (1.1, 0.5, 0.5) # makes a bit of a waddle
#tunings = (1.3, 0.7, 0.075) # a little slower to converge but steadiest
tunings = (1.7, 0.4, 0.05) # Default
leftMotor_PID = PID(1.0, 0.5, 0.05, setpoint=LTPI)
rightMotor_PID = PID(1.0, 0.5, 0.05, setpoint=RTPI)
leftMotor_PID.tunings = tunings
rightMotor_PID.tunings = tunings
#leftMotor_PID.sample_time = 0.01 # update every 0.01 seconds
#rightMotor_PID.sample_time = 0.01
leftMotor_PID.output_limits = (-100, 100
) # output value will be between 0 and 100
rightMotor_PID.output_limits = (-100, 100)
prev_leftEnc = 0
prev_rightEnc = 0
while True:
Pause()
if pause == 1:
pass
else:
velocity = 60 bearing = 0 rotation = 0 rotationAccuracy = 1 # PID Gubbins LTPI, RTPI = velocity, velocity # Ticks per Interval, initial setpoint #tunings = (2, 1.0, 0.01) # Fast but permissibly erratic #tunings = (1.1, 0.5, 0.5) # makes a bit of a waddle #tunings = (1.3, 0.7, 0.075) # a little slower to converge but steadiest tunings = (1.0, 0.5, 0.05) # Default rotationalTunings = (0.2, 0.1, 0.06) leftMotor_PID = PID(1.0, 0.5, 0.05, setpoint=LTPI) rightMotor_PID = PID(1.0, 0.5, 0.05, setpoint=RTPI) rotational_PID = PID(1.0, 0.5, 0.05, setpoint=0) leftMotor_PID.tunings = tunings rightMotor_PID.tunings = tunings rotational_PID.tunings = rotationalTunings #leftMotor_PID.sample_time = 0.01 # update every 0.01 seconds #rightMotor_PID.sample_time = 0.01 #rotational_PID.sample_time = 0.01 leftMotor_PID.output_limits = (-100, 100) # output value (Duty Cycle) rightMotor_PID.output_limits = (-100, 100) rotational_PID.output_limits = (-100, 100) # Odometry variables prev_leftEncF, prev_leftEncB = 0, 0 prev_rightEncF, prev_rightEncB = 0, 0 #Rotational PID will need some odoemtry in order to work out rotation travel, TotalTravel, thetaRad, theta = 0.0, 0.0, 0.0, 0.0
accelZ = accel_data['z']
gyroX = gyro_data['x']
gyroY = gyro_data['y']
gyroZ = gyro_data['z']
gyroX -= gyro_offset_x
gyroY -= gyro_offset_y
gyro_x_delta = (gyroX * time_diff)
gyro_y_delta = (gyroY * time_diff)
gyro_total_x += gyro_x_delta
gyro_total_y += gyro_y_delta
rotation_x = x_rotation(accelX, accelY, accelZ)
rotation_y = y_rotation(accelX, accelY, accelZ)
last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
inputSudut = round(last_x, decimalDigitSudut) + round(calibrationValue, decimalDigitSudut)
inputSudut = round(inputSudut, decimalDigit)
pid.tunings = (P, I, D)
PIDx = pid(inputSudut)
# output = computePID(inputSudut)
# PIDx = output
if inputSudut == batas:
equilibrium()
# print("berhenti")
elif PIDx < 0.0:
backward(-int(PIDx))
# print("back")
#StepperFor(-PIDx)
#if the PIDx data is higher than 0.0 than move appropriately forward
elif PIDx > 0.0:
forward(int(PIDx))
# print("forw")
#StepperBACK(PIDx)
import time
from subprocess import call
from simple_pid import PID
mutex = Lock()
mutex_pid = Lock()
logging.basicConfig(level=logging.DEBUG)
vazao_in = parametros['start_vazao_in']
vazao_out = parametros['start_vazao_out']
nivel_ref = parametros['start_nivel_ref'] # setpoint
nivel_atual = parametros['start_nivel_atual']
pid = PID(1, 0.1, 0.05, setpoint=nivel_ref)
pid.output_limits = (0, 10)
pid.tunings = (1.0, 0.2, 0.4)
class ProcessThread(Thread):
def __init__(self, cv, raio_inf, raio_sup, altura):
Thread.__init__(self)
self.cv = (
cv
) # Coeficiente de descarga do tanque -> relacionado com o tamanho do furo no tanque
self.raio_inf = raio_inf # Raio inferior do tanque
self.raio_sup = raio_sup # Raio superior do tanque
self.altura_tanque = altura
self.alfa = (raio_sup - raio_inf) / altura
@staticmethod
def dhdt(self, t, h, u):
def read_settings(fn):
with open(fn) as f:
params = json.load(f)
return params # (tunings["Kp"], tunings["Ki"], tunings["Kd"])
if __name__ == '__main__':
log = tl.LogFile()
settings_fn = "settings.json"
settings = read_settings(settings_fn)
settings_1 = settings["PID_1"]
settings_2 = settings["PID_2"]
pid_tunings = (settings_1["Kp"], settings_1["Ki"], settings_1["Kd"])
pid = PID()
pid.sample_time = updatePIDInt # /100 # settings["sample_time"]
pid.output_limits = (0, 100)
pid.tunings = pid_tunings
pid.setpoint = 25.0
pid.auto_mode = True
pid.proportional_on_measurement = False
print(pid.tunings)
HomebrewApp().run()
def main():
pygame.init()
pygame.font.init()
screen = pygame.display.set_mode((width, height))
clock = pygame.time.Clock()
tunings = (27, 2, 10)
pid = PID(*tunings,
output_limits=(-8000, 8000),
proportional_on_measurement=True)
pid.sample_time = 1 / REFRESH
draw_options = DrawOptions(screen)
pymunk.pygame_util.positive_y_is_up = True
font = pygame.font.SysFont('Lucida Console', 11)
space = pymunk.Space()
space.gravity = 0, -100
x = random.randint(120, 380)
drone = Drone((x, 450), space)
ground = Ground(space)
setpoint = 450
ticks = 0
while True:
for event in pygame.event.get():
# print(event)
if event.type == pygame.QUIT:
sys.exit(0)
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP:
setpoint += 5
elif event.key == pygame.K_DOWN:
setpoint -= 5
elif event.key == pygame.K_ESCAPE:
sys.exit(0)
elif event.key == pygame.K_1:
p, i, d = pid.tunings
pid.tunings = (p + 0.5, i, d)
elif event.key == pygame.K_q:
p, i, d = pid.tunings
pid.tunings = (p - 0.5, i, d)
elif event.key == pygame.K_2:
p, i, d = pid.tunings
pid.tunings = (p, i + 0.5, d)
elif event.key == pygame.K_w:
p, i, d = pid.tunings
pid.tunings = (p, i - 0.5, d)
elif event.key == pygame.K_3:
p, i, d = pid.tunings
pid.tunings = (p, i, d + 0.5)
elif event.key == pygame.K_e:
p, i, d = pid.tunings
pid.tunings = (p, i, d - 0.5)
elif event.type == pygame.MOUSEBUTTONUP:
setpoint = 600 - event.pos[1]
# ticks=0
screen.fill((0, 0, 0))
error = drone.body.position.y - setpoint
output = pid(error)
space.debug_draw(draw_options)
# if drone.shape.body.position.y < setpoint:
if True: # output > 0:
# drone.shape.body.apply_force_at_local_point((0, 340), (0, 0))
drone.shape.body.apply_force_at_local_point((0, output), (0, 0))
c = to_pygame(drone.body.position, screen)
pygame.draw.aaline(screen, (0, 128, 255, .75), c,
(c[0], c[1] - output // 10))
pygame.draw.aaline(screen, (128, 255, 128, .125),
to_pygame((50, setpoint), screen),
to_pygame((600 - 50, setpoint), screen))
# pygame.draw.aaline(screen, (255, 255, 255, .5), to_pygame((0,0), screen), to_pygame((100,100), screen))
textsurface = font.render(
f"pos:{drone.body.position.y:7.1f} set:{setpoint} error:{error:7.1f} o:{output:7.1f}",
False, WHITE)
screen.blit(textsurface, (10, 10))
textsurface = font.render(
f"pid:{['{:7.1f}'.format(p) for p in pid.components]}", False,
WHITE)
screen.blit(textsurface, (10, 25))
textsurface = font.render(
f"tun:{['{:7.1f}'.format(p) for p in pid.tunings]}", False, WHITE)
screen.blit(textsurface, (10, 40))
textsurface = font.render(f"ticks:{ticks}", False, WHITE)
screen.blit(textsurface, (10, 55))
space.step(1 / REFRESH)
pygame.display.update()
clock.tick(REFRESH)
pygame.display.set_caption(f"{drone.body.position.y:.1f} {setpoint}")
ticks += 1
gray = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
dst = cv2.filter2D(gray, -1, kernel)
faces = face_cascade.detectMultiScale(dst, 1.3, 5)
for (x, y, w, h) in faces:
cv2.rectangle(frame1, (x, y), (x + w, y + h), (0, 255, 0), 1)
centerFace = (int(x + w / 2), int(y + h / 2))
cv2.circle(frame1, centerFace, 5, (0, 255, 0), 1)
roi_gray = gray[y:y + h, x:x + w]
roi_color = frame1[y:y + h, x:x + w]
faceDetected = True
if faceDetected:
pidFaceLR.tunings = (0.5, 0.000, 0.001)
pidFaceLR.output_limits = (-0.5, 0.5)
pidFaceLR.sample_time = 0.01 # update every 0.01 seconds
pidFaceLR.setpoint = 0
ch1 = pidFaceLR(
(centerFace[0] - frame1Center[0]) / 100) * 100 + 100 # left right
pidFaceUD.tunings = (0.5, 0.000, 0.001)
pidFaceUD.output_limits = (-1, 1)
pidFaceUD.sample_time = 0.01 # update every 0.01 seconds
pidFaceUD.setpoint = 0
ch4 = pidFaceUD(
(centerFace[1] - frame1Center[1]) / 100) * 100 + 100 # up down
if 110 < w < 300:
ch2 = 80
#PID_Parameters[0] = cv2.getTrackbarPos('kP', 'CV PID Settings')
#PID_Parameters[1]= cv2.getTrackbarPos('ki', 'CV PID Settings')
#PID_Parameters[2] = cv2.getTrackbarPos('kd', 'CV PID Settings')
else: #Not Found
angle = 0
pidout = 0
speed = 0
dir = 0
fps.update()
fps.stop()
print(
"Angle(deg): {:+.2f}, PID: Kp{}, Ki{:.3f}, Kd{:.3f}, PIDOut{:+3.3f}, Speed_Command: {:+5}, Direction: {}, fps:{:.2f} , time:{:.2f}\r"
.format(angle, pid.Kp, pid.Ki, pid.Kd, pidout, speed, dir, fps.fps(),
fps.elapsed()),
end="")
pid.tunings = (PID_Parameters[0], PID_Parameters[1], PID_Parameters[2])
cv2.imshow("CV PID Pendulum", frame)
key = cv2.waitKey(1) & 0xFF
# if the 'q' key is pressed, stop the loop
if key == ord("q"):
s.write("0\n".encode())
s.flush()
s.close()
f.close()
fps.stop()
break
if key == ord("s") and enable == False:
f = open("data.txt", "w")
f.write("angle,pidout,speed,fps,elapsed\n")
import pyvesc
from pyvesc.messages import GetValues, SetRPM, SetCurrent, SetRotorPositionMode, GetRotorPosition, SetDutyCycle, GetValues
import serial
import time
from simple_pid import PID
import time
# Set your serial port here (either /dev/ttyX or COMX)
serialport = '/dev/ttyACM1'
pid = PID(1, 0.1, 0.05, setpoint=1)
pid.sample_time = 0.001 # update every 0.01 seconds
pid.setpoint = 10
pid.tunings = (700, 2000, 0.000000001)
pid.output_limits = (-100000, 100000) # output value will be between 0 and 10
print("la concha de tu puta madre")
def get_values_example():
print("running")
input_buffer = b""
output = 0
t = 0
old_tach = 0
with serial.Serial(serialport, baudrate=115200, timeout=0.05) as ser:
try:
# Optional: Turn on rotor position reading if an encoder is installed
ser.write(
pyvesc.encode(
SetRotorPositionMode(
SetRotorPositionMode.DISP_POS_MODE_ENCODER)))
from simple_pid import PID
pid = PID(1, 0.1, 0.05, setpoint=1)
pid.sample_time = 0.01
pid.setpoint = 10
pid.tunings = (1.0, 0.2, 0.4) #ki,Ti,Td
pid.output_limits = (0, 10)
while True:
control = pid(
9
) # The PID class implements __call__(), which means that to compute a new output value
