pololu goal finding

v1.0.1

4/19/2022

---

Features

Given any number of waypoints as (x,y,theta) polar coordinates, use the wheel encoders and localization to determine the robots current position, and navigate to it's goal using a PID Controller

Localization

Begin tracking the wheel encoders at the (0,0) position

Encoder wheel distance tracking:

// update the distance traveled by each wheel
sLeftT1 += (countsLeftT1 - countsLeftT2) * DIST_PER_TICK;
sRightT1 += (countsRightT1 - countsRightT2) * DIST_PER_TICK;

// get change of current and previous distance traveled
sLeftDelta = sLeftT1 - sLeftT2;
sRightDelta = sRightT1 - sRightT2;

Once encoder data has been updated, update the robots current position

Distance formula

For our usage we will implement

// get linear distance
currentGoalDistance = sqrt(sq(xGoal - x) + sq(y - yGoal));

PID Controller

Well not really. More so a P controller (proportional error). Set the error in radians by using arctan to compare current angle vs the goal.

currentError = theta - atan2(yGoal - yPos, xGoal - xPos);
PIDCorrection = KP * currentError;

Hardware

Pololu3piPlus32U4 Robot, Turtle Edition

Purchase Pololu 3pi+ OLED Robot from pololu

Libraries

Pololu3piPlus32U4

Pull and explore pololu git repository and add as a project dependency.

Pololu Usage

#include <Pololu3piPlus32U4.h>

using namespace Pololu3piPlus32U4;

Buzzer buzzer;
Motors motors;
Encoders encoders;

License

MIT