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()