def __init__(self, state, params=None, verbose=False):
super(PoseController, self).__init__(state, verbose=verbose)
self.pid.x = pid.PIDController(params.xy)
self.pid.y = pid.PIDController(params.xy)
self.pid.z = pid.PIDController(params.z)
self.pid.yaw = pid.PIDController(params.yaw)
self.command = 'pose'
def testHeater():
epsilon = 0.05
setupExperiment()
heater.power(0)
heater.on()
pid_controller = pid.PIDController(0.09*0.1, 0.002*0.1, 0.2*0.05)
pid_controller.soft_reset()
for target_temp in range(305, 310):
print "Adjusting temp to {t}".format(t=target_temp)
pid_controller.time_reset()
measured_temp = thermometer.temperature()
while measured_temp < target_temp:
measured_temp = thermometer.temperature()
pid_output = pid_controller.update(measured_temp, target_temp)
heater.power(clamp(pid_output,0.4))
print measured_temp, clamp(pid_output, 0.4)
time.sleep(0.5)
for i in range(60):
measured_temp = thermometer.temperature()
pid_output = pid_controller.update(measured_temp, target_temp)
heater.power(clamp(pid_output,0.4))
time.sleep(0.5)
print measured_temp
avg_iters = 40
phold = 0
for i in range(avg_iters):
measured_temp = thermometer.temperature()
pid_output = pid_controller.update(measured_temp, target_temp)
heater.power(clamp(pid_output,0.4))
phold += clamp(pid_output, 0.4)
time.sleep(0.5)
print measured_temp
phold = phold/avg_iters;
heater.power(phold)
for i in range(10):
print thermometer.temperature()
time.sleep(1)
heater.off()
def __init__(self, state, params=None, verbose=False):
super(TwistController, self).__init__(state, verbose=verbose)
self.pid.linear.x = pid.PIDController(params.linear_xy)
self.pid.linear.y = pid.PIDController(params.linear_xy)
self.pid.linear.z = pid.PIDController(params.linear_z)
self.pid.angular.x = pid.PIDController(params.angular_xy)
self.pid.angular.y = pid.PIDController(params.angular_xy)
self.pid.angular.z = pid.PIDController(params.angular_z)
self.load_factor_limit = params.limits('load_factor', -1.0)
self.force_z_limit = params.limits('force_z', -1.0)
self.torque_xy_limit = params.limits('torque_xy', -1.0)
self.torque_z_limit = params.limits('torque_z', -1.0)
self.command = 'twist'
def runExperiment(start_temp, end_temp, temp_res, base_directory):
setupExperiment()
start_time = datetime.now()
inter_directory = os.path.join(base_directory, "TEMP")
if not os.path.exists(base_directory):
os.makedirs(base_directory)
if not os.path.exists(inter_directory):
os.makedirs(inter_directory)
index = 1
heater.power(0)
heater.on()
beta = 1
pid_controller = pid.PIDController(0.09*beta, 0.002*beta, 0.2*beta)
pid_controller.soft_reset()
measured_temp = thermometer.temperature()
print "Preheating..."
while abs(measured_temp - start_temp) > 1.5:
measured_temp = thermometer.temperature()
print measured_temp
pid_output = pid_controller.update(measured_temp, start_temp)
heater.power(clamp(pid_output, 0.4))
time.sleep(0.5)
print "Finished preheating..."
pid_controller.soft_reset()
for target_temp in dec_range(start_temp, end_temp, temp_res):
print "Adjusting temp to {t}".format(t=target_temp)
pid_controller.time_reset()
measured_temp = thermometer.temperature()
while measured_temp < target_temp:
measured_temp = thermometer.temperature()
print measured_temp
pid_output = pid_controller.update(measured_temp, target_temp)
heater.power(clamp(pid_output,0.4))
time.sleep(0.5)
for i in range(60):
measured_temp = thermometer.temperature()
print measured_temp
pid_output = pid_controller.update(measured_temp, target_temp)
heater.power(clamp(pid_output,0.4))
time.sleep(0.5)
avg_iters = 40
phold = 0
for i in range(avg_iters):
measured_temp = thermometer.temperature()
print measured_temp
pid_output = pid_controller.update(measured_temp, target_temp)
heater.power(clamp(pid_output,0.4))
phold += clamp(pid_output, 0.4)
time.sleep(0.5)
phold = phold/avg_iters;
print "Holding temperature for measurement."
heater.power(phold)
data.append(collectDataPoint())
record_temp_file(inter_directory, index)
print "Measurement taken."
index += 1
# record all of the data once finished
file_path = os.path.join(base_directory, "FINALDATA")
pickle.dump(data, open(file_path, 'wb+'))
end_time = datetime.now()
diff_time = end_time - start_time
print "Experimental duration: " + str(diff_time)
print data
finishExperiment()
def velocityToWheel(vx, vy, vr):
global botR, wheelRadius
return (
(1 / wheelRadius) * 4 *
np.matmul(np.linalg.pinv(getJ(radians(botR))), np.array([vx, vy, vr])))
def wheelToForce(w1, w2, w3, w4):
global botR, wheelRadius
return (wheelRadius * 0.25 *
np.matmul(getJ(radians(botR)), np.array([w1, w2, w3, w4])))
target = 700
pidC = pid.PIDController(.6, 0.00001, 6, target)
def pidLoop(window):
global botX, wheelFL, wheelFR, wheelBR, wheelBL
pidC.updateError(botY)
print(pidC.error)
halp = pidC.correction()
wheels = velocityToWheel(halp, 0, 0)
wheelFL = wheels[0]
wheelFR = wheels[1]
wheelBL = wheels[2]
wheelBR = wheels[3]
def normPID():
connection.listen(1)
conn, addr = connection.accept()
keep_going = True
#turn_p = 0.5
#turn_d = 0.01
#turn_i = 0.005
#turn_s = 0.005
#maxSpeed = 75
turn_p = 0.5
turn_d = 0.01
turn_i = 0.01
turn_s = 0.005
maxSpeed = 50
turnControl = pid.PIDController(turn_p, turn_i, turn_d, turn_s)
speedControl = pid.PIDController(0, 0, 0, turn_s)
robot = Robot.Robot()
move = True
i = 0
max_tries = 5
current_try = 1
prev_speed = maxSpeed
prev_steer = 0
def move_robot(angle_diff, framerate):
global prev_speed, maxSpeed
print("move robot", angle_diff, framerate)
MAX_ABS_SPEED = 255.0
def __init__(self, graph_index=None, graph_time=None, graph_position=None, graph_error=None, graph_fan=None, graph_target=None):
self.graph_index, self.graph_time, self.graph_position, self.graph_error, self.graph_fan, self.graph_target = graph_index, graph_time, graph_position, graph_error, graph_fan, graph_target
self.run_pid = True
# world settings
self.gravity = 9.81
self.elasticity = 0.9
self.mass_density_of_air = 1.225 # kg/m^3
# https://met213.tech.purdue.edu/French/Example%20Problems/Ping%20Pong%20Ball%20Drop%20-%20Published%20Article.pdf
# http://aero-comlab.stanford.edu/Papers/AIAA-2011-3668-697.pdf
self.drag_coefficient = 0.65
# ball settings
self.ball_radius = 0.02 # 20 mm radius
self.ball_mass = 0.01 #0.0027 # 2.7 grams for ping pong ball
# graphing settings
self.t_series = []
self.y_series = []
self.v_series = []
self.fan_series = []
self.target_series = []
self.real_pos = []
self.real_error = []
# pygame settings
self.target_fps = 60.0
self.dt = 1.0 / self.target_fps
self.world_scale = 600.0 # pixels/meter
self.sim_win_height = 600
self.sim_win_width = 337
self.win_height = 600
self.win_width = 680
self.world_width = float(self.sim_win_width) / self.world_scale
self.world_height = float(self.sim_win_height) / self.world_scale
#initialize pygame
# self.screen = pygame.display.set_mode((self.sim_win_width, self.sim_win_height))
self.screen = pygame.display.set_mode((self.win_width, self.win_height))
pygame.display.set_caption('Ping-Pong Ball Simualator')
pygame.font.init()
font = pygame.font.Font('resources/COMIC.TTF', 25)
textsurface = font.render('Loading, please wait...', False, (255, 0, 0))
self.screen.blit(textsurface, (5, self.sim_win_height / 2))
pygame.display.flip()
# add simulation objects
self.start_height = self.ball_radius + 0.0005
self.ball = sim_obj.Ball(self.ball_radius, self.ball_mass, self.world_width / 2, self.start_height, self) # position in meters
self.fan = sim_obj.Fan(self)
pid_target_pos = 0.0
self.pid = pid.PIDController(pid_target_pos)
# add interface objects
self.slider_target = VSlider(self.screen, 'Target', 0.0, 0.0, 1.0, (360, 25), 550)
self.slider_kp = VSlider(self.screen, 'Kp', self.pid.Kp, 0, 10000, (425, 25), 550)
self.slider_ki = VSlider(self.screen, 'Ki', self.pid.Ki, 0, 10000, (490, 25), 550)
self.slider_kd = VSlider(self.screen, 'Kd', self.pid.Kd, 0, 10000, (555, 25), 550)
self.slider_bias = VSlider(self.screen, 'Bias', self.pid.bias, 0.0, 3000, (620, 25), 550)
self.slides = [self.slider_target, self.slider_kp, self.slider_ki, self.slider_kd, self.slider_bias]
return