Motor Control Task

Summary

This task implements the inner wheel-speed control loop for one motor. It reads the encoder, computes wheel speed, applies PI control, commands the motor driver, and optionally logs velocity data.

Important class

task_motor(mot, enc, goFlag, k_p, k_i, setpoint, dataValues, timeValues, pos_share=None, vel_share=None, invert_effort=False)

Generator-based motor-control task for one wheel.

Important methods

run()

Main task loop. It waits for an enable flag, zeros the encoder, enables the motor, computes measured speed, applies PI control, and sends effort to the driver.

_mps_to_radps(speed_mps)

Converts linear wheel speed in meters per second to angular speed.

_counts_us_to_radps(count_per_us)

Converts encoder count rate into angular speed.

_write_telemetry(meas_radps)

Publishes velocity and position telemetry through shares.

Important state constants

S0_INIT

Initialization state.

S1_WAIT

Idle state waiting for a nonzero go flag.

S2_RUN

Active closed-loop control state.

Important variables

self._mot

Motor-driver object used to command wheel effort.

self._enc

Encoder object used to measure wheel motion.

self._goFlag

Mode share controlling task behavior.

self._k_p / self._k_i

PI gains for the inner speed loop.

self._setpoint

Wheel speed command share in meters per second.

self._dataValues / self._timeValues

Queues used to log measured wheel speed.

self._pos_share / self._vel_share

Optional shares used to publish wheel position and wheel velocity.

self._invert_effort

Optional sign inversion applied to the computed duty cycle.

self._integral

Integral state of the PI controller.

self._ref_vel

Most recent angular velocity setpoint.

self._battery_volts

Battery voltage used to convert requested volts into PWM duty cycle.

self._wheel_radius_m

Wheel radius used for unit conversions.

self._counts_per_wheel_rev

Encoder counts per wheel revolution.

self._two_pi

Cached constant used in the conversion formulas.

Go-flag meaning

0

Stop. The motor is disabled and the task returns to the wait state.

1

Run closed-loop control and log data until the logging queues fill, then stop automatically.

2

Run continuous closed-loop control without auto-stop logging behavior.

Control behavior

The task converts the setpoint from meters per second to radians per second, measures actual wheel speed from the encoder, and computes:

  • error = reference - measured

  • integral += error * dt

  • effort_volts = Kp * error + Ki * integral

The voltage command is converted into a duty cycle using the battery voltage and clamped to the range -100 to 100 percent before being sent to the motor driver.