ESP32 ‐ WROOM

Unpacking Communication

This section focuses on receiving and processing commands over the I2C interface. The commands are received as strings, parsed, and converted into structured data for further processing.

Each command follows the structure:

<NUMBER> <NUMBER> <NUMBER>\n

• <NUMBER>: An integer (can be negative).
• Spaces: Separators between numbers.
• \n: Newline character marking the end of a command.

Where everything is processed inside the main:

Data Reception:

The i2c_slave.read_buffer() function reads data from the I2C bus. If no data is received (received_data == NULL), the loop continues without processing.

Command Division:

Because commands can accumulate in the buffer divideI2CPacket(received_data, &command_count) splits the received data into individual commands based on the newline character \n. It returns an array of command strings (commands) and updates command_count with the number of valid commands.

Command Parsing:

Iterates over each command string. Uses parseServoCommand(command) to convert the string into a RpiDataPacket structure containing the command and its parameters. If parsing fails (numbers == NULL), it skips to the next command.

Command Processing:

After parsing the commands into RpiDataPacket structures, the system uses a switch statement to handle different command types based on the command field of the RpiDataPacket.

1. Set Angle (SET_ANGLE)

• Purpose: Adjusts the angle of a specified servo.
• Validation:
  • Checks if the servo ID (param1) is valid.
  • Ensures that auto-aim is not enabled, as manual control is only allowed when auto-aim is disabled.
• Action:
  • Calls the set_angle method on the specified servo with the desired angle (param2).
  • Resets both the X and Y PID controllers to prevent any residual control effects from previous operations.

2. Set Minimum Pulse Width (SET_MIN_PULSE_WIDTH)

• Purpose: Updates the minimum pulse width setting for a specified servo.
• Validation:
  • Checks for a valid servo ID.
  • Ensures auto-aim is disabled to allow manual configuration.
• Action:
  • Calls set_min_pulse_width on the specified servo with the new minimum pulse width value.

3. Set Maximum Pulse Width (SET_MAX_PULSE_WIDTH)

• Purpose: Updates the maximum pulse width setting for a specified servo.
• Validation:
  • Checks for a valid servo ID.
  • Ensures auto-aim is disabled.
• Action:
  • Calls set_max_pulse_width on the specified servo with the new maximum pulse width value.

4. Set Error (SET_ERROR)

• Purpose: Adjusts servo positions based on error values from a tracking system, primarily used when auto-aim is enabled.
• Validation:
  • Ensures that auto-aim is not in manual mode; the command is ignored if it is.
• Process:
  • Creates an error coordinate with x and y values from param1 and param2.
  • If both error values are zero:
    • Checks if a "reseter" task is already running.
    • If not, creates a "reseter" task to reset the controllers after a delay.
    • Skips further processing to avoid unnecessary calculations.
  • If the error is non-zero and a "reseter" task exists:
    • Deletes the "reseter" task to prevent it from resetting the controllers prematurely.
• Action:
  • Calculates control outputs using the X and Y PID controllers based on the error values.
  • Logs the error and control values for debugging purposes.
  • Adjusts the servos by adding the control outputs to their current pulse widths using the add_pulse_width method.

5. Set Controller Parameters (SET_CONTROLLER_PARAMETERS)

• Purpose: Updates specific parameters of the PID controllers or resets them.
• Process:
  • Casts param1 to a ControllerParameters enum to identify which parameter to update.
  • Converts param2 to a floating-point value (dividing by 1000.0) for precision.
• Action:
  • Depending on the parameter specified (KP, KI, KD, KS, or RESET), it updates the corresponding gains of both X and Y PID controllers or resets them.
• Error Handling:
  • Logs an error message if an invalid controller parameter is specified.

6. Toggle Auto-Aim (TOGGLE_AUTO_AIM)

• Purpose: Changes the state of the auto-aim feature.
• Validation:
  • Checks if the new auto-aim state (param2) is within the valid range.
• Action:
  • Updates the auto_aim_state variable by casting param2 to the AutoAimState enum.
  • Logs the new auto-aim state for confirmation.

7. Default Case

• Purpose: Handles any unrecognized or invalid commands.
• Action:
  • Logs an error message indicating that an invalid command was received.

PID Controller

The PIDController class implements a PID control loop to calculate the control output based on the error between a desired setpoint and the current process variable. It adjusts the control signal to minimize this error over time, providing smooth and precise control for systems such as servo motors.

Control Calculation

The core functionality lies in the calculateControl(double error) method, which computes the control output using the PID formula:

1. Proportional Term (up):

   • Calculated as up = kp * error.
   • Provides an immediate response proportional to the current error.
   • A higher proportional gain (kp) increases the responsiveness but can cause overshoot.

