The propulsion component takes request for navigation and translates them to thruster movements. It first reads the umpteen navigational control parameters from the configuration settings, then begins its own thread of execution to handle requests. Requests are made from behaviors running in other threads, so all access is synchronized. Requests are almost always in the form of a target waypoint, consisting of a position and heading. The propulsion thread continuously performs the following calculations: It checks the current mode of propulsion. If there is no request, it idles. If there is a waypoint request, it calculates the distance from the current position and heading to the target. It then finds the instantaneous linear and angular that brings the vehicle's configuration closer to the target. These accelerations are converted to force and torque based on the mass and moment of inertia of the vehicle, as well as its current linear and angular velocity. Next, the propulsion component converts the force and torque to desired thrust forces from each of the four thrusters. This is a linear mapping, because the force and torque equations are linear and there are four inputs (three one-dimensional forces and one torque) and four unknowns (the thrust delivered by each of the four thrusters). The delivered thrusts are scaled according to input parameters that determine how depth, position, and heading progress are weighted against each other. The thrusts are scaled and clamped to within the range possible given the thrusters, and the resulting forces are converted to control voltages using a cubic model of the thrust-to-control-voltage relationship derived for a single thruster. These control voltages are sent to the thrusters through the DAC analog outputs. The propulsion system can deliver arbitrary direction forces and torques independently of each other -- the robot can move in any direction while facing any direction.
The conversion algorithm described above changes according to input parameters. For instance,the
short_distance parameter determines the distance between current state and target waypoint inside which the vehicle's acceleration will be proportional to the distance. Thus with
short_distance equal to one meter, the vehicle will not attempt to slow down until it is within one meter of its waypoint. Furthermore, the vehicle will not attempt to assume its target heading until it is within
short_distance of the target, instead turning towards its target for most efficient travel up to that proximity. Variables such as
kp_heading change how the propulsion system weights differences in depth and heading (between current state and target state) over differences in position. A high
kp_depth results in navigation that sacrifices linear velocity for correct direction.