def update_heading_profile(self):
heading = self.imu.getAngle()
# this ensures we always rotate the same way going from the switch to
# the scale, but that we do not do 360s while maniuplating cubes at the
# switch
delta = constrain_angle(self.end_heading - heading)
end_heading = heading + delta
self.heading_function, self.heading_traj_tm = generate_trapezoidal_function(
heading, 0, end_heading, 0, v_max=3, a_pos=2, a_neg=2)
self.last_heading_error = 0
self.heading_error_i = 0
def increment_segment(self):
if self.segment_idx is None:
self.segment_idx = 0
else:
self.segment_idx += 1
self.segment = (self.waypoints_xy[self.segment_idx+1]
- self.waypoints_xy[self.segment_idx])
start_speed = self.waypoints[self.segment_idx][3]
end_speed = self.waypoints[self.segment_idx+1][3]
seg_length = np.linalg.norm(self.segment)
self.speed_function = generate_trapezoidal_function(
0, start_speed, seg_length, end_speed,
self.top_speed, self.top_accel, self.top_decel)
def update_linear_profile(self, motion_params, start_speed, end_speed):
v_max, a_pos, a_neg = motion_params
self.speed_function, self.distance_traj_tm = generate_trapezoidal_function(
0,
start_speed,
self.end_distance,
end_speed,
v_max=v_max,
a_pos=a_pos,
a_neg=a_neg)
self.linear_position = 0
self.last_position = self.chassis.position
print(f'start_position {self.last_position}')
self.last_linear_error = 0
self.linear_error_i = 0
def increment_segment(self, segment=None, start_speed=None):
if self.segment_idx is None:
self.segment_idx = 0
elif segment is not None:
if segment == self.segment_idx:
return
self.segment_idx = segment
else:
self.segment_idx += 1
self.segment = (self.waypoints_xy[self.segment_idx+1]
- self.waypoints_xy[self.segment_idx])
if start_speed is None:
start_speed = self.waypoints[self.segment_idx][3]
end_speed = self.waypoints[self.segment_idx+1][3]
seg_length = np.linalg.norm(self.segment)
print("NEW SEGMENT Start speed {} top_speed {} end_speed {} length {}".format(start_speed, self.top_speed, end_speed, seg_length))
self.speed_function, end_tm = generate_trapezoidal_function(
0, start_speed, seg_length, end_speed,
self.top_speed, self.top_accel, self.top_decel, time_mode=False)