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:

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.

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.

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.

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.

• 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.

• 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.

• 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.

• 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.

• 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.

• 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.

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

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.

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.

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.

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.