Position Control

It is time to implement position control on the robot. We’ve had previous “go to goal” exercises, but this will be the first time we implement a control loop to achieve that goal!

What you will learn

• How to implement position control on the robot
• How to structure Arduino code to handle multiple tasks

Lecture

Position control sketch setup

Setting up motor control

Setting up forward kinematics and plotting the position

Implementing position control

Interactive

TRACK_WIDTH = 0.17;
TIME_STEP = 0.1;
TIME_END = 20;

function forward_kinematics(vl, vr, x, y, θ) {
  const v = (vr + vl) / 2.0;
  const θdot = (vr - vl) / TRACK_WIDTH;

  x += v * Math.cos(θ) * TIME_STEP;
  y += v * Math.sin(θ) * TIME_STEP;
  θ += θdot * TIME_STEP;

  return [x, y, θ];
}

function position_control(x, y, θ, gx, gy, gt) {
  const d = Math.sqrt((gx - x) ** 2 + (gy - y) ** 2);

  if (d < gt) { return [0, 0]; }

  let v = Math.min(K_position * d, max_linear);

  const angle_to_goal = Math.atan2(gy - y, gx - x);
  const θerror = angle_to_goal - θ;
  let θdot = Math.min(K_orientation * θerror, max_angular);

  const vl = v - θdot * TRACK_WIDTH / 2;
  const vr = v + θdot * TRACK_WIDTH / 2;

  return [vl, vr];
}

function simulate(gx, gy, gt) {
  let x = 0;
  let y = 0;
  let θ = 0;
  let vl = 0;
  let vr = 0;

  const data = [];
  for (let t = 0; t < 10; t += TIME_STEP) {
    [x, y, θ] = forward_kinematics(vl, vr, x, y, θ);
    [vl, vr] = position_control(x, y, θ, gx, gy, gt);
    data.push({x, y, θ});
  }

  return data;
}

viewof goal_x = Inputs.range([-2, 2], {value: 1, step: 0.1, label: "Goal x"})
viewof goal_y = Inputs.range([-2, 2], {value: -1, step: 0.1, label: "Goal y"})
viewof goal_threshold = Inputs.range([0, 0.4], {value: 0.1, step: 0.1, label: "Goal threshold"})
viewof K_position = Inputs.range([0, 4], {value: 0.2, step: 0.1, label: "K_position"})
viewof K_orientation = Inputs.range([0, 4], {value: 1, step: 0.1, label: "K_orientation"})
viewof max_linear = Inputs.range([0, 2], {value: 0.3, step: 0.1, label: "Max linear velocity"})
viewof max_angular = Inputs.range([0, 2], {value: 1, step: 0.1, label: "Max angular velocity"})

trajectory = simulate(goal_x, goal_y, goal_threshold);

Plot.plot({
  grid: true,
  aspectRatio: 1,
  x: {domain: [-2, 2]},
  y: {domain: [-2, 2]},
  marks: [
    Plot.dot(trajectory, {x: "x", y: "y"}),
    Plot.dot([{x: goal_x, y: goal_y}], {x: "x", y: "y", fill: "red", r: goal_threshold*200}),
  ]
})

Exercise

Your task is to follow the lecture videos and implement position control on your robot.

You will submit your responses on gradescope. Only one partner should submit. The submitter will add the other partner through the gradescope interface.

Additional details for using gradescope can be found here:

• Submitting an Assignment
• Adding Group Members
• gradescope Student Help Center

You should open the gradescope assignment now so that you know what to work complete.

Grading

I will grade all exercises using a scale of “Nailed It” / “Not Yet”. See the course grading policy for more information, and check gradescope for deadlines.

Overview

You should use the pseudocode from the previous chapter and your simulation implementation to guide your Arduino code. See gradescope for submission details.

To use the plot on the website, you should output your values in the following format:

"x=%f y=%f theta=%f vl=%f vr=%f"

Wrap-Up

You can now command your robot to any specific position. We’ll later look at how noise affects this process, and how we can plot “proper” positions and paths.

Resources

• How to format strings without the String class | C++ for Arduino