2. Integral Term (ui):

   • Updated as ui = prev_ui + kp * ki * prev_error.
   • Accumulates past errors to eliminate steady-state offset.
   • The integral gain (ki) determines the contribution of the accumulated error.
   • prev_ui stores the previous integral term for continuous accumulation.

3. Derivative Term (ud):

   • Calculated as ud = kp * kd * (error - prev_error).
   • Predicts future error based on the rate of change.
   • The derivative gain (kd) dampens the system response, reducing overshoot and oscillations.

4. Control Output (u):

   • Computed as u = ks * (up + ui + ud).
   • The scaling factor (ks) adjusts the overall control output magnitude.
   • Summing the three terms provides a balanced control action considering present, past, and future errors.

After calculating u, the method updates the internal state:

• prev_error is set to the current error for use in the next iteration.
• prev_ui is updated to the current ui, preserving the integral term's history.

Parameter Update Methods

The class includes methods to update controller parameters at runtime:

• updateKp(double kp): Updates the proportional gain.
• updateKi(double ki): Updates the integral gain.
• updateKd(double kd): Updates the derivative gain.
• updateKs(double ks): Updates the scaling factor.

These methods allow dynamic tuning of the controller to adapt to changing system conditions or to optimize performance.

Reset Function

The reset() method reinitializes the controller's internal state:

• Resets prev_ui and prev_error to zero.
• Useful when the system needs to restart control from a known state, eliminating accumulated errors.

Servo PWM Control

The Servo class encapsulates the functionality required to interface with servo motors via the ESP32's LEDC (LED Control) PWM hardware. It initializes the necessary timers and channels, and provides methods to set servo angles, adjust pulse widths, and update servo positions accordingly.

Initialization

When a Servo object is created, it performs the following steps:

1. LEDC Timer Configuration:

   • Sets up a PWM timer with a frequency of 50 Hz, which is standard for servo control.
   • Uses a 13-bit duty resolution, providing fine control over the pulse width.
   • Selects the appropriate LEDC mode (high-speed or low-speed) based on the hardware capabilities.

2. LEDC Channel Configuration:

   • Assigns the specified GPIO pin to the LEDC channel.
   • Initializes the channel with zero duty cycle initially.
   • Associates the channel with the configured timer.

3. Default Pulse Width Range:

   • Sets default minimum and maximum pulse widths (typically 1000 µs to 2000 µs).
   • These values correspond to the servo's physical limits (0 to 180 degrees).

Servo Movement Control

Setting Servo Angle

The set_angle(int angle) method allows setting the servo to a specific angle within its operating range:

• Input Validation:

  • Clamps the input angle between 0 and the maximum degree (usually 180 degrees) to prevent invalid positions.

• Pulse Width Calculation:

  • Calculates the required pulse width corresponding to the desired angle.
  • Uses linear interpolation between the minimum and maximum pulse widths based on the angle:

    pulse_width = min_pulse_width + (angle) * ((max_pulse_width - min_pulse_width) / SERVO_MAX_DEGREE)
    

  • This ensures that the full range of motion is mapped proportionally to the pulse width range.

• Updating the Servo:

  • Calls the update() method to apply the new pulse width to the servo.

Adjusting Pulse Width

The add_pulse_width(uint32_t change) method adjusts the current pulse width by a specified amount:

• Change Application:

  • Adds the change value to the current pulse width.
  • Can be positive (increase pulse width) or negative (decrease pulse width).

• Boundary Enforcement:

  • Ensures that the new pulse width stays within the defined minimum and maximum limits.
  • Prevents the servo from moving beyond its physical capabilities.

• Updating the Servo:

  • Calls the update() method to apply the adjusted pulse width.

Updating Servo Position

The update() method applies the current pulse width to the servo hardware:

• Duty Cycle Calculation:

  • Converts the pulse width (in microseconds) to a duty cycle value compatible with the LEDC hardware.
  • The formula used is:

    duty = (pulse_width * max_duty) / period
    

    • max_duty is determined by the duty resolution (e.g., 2^13 for 13-bit resolution).
    • period is the PWM period in microseconds (e.g., 20,000 µs for 50 Hz).

• Applying Duty Cycle:

  • Sets the duty cycle on the LEDC channel.
  • Updates the duty cycle to take effect immediately.

Pulse Width Configuration

The class provides methods to configure the pulse width range:

• set_min_pulse_width(uint32_t min_pulse_width):

  • Updates the minimum pulse width.
  • Adjusts the current pulse width to the new minimum and updates the servo.

• set_max_pulse_width(uint32_t max_pulse_width):

  • Updates the maximum pulse width.
  • Adjusts the current pulse width to the new maximum and updates the servo.

These methods allow calibration of the servo's range to match specific hardware characteristics.