Lab 12

Objective

The objective of this lab is to integrate the major components developed in previous labs and use them to complete path planning and execution. This includes combining ToF-based linear PID control, IMU-based orientation PID control, BLE communication, and sensor feedback to control the robot’s motion more accurately.

Lab Tasks

In this lab, I implemented waypoint-based path planning and execution. A sequence of predefined waypoints was used to define the desired trajectory. For each segment between two consecutive waypoints, I computed the required rotation angle and distance. Based on this, I integrated both orientation PID control and linear PID control from previous labs to perform feedback control. This allows the robot to rotate to a precise angle and then move forward a specified distance for each segment, enabling accurate path following.

Arduino Implementation

Compared to previous PID control implementations, each motion step now consists of two stages: 1.Rotating the robot to the desired heading; 2.Moving forward for a target distance

To implement this, I first measured the initial ToF distance and then computed the corresponding target distance for the motion segment. The linear PID controller was used to regulate the forward motion based on the difference between the current and target ToF readings.

void move_forward_tof_delta(float Kp, float Ki, float Kd, int distance_mm, int timeout_ms, int stop_tol_mm) {
  stop();
  // Because the front distance sensor is broken, the distanceSensorLeft is the front sensor now.
  distanceSensorLeft.startRanging();

  // Read initial ToF distance
  int initial_tof = read_valid_tof();
  int target_tof = initial_tof - distance_mm;
  
  unsigned long start_time = millis();
  int stable_count = 0;

  while (millis() - start_time < (unsigned long)timeout_ms) {
    if (!distanceSensorLeft.checkForDataReady()) {
      delay(2);
      continue;
    }

    int current_tof = distanceSensorLeft.getDistance();
    distanceSensorLeft.clearInterrupt();

    if (current_tof < 30 || current_tof > 4000) {
      continue;
    }

    float pterm, iterm, dterm;
    int u = pid_position_tof(Kp, Ki, Kd, current_tof, target_tof, pterm, iterm, dterm);

    int err = current_tof - target_tof;

    if (abs(err) <= stop_tol_mm) {
      stable_count++;
    } else {
      stable_count = 0;
    }

    // ... optionally record PID data here

    if (stable_count >= 3) {
      break;
    }

    delay(5);
  }

  distanceSensorLeft.stopRanging();
  stop();

  // ... optionally send summary and PID log here
}

The control logic was implemented using a case structure, similar to previous labs. The overall PID control framework remains the same, including error computation, proportional-integral-derivative updates, and output. By combining orientation and linear control in sequence, the robot can execute each planned segment.

Other control cases and codes, such as turning a specific angle, linear PID control and orientation PID control are implemented similarly to previous labs, with some adjustments to accommodate the new control flow and parameters.

Python Implementation

On the Python side, I predefined all the waypoints that define the desired path. I then implemented functions to compute the required rotation angle and travel distance between consecutive waypoints.

For example, when moving from (-4, -3) to (-2, -1), I first compute the displacement (dx, dy). Using the atan2 function, I calculate the required rotation angle, which is $45^\circ$. Then, I compute the corresponding distance. The function send_turn is called to execute the rotation, and send_move_tof is called to move the robot forward. In addition, I return the current heading after executing the segment, which will be used as the previous heading for the next segment. This way, I can ensure that the robot correctly updates its orientation and position as it follows the planned path.

def execute_segment(p0, p1, previous_heading=0.0):
    """
    p0, p1: grid coordinates, e.g. (-4, -3), (-2, -1)
    """
    x0, y0 = p0
    x1, y1 = p1

    dx = x1 - x0
    dy = y1 - y0

    desired_heading = math.degrees(math.atan2(dx, dy))
    turn_angle = wrap_to_180(desired_heading - previous_heading)

    distance_cells = math.sqrt(dx * dx + dy * dy)
    distance_mm = distance_cells * CELL_SIZE_MM

    // ... print some debug info

    send_turn(turn_angle, kp=ORI_KP, ki=ORI_KI, kd=ORI_KD)
    time.sleep(0.3)

    send_move_tof(distance_mm)
    time.sleep(0.3)

    send_stop()

    return desired_heading

WAYPOINTS = [(-4, -3), (-2, -1), (1, -1), (2, -3), (5, -3), (5, -2), (5, 3), (0, 3), (0, 0)]

prev_heading = 0.0

for i in range(len(WAYPOINTS) - 1):
    p0 = WAYPOINTS[i]
    p1 = WAYPOINTS[i + 1]

    prev_heading = execute_segment(p0, p1, previous_heading=prev_heading)

    time.sleep(0.5)

For each segment, the computed parameters were sent to the robot via BLE commands, triggering the corresponding control case on the Arduino. This enables the robot to execute the planned trajectory step by step. This is the video of the whole process, and the robot successfully follows the path defined by the waypoints.

Discussion

The integration of orientation and linear control allows the robot to follow paths defined by multiple waypoints. However, the path planning and execution was not very accurate all the time. And in the video, I sometimes need to adjust the position a little by hand to make the robot reach the target, but the whole process is successful. This may be due to several factors, including sensor noise, PID tuning parameters and so on. In order to detect the distance in map, we should use Long mode of ToF sensor, and this mode is not very accurate compared with Short mode. I think another reason is that the rotation is not very precise, so it would have some error and deviation which would affect the overall performance.

In the future, I would like to explore how to improve the accuracy and robustness of the system. For example, I can try to collect distances from ToF sensors firstly and compute the average distance to reduce the noise. I can also try to tune the PID parameters more carefully and make the robot more stable during rotation.

Reference

Thanks to Professor Helbling and the TAs for their help during lab sessions. I refer to Wenyi Fu's and Lucca Correia's websites as guidance and as references. I used AI tools to help me with refining the writing and improving the clarity of my report.