Challenge 1: Delay‐Based Path Following

Goal

The overall goal of this set of challenges will be to develop a system that can map its environment, plan a series of moves towards a goal position, and execute said moves reliably. For now, though, we'll start with the basics of moving from one point to another. Delay based systems are some of the simplest to implement for robotic motion. The basic idea is:

1. Run some experiments to determine the speed at which your system moves. For our tank drive robot, this might be its top speed when we run both motors at PWM 255.
2. Use that information to estimate how long you must run the system to reach a desired state. For example, if we know the top speed of our robot to be 0.4 meters per second, and we wish to move forward 1 meter, we can estimate that we should drive forward for approximately $1/0.4 =2.5$ seconds to achieve this motion.

By the end of this challenge, you will have written a system that can drive our simple tank robot to a pre-programmed series of points along a path.

Setup

Alright, lets get started!

First thing's first, create a folder and an ardMain.cpp for a new sketch. You can fill this with the same skeleton code from the Getting Started page:

#include <mosscap/challenge1.h>

#include <Motor.h>

Motor left(11, 10, 12);
Motor right(21, 20, 22);

void setup() {}

void loop() {}

Note that instead of including mosscap/defaultTank.h we've instead included mosscap/challenge1.h. This file configures the simulator slightly differently, adding a path to the 'floor' below the robot and providing the list of locations that the robot must travel to.

Before moving on, remember to run Mosscap: Initialize Environment to make those red squiggles go away!

A closer look at our new environment

Before we dive in any deeper, let's take a moment to understand how the simulator is set up for this challenge.

Similarly to the Getting Started example, our motors and button are on the following pins:

• The left motor is put on pins 11, 10, 12
• The right motor is put on pins 21, 20, 21
• The button is wired to pin 1

Additionally, we also now have access to a set of points that we must drive to. Our robot starts at (0.6,0.75) (measured from the bottom left corner) and the goal is to drive along the path defined by the series of points provided by pathPoints. This variable is a 2D array that stores a series of 8 goal positions (or 'waypoints') for the robot to pass through.

Some Guidance

While these challenges are meant to be completed with little guidance, you may want to structure your code something like the following:

double driveSpeed = /* Fill in with your number */;
double turnSpeed = /* Fill in with your number */;

void driveDistance(double dist) {
    // 1. Determine the amount of time the robot needs to drive for
    // 2. Command motors to move
    // 3. Delay for calculated time
    // 4. Stop the robot
}

void turnAngle(double angle) {
    // This will be very similar to the `driveDistance` function
}

void setup() {
    // Set up button
    pinMode(1, INPUT);
}

// To keep track of which direction we're currently facing
double angle = 0;

void loop() {
    // Wait for user to press the button
    while (!digitalRead(1)) {}
    while (digitalRead(1)) {}

    // For each point in the list: turn towards it and drive there
    for (int i = 0; i < pathPoints.size(); i++) {
        // 1. Use the current angle and position (pathPoints.at(i - 1) be careful if i = 0!) to determine the amount to turn
        // 2. Call `turnAngle()` to turn the robot toward the next point
        // 3. Calculate the distance to be driven (perchance with some help from Pythagoras)
        // 4. Call `driveDistance()` to move the robot to the next point
    }
}

You may notice that you must determine the driving and turning speed of the robot. You can certainly do this ahead of time with some experimentation, or you can take an initial guess and tweak the values as you test your other functions until everything lines up.

A Note on Accuracy

You may notice that it is incredibly challenging to get the robot to behave predictably in this challenge. Indeed, the robot will drift to the side, not turn as much as you think, and end up in slightly different places each run. This is one of the great challenges of robotics!

How do we build a system that behaves (somewhat) deterministically when there are countless unpredictable factors working against us in the real world?

For this challenge, don't worry too much about being dead accurate every single time, we will gradually explore more robust (and complex) methods to drive our robot predictably throughout this series of challenges.

Solution

#include <mosscap/challenge1.h>

#include <Motor.h>

Motor left(11, 10, 12);
Motor right(21, 20, 22);

double driveSpeed = 0.5; // Meters per second
double turnSpeed = 275; // Degrees per second

void driveDistance(double dist) {
    // Convert from seconds to milliseconds as `delay()` takes a number of milliseconds
    int time = int(dist / driveSpeed * 1000);
    left.run(255);
    right.run(255);

    delay(time);

    left.run(0);
    right.run(0);
}

void turnAngle(double angle) {
    int time = abs(angle / turnSpeed * 1000);

    if (angle > 0) {
        left.run(-255);
        right.run(255);
    } else {
        left.run(255);
        right.run(-255);
    }

    delay(time);

    left.run(0);
    right.run(0);
}

void setup() {
    // Set up button
    pinMode(1, INPUT);
}

// To keep track of which direction we're currently facing
double angle = 0;

void loop() {

    // Wait for user to press the button
    while (!digitalRead(1)) {}
    while (digitalRead(1)) {}

    // For each point in the list: turn towards it and drive there
    for (int i = 0; i < pathPoints.size(); i++) {
        // Determine current and target positions
        double currentX = pathPoints[i][0];
        double currentY = pathPoints[i][1];

        double targetX, targetY;
        if (i == pathPoints.size() - 1) {
            targetX = pathPoints[0][0];
            targetY = pathPoints[0][1];
        } else {
            targetX = pathPoints[i + 1][0];
            targetY = pathPoints[i + 1][1];
        }

        // Calculate number of degrees to turn
        // Convert to degrees
        double targetAngle = atan2(targetY - currentY, targetX - currentX) / PI * 180.0;

        double angleDelta = targetAngle - angle;

        // Ensure that we don't turn more than we need to (turn 90 to the left instead of 270 to the right)
        if (abs(angleDelta) > 180.0) {
            if (angleDelta > 0) {
                angleDelta -= 180.0;
            } else {
                angleDelta += 180.0;
            }

            angleDelta *= -1;
        }

        // Turn the robot
        turnAngle(angleDelta);

        // Update our current angle for the next iteration of the loop
        angle = targetAngle;

        // Calculate distance to drive
        double dist = sqrt(pow(targetX - currentX, 2) + pow(targetY - currentY, 2));

        // Move the robot
        driveDistance(dist);
    }
}