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 __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, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.odometry = Odometry() self.pd_controller = PDController(500, 100, -75, 75) # self.pid_controller = PIDController(500, 100, 0, -75, 75, -50, 50) self.pid_controller = PIDController(250, 30, 1, -100, 100, -100, 100)
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 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