class HubMotor:
def __init__(
self,
name,
wheel_radius,
gear_ratio,
poll_pairs,
can_node_id,
can_socket,
):
self.name = name
self.can_node_id = can_node_id
self.can_socket = can_socket
self.wheel_radius = wheel_radius
self.gear_ratio = gear_ratio
self.poll_pairs = poll_pairs
self.max_current = 20
self._latest_state = motor_pb2.MotorControllerState()
self._latest_stamp = Timestamp()
self.can_socket.add_reader(self._handle_can_message)
self._last_tachometer_stamp = None
self._delta_time_seconds = 0.0
self._average_delta_time = 0.0
def _handle_can_message(self, cob_id, data, stamp):
can_node_id = (cob_id & 0xff)
if can_node_id != self.can_node_id:
return
command = (cob_id >> 8) & 0xff
parser = g_vesc_msg_parsers.get(command, None)
if parser is None:
logger.warning(
'No parser for command :%x node id: %x',
command,
can_node_id,
)
return
logger.debug('can node id %02x', can_node_id)
parser(self._latest_state, data)
self._latest_stamp.CopyFrom(stamp)
if command == VESC_STATUS_MSG_5:
if self._last_tachometer_stamp is not None:
self._delta_time_seconds = (
self._latest_stamp.ToMicroseconds() -
self._last_tachometer_stamp.ToMicroseconds()) * 1e-6
self._average_delta_time = self._average_delta_time * (
0.9) + self._delta_time_seconds * 0.1
else:
self._last_tachometer_stamp = Timestamp()
self._last_tachometer_stamp.CopyFrom(self._latest_stamp)
# only log on the 5th vesc message, as we have complete state at that point.
event = make_event('%s/state' % self.name,
self._latest_state,
stamp=self._latest_stamp)
get_event_bus(self.name).send(event)
def _send_can_command(self, command, data):
cob_id = int(self.can_node_id) | (command << 8)
# print('send %x'%cob_id, '%x'%socket.CAN_EFF_FLAG)
# socket.CAN_EFF_FLAG for some reason on raspberry pi this is
# the wrong value (-0x80000000 )
eff_flag = 0x80000000
self.can_socket.send(cob_id, data, flags=eff_flag)
def _tach_to_rads(self, er):
'''compute radians from electric revs'''
rotations = er / (self.poll_pairs * self.gear_ratio)
return rotations * 2 * math.pi
def average_update_rate(self):
return 1.0 / max(self._average_delta_time, 1e-6)
def tachometer_rads(self):
return self._tach_to_rads(self._latest_state.tachometer.value)
def velocity_rads(self):
return self._tach_to_rads(self._latest_state.rpm.value) / 60.0
def send_rpm_command(self, rpm):
RPM_FORMAT = '>i' # big endian, int32
erpm = rpm * self.poll_pairs * self.gear_ratio
self._latest_state.commanded_rpm.value = int(erpm)
self._latest_state.ClearField('commanded_brake_current')
data = struct.pack(RPM_FORMAT, int(erpm))
self._send_can_command(VESC_SET_RPM, data)
def send_velocity_command(self, velocity_m_s):
rpm = 60 * velocity_m_s / (self.wheel_radius * 2 * math.pi)
self.send_rpm_command(rpm)
def send_velocity_command_rads(self, rads_second):
rpm = 60 * rads_second / (2 * math.pi)
self.send_rpm_command(rpm)
def send_current_command(self, current_amps):
CURRENT_FORMAT = '>i' # big endian, int32
data = struct.pack(
CURRENT_FORMAT,
int(
1000 * max(
min(current_amps, self.max_current),
-self.max_current,
), ),
)
self._send_can_command(VESC_SET_CURRENT, data)
def send_current_brake_command(self, current_amps):
CURRENT_BRAKE_FORMAT = '>i' # big endian, int32
self._latest_state.ClearField('commanded_rpm')
self._latest_state.commanded_brake_current.value = current_amps
data = struct.pack(
CURRENT_BRAKE_FORMAT,
int(1000 * np.clip(current_amps, 0, self.max_current), ),
)
self._send_can_command(VESC_SET_CURRENT_BRAKE, data)
def get_state(self):
return self._latest_state
def _command_loop(self, n_periods):
# n_periods is the number of periods since the last call. Should normally be 1.
now = Timestamp()
now.GetCurrentTime()
if (self.n_cycle % (5 * self.command_rate_hz)) == 0:
logger.info(
'\nright motor:\n %s\nleft motor:\n %s\n state:\n %s',
MessageToString(self.right_motor.get_state(),
as_one_line=True),
MessageToString(self.left_motor.get_state(), as_one_line=True),
MessageToString(self.tractor_state, as_one_line=True),
)
self.tractor_state.stamp.CopyFrom(now)
self.tractor_state.wheel_velocity_rads_left = self.left_motor.velocity_rads(
)
self.tractor_state.wheel_velocity_rads_right = self.right_motor.velocity_rads(
)
self.tractor_state.average_update_rate_left_motor = self.left_motor.average_update_rate(
)
self.tractor_state.average_update_rate_right_motor = self.right_motor.average_update_rate(
)
if self.config.topology == TractorConfig.TOPOLOGY_FOUR_MOTOR_SKID_STEER:
self.tractor_state.wheel_veolcity_rads_left_aft = self.left_motor_aft.velocity_rads(
)
self.tractor_state.wheel_veolcity_rads_right_aft = self.right_motor_aft.velocity_rads(
)
self.tractor_state.average_update_rate_left_aft_motor = self.left_motor_aft.average_update_rate(
)
self.tractor_state.average_update_rate_right_aft_motor = self.right_motor_aft.average_update_rate(
)
if self._last_odom_stamp is not None:
dt = (now.ToMicroseconds() -
self._last_odom_stamp.ToMicroseconds()) * 1e-6
min_dt = 0.0
max_dt = 1.0 # 1 second
if dt < min_dt or dt > max_dt:
# this condition can occur if n_periods skipped is high
# or negative if for some reason the clock is non-monotonic - TODO(ethanrublee) should we use a monotonic clock?
logger.warn(
'odometry time delta out of bounds, clipping. n_period=%d dt=%f min_dt=%f max_dt=%f',
n_periods, dt, min_dt, max_dt)
self.tractor_state.dt = np.clip(dt, min_dt, max_dt)
tractor_pose_delta = self.kinematics.compute_tractor_pose_delta(
self.tractor_state.wheel_velocity_rads_left,
self.tractor_state.wheel_velocity_rads_right,
self.tractor_state.dt,
)
self.odom_pose_tractor = self.odom_pose_tractor.dot(
tractor_pose_delta)
self.odom_poses_tractor.push(self.odom_pose_tractor, now)
self.tractor_state.abs_distance_traveled += np.linalg.norm(
tractor_pose_delta.trans)
self.tractor_state.odometry_pose_base.a_pose_b.CopyFrom(
se3_to_proto(self.odom_pose_tractor))
self.tractor_state.odometry_pose_base.frame_a = 'odometry/wheel'
self.tractor_state.odometry_pose_base.frame_b = 'tractor/base'
self.event_bus.send(
make_event(
'pose/tractor/base',
self.tractor_state.odometry_pose_base,
stamp=now,
), )
self._last_odom_stamp = now
self.n_cycle += 1
brake_current = 10.0
steering_command = self.steering.get_steering_command()
if steering_command.brake > 0.0:
self.tractor_state.commanded_brake_current = brake_current
self.tractor_state.commanded_wheel_velocity_rads_left = 0.0
self.tractor_state.commanded_wheel_velocity_rads_right = 0.0
self.tractor_state.target_unicycle_velocity = 0.0
self.tractor_state.target_unicycle_angular_velocity = 0.0
self.right_motor.send_current_brake_command(brake_current)
self.left_motor.send_current_brake_command(brake_current)
if self.config.topology == TractorConfig.TOPOLOGY_FOUR_MOTOR_SKID_STEER:
self.right_motor_aft.send_current_brake_command(brake_current)
self.left_motor_aft.send_current_brake_command(brake_current)
self.move_to_goal_controller.reset()
elif steering_command.mode in (SteeringCommand.MODE_SERVO, ):
self._servo()
elif steering_command.mode in (
SteeringCommand.MODE_JOYSTICK_MANUAL,
SteeringCommand.MODE_JOYSTICK_CRUISE_CONTROL):
self._command_velocity(steering_command.velocity,
steering_command.angular_velocity)
self.event_bus.send(make_event('tractor_state', self.tractor_state))