def __init__(
self,
dbw_parameters, # Set of base parameters
):
self.is_initialized = False
self.last_time = rospy.get_time()
self.dbw_parameters = dbw_parameters
self.steering_controller = YawController(
wheel_base = self.dbw_parameters['wheel_base'],
steer_ratio = self.dbw_parameters['steer_ratio'],
min_speed = self.dbw_parameters['min_vehicle_speed'],
max_lat_accel = self.dbw_parameters['max_lat_accel'],
max_steer_angle = self.dbw_parameters['max_steer_angle'])
self.low_pass_filter_vel = LowPassFilter(samples_num = LOW_PASS_FREQ_VEL * self.dbw_parameters['dbw_rate'])
self.low_pass_filter_yaw = LowPassFilter(samples_num = LOW_PASS_FREQ_YAW * self.dbw_parameters['dbw_rate'])
self.throttle_controller = PIDController(
kp = 0.3,
ki = 0.01,
kd = 0.01,
mn = self.dbw_parameters['vehicle_min_throttle_val'],
mx = self.dbw_parameters['vehicle_max_throttle_val'],
integral_period_sec = INTEGRAL_PERIOD_SEC)
self.steering_controller2 = PIDController(
kp = 3.0,
ki = 0.0,
kd = 1.0,
mn = -self.dbw_parameters['max_steer_angle'],
mx = self.dbw_parameters['max_steer_angle'],
integral_period_sec = INTEGRAL_PERIOD_SEC)
def main():
elbowPid = PIDController(10., 1, 1)
shoulderPid = PIDController(1., .001, .1)
lastTime = time.time()
print "actual, delinearizedSetPoint, afterPID, target"
while True:
reading = readAin(AIN3, ELBOW_RANGE)
log.debug("Read: %s" % reading)
pistonPressure = getElbowPressureFromGravity(reading)
now = time.time()
dt = now - lastTime
elbowRate = 0
# shoulderControl = 0
if sinWave:
elbowRate = elbowPid.update(math.sin(now/2.)/2.+.5, reading, dt)
# shoulderControl = shoulderPid.update(math.sin(now/3.)/8.+.85, reading, dt)
else:
elbowRate = elbowPid.update(elbowSetPoint, reading, dt)
# shoulderControl = shoulderPid.update(shoulderSetPoint, reading, dt)
relevantPressure = pistonPressure[1 if elbowRate > 0 else 0]
kv = getValveCommandFromControlSignal(elbowRate, relevantPressure)
print "%s, %s, %s, %s" % (reading, kv, elbowRate, math.sin(now/2.)/2.)
elbowPair = mapControlToElbowPwmPair(kv)
# shoulderPair = mapControlToShoulderPwmPair(shoulderControl)
executeElbowPwmPair(elbowPair)
# executeShoulderPwmPair(shoulderPair)
lastTime = now
time.sleep(.01) # because the valve response rate is 10Hz
def main():
elbowPid = PIDController(10., 1, 1)
shoulderPid = PIDController(1., .001, .1)
lastTime = time.time()
print "actual, delinearizedSetPoint, afterPID, target"
while True:
reading = readAin(AIN3, ELBOW_RANGE)
log.debug("Read: %s" % reading)
pistonPressure = getElbowPressureFromGravity(reading)
now = time.time()
dt = now - lastTime
elbowRate = 0
# shoulderControl = 0
if sinWave:
elbowRate = elbowPid.update(
math.sin(now / 2.) / 2. + .5, reading, dt)
# shoulderControl = shoulderPid.update(math.sin(now/3.)/8.+.85, reading, dt)
else:
elbowRate = elbowPid.update(elbowSetPoint, reading, dt)
# shoulderControl = shoulderPid.update(shoulderSetPoint, reading, dt)
relevantPressure = pistonPressure[1 if elbowRate > 0 else 0]
kv = getValveCommandFromControlSignal(elbowRate, relevantPressure)
print "%s, %s, %s, %s" % (reading, kv, elbowRate,
math.sin(now / 2.) / 2.)
elbowPair = mapControlToElbowPwmPair(kv)
# shoulderPair = mapControlToShoulderPwmPair(shoulderControl)
executeElbowPwmPair(elbowPair)
# executeShoulderPwmPair(shoulderPair)
lastTime = now
time.sleep(.01) # because the valve response rate is 10Hz
def pid_process(output, p, i, d, box_coord, origin_coord, action):
signal.signal(signal.SIGINT, signal_handler)
p = PIDController(p.value, i.value, d.value)
p.reset()
while True:
error = origin_coord - box_coord.value
output.value = p.update(error)
def pid_process(output, p, i, d, box_coord, origin_coord, action):
# signal trap to handle keyboard interrupt
signal.signal(signal.SIGINT, signal_handler)
# create a PID and initialize it
pid = PIDController(p.value, i.value, d.value)
pid.reset()
# loop indefinitely
while True:
error = origin_coord - box_coord.value
output.value = pid.update(error)
logging.info(f'{action} error {error} angle: {output.value}')
def test_P_positive(self):
kp = 0.01
ki = 0
kd = 0
state = 0
minOutput = 0.0
maxOutput = 5.0
pid = PIDController(kp, ki, kd, state, minOutput, maxOutput)
currentState = 4900
targetState = 5000
timeDelta = 0.134 # timeDelta is irrelevant for P
output = pid.compute(currentState, targetState, timeDelta)
self.assertEqual(output, 1.0)
def test_P_zero(self):
kp = 1
ki = 0
kd = 0
state = 0
minOutput = -10
maxOutput = 10
pid = PIDController(kp, ki, kd, state, minOutput, maxOutput)
currentState = 0.0
targetState = 0.0
timeDelta = 1 # timeDelta is irrelevant for P
output = pid.compute(currentState, targetState, timeDelta)
self.assertTrue(output == 0)
def test_P_negative(self):
kp = 1
ki = 0
kd = 0
state = 0
minOutput = -10
maxOutput = 10
pid = PIDController(kp, ki, kd, state, minOutput, maxOutput)
currentState = 0.1
targetState = 0
timeDelta = 10009 # timeDelta is irrelevant for P
output = pid.compute(currentState, targetState, timeDelta)
self.assertEqual(output, -0.1)
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image,
self.image_callback)
self.depth_sub = rospy.Subscriber("/camera/depth/image", Image,
self.depth_callback)
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi',
Twist,
queue_size=1)
self.app = App(0)
self.image_updated = False
self.depth_updated = False
self.image = None
self.depth = None
self.control = PIDController(Kp=0.1, Ki=0, Kd=0)
def test_D_zero(self):
kp = 0
ki = 0
kd = 1
state = 1
minOutput = 0.0
maxOutput = 500.0
pid = PIDController(kp, ki, kd, state, minOutput, maxOutput)
currentState = 1
targetState = 13 # a change in targetState should not influence derivative
timeDelta = 1
output = pid.compute(currentState, targetState, timeDelta)
self.assertEqual(output, 0.0)
def test_D_one(self):
kp = 0
ki = 0
kd = 1
state = 1
minOutput = -500.0
maxOutput = 500.0
pid = PIDController(kp, ki, kd, state, minOutput, maxOutput)
currentState = 2
targetState = 1
timeDelta = 1
output = pid.compute(currentState, targetState, timeDelta)
self.assertEqual(output, -1)
def __init__(self):
self.pids = {}
# self.pids['aileron'] = PIDController(
# 'aileron', p=0.8, i=0.01, d=0.30, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0))
# self.pids['pitch'] = PIDController(
# 'pitch', p=0.6, i=0.01, d=0.40, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0))
# self.pids['throttle'] = PIDController(
# 'throttle', p=1.6, i=0.30, d=0.80, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0))
# self.pids['yaw'] = PIDController(
# 'yaw', p=0.5, i=0.01, d=0.20, output_limits=(-1.0, 1.0), input_limits=(-1.0, 1.0))
self.pids['aileron'] = PIDController('aileron',
p=0.8,
i=0.01,
d=0.30,
output_limits=(-1.0, 1.0),
input_limits=(-1.0, 1.0))
self.pids['pitch'] = PIDController('pitch',
p=0.6,
i=0.01,
d=0.40,
output_limits=(-1.0, 1.0),
input_limits=(-1.0, 1.0))
self.pids['throttle'] = PIDController('throttle',
p=1.6,
i=0.30,
d=0.80,
output_limits=(-1.0, 1.0),
input_limits=(-1.0, 1.0))
self.pids['yaw'] = PIDController('yaw',
p=0.5,
i=0.01,
d=0.20,
output_limits=(-1.0, 1.0),
input_limits=(-1.0, 1.0))
self.setpoints = {
'aileron': 0.0,
'pitch': 0.0,
'throttle': 0.0,
'yaw': 0.0
}
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image, self.image_callback)
self.depth_sub = rospy.Subscriber("/camera/depth/image", Image, self.depth_callback)
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=1)
self.app = App(0)
self.image_updated = False
self.depth_updated = False
self.image = None
self.depth = None
self.control = PIDController(Kp=0.1, Ki=0, Kd=0)
def test_I_count(self):
kp = 0
ki = 1
kd = 0
state = 1
minOutput = -500.0
maxOutput = 500.0
pid = PIDController(kp, ki, kd, state, minOutput, maxOutput)
currentState = 2
targetState = 1
timeDelta = 0.5
output = pid.compute(currentState, targetState, timeDelta)
self.assertEqual(output, -0.5)
currentState = 3
targetState = 2.5
timeDelta = 0.5
output = pid.compute(currentState, targetState, timeDelta)
self.assertEqual(output, -0.75)
currentState = 300
targetState = 250
timeDelta = 2
output = pid.compute(currentState, targetState, timeDelta)
self.assertEqual(output, -100.75)
currentState = 400
targetState = 0
timeDelta = 2
output = pid.compute(currentState, targetState, timeDelta)
self.assertEqual(output, -500)
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image,
self.image_callback)
self.depth_sub = rospy.Subscriber("/camera/depth/image", Image,
self.depth_callback)
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi',
Twist,
queue_size=1)
self.app = App(0)
self.image_updated = False
self.depth_updated = False
self.image = None
self.depth = None
self.control = PIDController(Kp=0.1, Ki=0, Kd=0)
def image_callback(self, data):
self.image = self.bridge.imgmsg_to_cv2(data, "bgr8")
self.image_updated = True
self.callback_uniform()
def depth_callback(self, data):
self.depth = self.bridge.imgmsg_to_cv2(data, "16UC1")
self.depth_updated = True
def callback_uniform(self):
if self.image_updated and self.depth_updated:
cv_image = self.image
(rows, cols, channels) = cv_image.shape
ret = self.app.run(cv_image)
self.image_updated = False
self.depth_updated = False
if ret is not None:
y, x = ret
# print y, cols
dist = self.depth[int(x), int(y)][0]
turn_cmd = Twist()
turn_cmd.linear.x = 0.1 if dist > 0 else 0
diff = y - cols / 2
output = self.control.output(-diff)
print(output)
turn_cmd.angular.z = radians(output)
self.cmd_vel.publish(turn_cmd)
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image, self.image_callback)
self.depth_sub = rospy.Subscriber("/camera/depth/image", Image, self.depth_callback)
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=1)
self.app = App(0)
self.image_updated = False
self.depth_updated = False
self.image = None
self.depth = None
self.control = PIDController(Kp=0.1, Ki=0, Kd=0)
def image_callback(self, data):
self.image = self.bridge.imgmsg_to_cv2(data, "bgr8")
self.image_updated = True
self.callback_uniform()
def depth_callback(self, data):
self.depth = self.bridge.imgmsg_to_cv2(data, "16UC1")
self.depth_updated = True
def callback_uniform(self):
if self.image_updated and self.depth_updated:
cv_image = self.image
(rows, cols, channels) = cv_image.shape
ret = self.app.run(cv_image)
self.image_updated = False
self.depth_updated = False
if ret is not None:
y, x = ret
# print y, cols
dist = self.depth[int(x), int(y)][0]
turn_cmd = Twist()
turn_cmd.linear.x = 0.1 if dist > 0 else 0
diff = y - cols / 2
output = self.control.output(-diff)
print(output)
turn_cmd.angular.z = radians(output)
self.cmd_vel.publish(turn_cmd)
class Controller(object):
def __init__(
self,
dbw_parameters, # Set of base parameters
):
self.is_initialized = False
self.last_time = rospy.get_time()
self.dbw_parameters = dbw_parameters
self.steering_controller = YawController(
wheel_base = self.dbw_parameters['wheel_base'],
steer_ratio = self.dbw_parameters['steer_ratio'],
min_speed = self.dbw_parameters['min_vehicle_speed'],
max_lat_accel = self.dbw_parameters['max_lat_accel'],
max_steer_angle = self.dbw_parameters['max_steer_angle'])
self.low_pass_filter_vel = LowPassFilter(samples_num = LOW_PASS_FREQ_VEL * self.dbw_parameters['dbw_rate'])
self.low_pass_filter_yaw = LowPassFilter(samples_num = LOW_PASS_FREQ_YAW * self.dbw_parameters['dbw_rate'])
self.throttle_controller = PIDController(
kp = 0.3,
ki = 0.01,
kd = 0.01,
mn = self.dbw_parameters['vehicle_min_throttle_val'],
mx = self.dbw_parameters['vehicle_max_throttle_val'],
integral_period_sec = INTEGRAL_PERIOD_SEC)
self.steering_controller2 = PIDController(
kp = 3.0,
ki = 0.0,
kd = 1.0,
mn = -self.dbw_parameters['max_steer_angle'],
mx = self.dbw_parameters['max_steer_angle'],
integral_period_sec = INTEGRAL_PERIOD_SEC)
def control(
self,
target_dx,
target_dyaw,
current_dx,
current_dyaw,
dbw_status = True):
throttle = 0
brake = 0
steering = 0
correct_data = False
cur_time = rospy.get_time()
current_dx_smooth = self.low_pass_filter_vel.filt(current_dx)
target_dyaw_smooth = self.low_pass_filter_yaw.filt(target_dyaw)
if not dbw_status:
self.is_initialized = False
else:
if not self.is_initialized:
self.reset()
self.is_initialized = True
dv = target_dx - current_dx_smooth
dt = cur_time - self.last_time
if dt >= MIN_TIME_DELTA:
steering = self.steering_controller.get_steering(target_dx, target_dyaw_smooth, current_dx_smooth) + self.steering_controller2.step(target_dyaw_smooth, dt)
steering = max(min(steering, self.dbw_parameters['max_steer_angle']), -self.dbw_parameters['max_steer_angle'])
throttle = self.throttle_controller.step(dv, dt)
if target_dx <= current_dx_smooth and current_dx_smooth <= self.dbw_parameters['min_vehicle_speed']:
throttle = 0.0
brake = self.dbw_parameters['min_vehicle_break_torque']
elif throttle <= self.dbw_parameters['vehicle_throttle_dead_zone']:
deceleration = abs(self.dbw_parameters['decel_limit'] * (throttle - self.dbw_parameters['vehicle_throttle_dead_zone']) / (self.dbw_parameters['vehicle_throttle_dead_zone'] - self.dbw_parameters['vehicle_min_throttle_val']))
throttle = 0.0
brake = self.dbw_parameters['vehicle_mass'] * self.dbw_parameters['wheel_radius'] * deceleration
correct_data = True
self.last_time = cur_time
return throttle, brake, steering, correct_data
def reset(self):
self.steering_controller.reset()
self.steering_controller2.reset()
self.throttle_controller.reset()
class ApogeeSimulator(KFSimulator):
brake_angles = [0.] # Servo angle values for drag brakes (rads)
def __init__(self, engine):
super(ApogeeSimulator, self).__init__()
self._eng = engine
self.pid = PIDController(TARGET_APOGEE, KP, KI, KD)
@property
def brake_angle(self):
return self.brake_angles[-1]
@brake_angle.setter
def brake_angle(self, angle):
self.brake_angles.append(angle)
def _get_drag_factor(self, brake_angle):
"""Map from drag brake angle to drag factor
brake_angle: (rad)
returns: velocity (m/s)
"""
return DRAG_FACTOR * (1 + DRAG_GAIN * np.sin(brake_angle)**2)
def _estimate_peak_altitude(self, alt, velocity, brake_angle):
term_vel_sqrd = (MASS *
-GRAVITY[1]) / self._get_drag_factor(brake_angle)
return alt[1] + (term_vel_sqrd / (2 * -GRAVITY[1]) * np.log(
(velocity[1]**2 + term_vel_sqrd) / term_vel_sqrd))
def _actuate(self, commanded_brake_angle):
"""Takes a slew rate for the drag brakes, clamps it,
and uses it to compute the new brake angle.
"""
commanded_brake_rate = commanded_brake_angle - self.brake_angle
slew_rate = commanded_brake_rate / CMD_PERIOD
clamped_slew_rate = np.clip(slew_rate, -MAX_SERVO_SLEW_RATE,
MAX_SERVO_SLEW_RATE)
return np.clip((self.brake_angle + (clamped_slew_rate * CMD_PERIOD)),
0, (np.pi / 2))
def _accelerometer_model(self, acceleration):
"""
Accelerometer sensor model
"""
accel = acceleration - GRAVITY
accel_vector = np.cos(np.arctan2(accel[0], accel[1]) -
self.pitch) * np.linalg.norm(accel)
return np.random.normal(accel_vector, ACCEL_STD)
def _altimeter_model(self, altitude):
"""
Barometric altimeter sensor model
"""
return np.random.normal(altitude[1], BARO_STD)
def simulate(self):
while not self.terminated:
# Runs PID controller and returns commanded drag brake angle.
# Actuate drag brakes if rocket is coasting
est_apogee = self._estimate_peak_altitude(
np.array([0, self.kf.x[0]]), np.array([0, self.kf.x[1]]),
self.brake_angle)
# Get the attempted servo command from the PID controller.
# This may not end up being the actual servo angle, because
# the servo can only move so fast
brake_angle = self._actuate(self.brake_angle +
self.pid.update(self.time, est_apogee))
# TODO rewrite this portion; slight messy.
sim_time_end = self.time + CMD_PERIOD - SIM_TIME_INC / 2
first = True
while self.time < sim_time_end:
self.brake_angle = brake_angle
self.tick()
self.kf.predict()
if first:
# Barometer sensor inline
self.kf.update(
np.array([
self._altimeter_model(self.altitude),
self._accelerometer_model(self.acceleration) +
GRAVITY[1]
]))
else:
self.kf.update2(
np.array([
self._accelerometer_model(self.acceleration) +
GRAVITY[1]
]))
first = False
# Appends values to lists
self.estimated_altitude = self.kf.x[0]
self.estimated_velocity = self.kf.x[1]
self.estimated_acceleration = self.kf.x[2]
# Increment the time
self.time += SIM_TIME_INC
self._print_results()
def _print_results(self):
print('')
print('========== RESULTS ==========')
print('PEAK ALTITUDE:: {} meters'.format(
max([x[1] for x in self.altitude_values])))
print('=============================')
print('')
def _calculate_forces(self):
# Add Engine thrust and attitude to forces
# Gravitational force
forces = MASS * GRAVITY
# Thrust and rotation
forces += self._eng.thrust(self.time) * np.array(
[np.sin(self.pitch), np.cos(self.pitch)])
# Air drag
forces += self._get_drag_factor(
self.brake_angle) * -self.velocity**2 * np.sign(self.velocity)
return forces
def _terminate(self):
return self.velocity[1] < 0
def plot(self, title=None):
# create PID graph
self.pid.figure('Apogee PID Controller Error')
kf_label = 'KF estimation'
plt.figure(title or 'Simulation Outcomes')
plt.subplot(4, 1, 1)
plt.plot(self.time_values, [x[1] for x in self.altitude_values],
label='Altitude')
plt.plot(self.time_values, self.kalman_altitude_values, label=kf_label)
plt.ylabel('Altitude (m)')
plt.legend()
plt.subplot(4, 1, 2)
plt.plot(self.time_values, [x[1] for x in self.velocity_values],
label='Velocity')
plt.plot(self.time_values, self.kalman_velocity_values, label=kf_label)
plt.ylabel('Velocity (m/s)')
plt.legend()
plt.subplot(4, 1, 3)
plt.plot(self.time_values, [x[1] for x in self.acceleration_values],
label='Acceleration')
plt.plot(self.time_values,
self.kalman_acceleration_values,
label=kf_label)
plt.ylabel('Acceleration (m/s^2)')
plt.legend()
plt.subplot(4, 1, 4)
plt.plot(self.time_values, [np.rad2deg(x) for x in self.brake_angles])
plt.ylabel('Servo Angle (degrees)')
plt.xlabel('Time (s)')
return plt
def __init__(self, engine):
super(ApogeeSimulator, self).__init__()
self._eng = engine
self.pid = PIDController(TARGET_APOGEE, KP, KI, KD)
from driver import RobotDriver
from encoders import EncoderController
from pid import PIDController
if __name__ == '__main__':
encoder_controller = EncoderController(
left_motor_pin_a=cfg.LEFT_MOTOR_PIN_A,
left_motor_pin_b=cfg.LEFT_MOTOR_PIN_B,
right_motor_pin_a=cfg.RIGHT_MOTOR_PIN_A,
right_motor_pin_b=cfg.RIGHT_MOTOR_PIN_B,
encoder_resolution=cfg.ENCODER_RESOLUTION)
pid_controller = PIDController(proportional_coef=cfg.PID_KP,
integral_coef=cfg.PID_KI,
derivative_coef=cfg.PID_KD,
windup_guard=cfg.PID_WINDUP_GUARD,
current_time=None)
rd = RobotDriver(wheel_diameter=cfg.WHEEL_DIAMETER_MM,
wheelbase=cfg.WHEELBASE_MM,
max_angular_velocity=cfg.MAX_ANGULAR_VELOCITY_RPM,
encoder_controller=encoder_controller,
pid_controller=pid_controller)
rd.drive_forward(0.2, 0.5)
left_velocities = rd.get_encoder_controller().get_left_encoder(
).get_velocity_records()
right_velocities = rd.get_encoder_controller().get_right_encoder(
).get_velocity_records()
import krpc
from pid import PIDController
throttle_controller = PIDController(0.2, 0.2, 0.3)
goal_speed = 200
conn = krpc.connect(name='Hello World')
vessel = conn.space_center.active_vessel
control = vessel.control
ref_frame = vessel.orbit.body.reference_frame
flight = vessel.flight(ref_frame)
while True:
output_throttle = throttle_controller.trigger(goal_speed, flight.speed, 1)
control.throttle = output_throttle
if output_throttle < -0.1:
control.brakes = True
else:
control.brakes = False
print("Speed: {}, Throttle: {}".format(flight.speed, output_throttle))
def test_pid(kp, ki, kd, stime, use_pid=True):
left_encoder = Encoder(motor_pin_a=cfg.LEFT_MOTOR_PIN_A,
motor_pin_b=cfg.LEFT_MOTOR_PIN_B,
sampling_time_s=stime)
left_encoder_counter = EncoderCounter(encoder=left_encoder)
left_encoder_counter.start()
pid_controller = PIDController(proportional_coef=kp,
integral_coef=ki,
derivative_coef=kd,
windup_guard=cfg.PID_WINDUP_GUARD,
current_time=None)
max_steps_velocity_sts = cfg.ENCODER_RESOLUTION * cfg.MAX_ANGULAR_VELOCITY_RPM / 60
kit = MotorKit(0x40)
left_motor = kit.motor1
velocity_levels = [1000, 2000, 3000, 4000, 5000, 6000, 0]
velocity_levels = [3000, 0]
sleep_time = 5
velocities_level_records = deque([])
velocities_steps_records = deque([])
pid_velocities_steps_records = deque([])
timestamps_records = deque([])
for v in velocity_levels:
pid_controller.reset()
pid_controller.set_set_point(v)
left_motor.throttle = max(min(1, v / max_steps_velocity_sts), 0)
start = time.time()
current_time = time.time()
while current_time - start < sleep_time:
timestamp, measured_steps_velocity_sts = left_encoder_counter.get_velocity(
)
# PID control
if use_pid:
new_steps_velocity_sts = pid_controller.update(
-measured_steps_velocity_sts, current_time)
left_motor.throttle = max(
min(1, new_steps_velocity_sts / max_steps_velocity_sts), 0)
else:
new_steps_velocity_sts = -1
velocities_level_records.append(v)
velocities_steps_records.append(-measured_steps_velocity_sts)
pid_velocities_steps_records.append(new_steps_velocity_sts)
timestamps_records.append(timestamp)
current_time = time.time()
left_motor.throttle = 0
left_encoder_counter.finish()
records_left = pd.DataFrame({
'velocity_steps': velocities_steps_records,
'velocity_levels': velocities_level_records,
'pid_velocity_steps': pid_velocities_steps_records,
'timestamp': timestamps_records
})
records_left['velocity_ms'] = records_left[
'velocity_steps'] * cfg.WHEEL_DIAMETER_MM * np.pi / (
1000 * cfg.ENCODER_RESOLUTION)
records_left.set_index('timestamp', drop=True)
return records_left
