def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.p_controller = PController(k_p=500.0)
self.pd_controller = PDController(k_p=500.0, k_d=50.0)
def __init__(self, filename):
# check if we are running in screen. If not, terminate
# this command will fail if the program is executed outside of screen
try:
subprocess.check_output(['pgrep', '-f', 'screen'])
except subprocess.CalledProcessError:
raise RuntimeError(
"Program is not running in screen. Aborting. Please restart in screen."
)
# initialize P controller
self.p_ctrl = PController(filename)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
# define the gains here
self.kp = 1000
self.kd = 3
self.minOutput = -200
self.maxOutput = 200
# instantiate your controllers here
self.p_controller = PController(self.kp, self.minOutput,
self.maxOutput)
self.pd_controller = PDController(self.kp, self.kd, self.minOutput,
self.maxOutput)
class Run:
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
# define the gains here
self.kp = 1000
self.kd = 3
self.minOutput = -200
self.maxOutput = 200
# instantiate your controllers here
self.p_controller = PController(self.kp, self.minOutput,
self.maxOutput)
self.pd_controller = PDController(self.kp, self.kd, self.minOutput,
self.maxOutput)
def run(self):
self.create.start()
self.create.safe()
self.servo.go_to(70)
self.time.sleep(2)
goal_distance = 0.5
base_speed = 100
adjust = 1
self.create.drive_direct(base_speed, base_speed)
while True:
distance = self.sonar.get_distance()
if distance is not None:
# print(distance)
# update controllers and move robot here
# ...
u = self.p_controller.update(distance, goal_distance)
u = self.pd_controller.update(distance, goal_distance,
self.time.time())
print(u)
v_right = base_speed - u * adjust
v_left = base_speed + u * adjust
self.create.drive_direct(int(v_right), int(v_left))
self.time.sleep(0.01)
class Kiln:
# initializes with the filename of the temperature profile csv
def __init__(self, filename):
# check if we are running in screen. If not, terminate
# this command will fail if the program is executed outside of screen
try:
subprocess.check_output(['pgrep', '-f', 'screen'])
except subprocess.CalledProcessError:
raise RuntimeError(
"Program is not running in screen. Aborting. Please restart in screen."
)
# initialize P controller
self.p_ctrl = PController(filename)
def run(self):
# initialize shutdown signal receivers
signal(SIGINT, self.shutdown)
signal(SIGTERM, self.shutdown)
try:
self.is_running = True
while self.is_running:
# update p controller with target temperature
self.p_ctrl.update(0.0)
time.sleep(1.0)
self.shutdown("finished")
except:
if sys.exc_info()[0] != SystemExit:
print("exception caught:")
sys.excepthook(sys.exc_info()[0],
sys.exc_info()[1],
sys.exc_info()[2])
self.shutdown("exception caught")
def shutdown(self, signal_received=None, frame=None):
if signal_received == 2:
signal_received = "ctrl+C"
elif signal_received == 15:
signal_received = "SIGTERM"
print("\n\n\nshutdown signal received: " + str(signal_received))
# turn off the relay
self.p_ctrl.relay.turn_off()
print("relay off")
# write any logged data
self.p_ctrl.logger.write()
# un-initialize GPIOs
GPIO.cleanup()
sys.exit(0)
class Run:
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
# define the gains here
# self.kp = 100
self.kp = 1000
self.kd = 20
self.minOutput = -250
self.maxOutput = 250
# instantiate your controllers here
self.p_controller = PController(self.kp, self.minOutput,
self.maxOutput)
self.pd_controller = PDController(self.kp, self.kd, self.minOutput,
self.maxOutput)
self.previous_time_l = None
self.previous_error_l = None
self.previous_time_r = None
self.previous_error_r = None
def run(self):
self.create.start()
self.create.safe()
self.servo.go_to(70)
self.time.sleep(2)
goal_distance = 0.5
while True:
distance = self.sonar.get_distance()
if distance is not None:
# update controllers and move robot here
# ...
differential_controller = True
error = goal_distance - distance
velocity_left_wheel = self.f_left(differential_controller,
self.time.time(), error)
velocity_right_wheel = self.f_right(differential_controller,
self.time.time(), error)
print("[curr_state = %f, error = %f, right: %f, left: %f]\n" %
(distance, goal_distance - distance,
velocity_right_wheel, velocity_left_wheel))
self.create.drive_direct(int(velocity_right_wheel),
int(velocity_left_wheel))
self.time.sleep(0.1)
def f_left(self, differential_controller, current_time,
current_error) -> float:
if differential_controller:
delta_power = self.pd_controller.update(current_error, current_time, \
self.previous_error_l, self.previous_time_l)
else:
delta_power = self.p_controller.update(current_error)
self.previous_time_l = current_time
self.previous_error_l = current_error
power = BASE_SPEED + delta_power
return power
def f_right(self, differential_controller, current_time,
current_error) -> float:
if differential_controller:
delta_power = self.pd_controller.update(current_error, current_time, \
self.previous_error_r, self.previous_time_r)
else:
delta_power = self.p_controller.update(current_error)
self.previous_time_r = current_time
self.previous_error_r = current_error
power = BASE_SPEED - delta_power
return power
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.p_controller = PController(k_p=500.0)
self.pd_controller = PDController(k_p=500.0, k_d=50.0)
def run(self):
def f_left(error, speed, curr_time_) -> float:
ctrl = self.p_controller.update(error)
# ctrl = self.pd_controller.update(
# error,
# error - self.pd_controller.prev_l_error,
# curr_time_ - self.pd_controller.prev_l_time
# )
print("left ctrl: %f" % ctrl)
self.pd_controller.prev_l_error = error
self.pd_controller.prev_l_time = curr_time_
return clamp(speed + ctrl, self.p_controller.range_min,
self.p_controller.range_max)
def f_right(error, speed, curr_time_) -> float:
ctrl = self.p_controller.update(error)
# ctrl = self.pd_controller.update(
# error,
# error - self.pd_controller.prev_r_error,
# curr_time_ - self.pd_controller.prev_r_time
# )
# print("right ctrl: %f" % ctrl)
self.pd_controller.prev_r_error = error
self.pd_controller.prev_r_time = curr_time_
return clamp(speed - ctrl, self.p_controller.range_min,
self.p_controller.range_max)
self.create.start()
self.create.safe()
self.servo.go_to(50)
self.time.sleep(1)
start_time = self.time.time()
curr_time = start_time
goal = 0.5
print(goal)
vl = 100.0
vr = 100.0
plt_distance = ()
start_time = self.time.time()
curr_time = start_time
# prev_time = curr_time
while curr_time - start_time < 100:
curr_state = self.sonar.get_distance()
self.time.sleep(0.1)
vleft = f_left(goal - curr_state, vl, curr_time)
vright = f_right(goal - curr_state, vr, curr_time)
print("[curr_state = %f, error = %f\nleft: %f, right: %f]\n" %
(curr_state, goal - curr_state, vleft, vright))
# self.create.drive_direct(
# f_left(goal - curr_state, vl),
# f_right(goal - curr_state, vr)
# )
# self.create.drive_direct(vleft, vright)
self.create.drive_direct(
right_wheel_velocity_in_mm_per_sec=int(vright),
left_wheel_velocity_in_mm_per_sec=int(vleft))
# prev_time = curr_time
curr_time = self.time.time()