1. Introduction to Robots and the Mind - Path Integration -
Bert Wachsmuth & Michael Vigorito
Seton Hall University

2. Robot with Sense of Location
Goal: Robot should know its location, the location of “home”, and that of “food” found Superimpose a Cartesian Coordinate system with home at the origin and maintain “(x,y) location” and “heading” variables. Start at location = (0,0), heading = 0 degree and update both variables at every move

3. Base Program
public class KnowThyPlace { // define constants: TURN_FACTOR, TURN_SPEED, TRAVEL_FACTOR, TRAVEL_SPEED // define 2 fields for left/right motors // define following methods public static void turn(double angle) public static void travel(double dist) public static void main(String args[]) }

4. Adding “brain cells”
public class KnowThyPlace { // as before but add three fields public static double x = 0.0; public static double y = 0.0; public static double heading = 0.0; // define following methods public static void turn(double angle) public static void travel(double dist) public static void main(String args[]) }

5. Figuring new heading

6. Keeping Track while Turning
public static void turn(double angle) { motorLeft.setSpeed(TURN_SPEED); motorRight.setSpeed(TURN_SPEED); double degrees = TURN_FACTOR * angle; motorRight.rotate((int)degrees, true); motorLeft.rotate(-(int)degrees); // updating heading of robot heading = (heading + angle) % 360; }

7. Figuring new (x,y) location

8. Keeping Track while Driving
public static void travel(double dist) { motorLeft.setSpeed(TRAVEL_SPEED); motorRight.setSpeed(TRAVEL_SPEED); double degrees = TRAVEL_FACTOR * angle; motorRight.rotate((int)degrees); motorLeft.rotate((int)degrees); // Updating (x,y) location of robot x = x + dist*Math.cos(heading*Math.PI/180.0); y = y + dist*Math.sin(heading*Math.PI/180.0); }

9. Distance and Angle
We need to compute the distance between two points (x,y) and (xNew,yNew) and the angle between them:

10. Distance and Angle
Here is the code to compute distance and angle between points (x1,y1) and (x2,y2):
public static double findDistance(double x1, double y1,
double x2, double y2) {
return Math.sqrt( (x2–x1)*(x2–x1) + (y2–y1)*(y2–y1) ); }
public static double findAngle(double x1, double y1,
return Math.atan2( (y2-y1), (x2-x1) ) * / Math.PI;
Note the use of Math.atan2, not the regular Math.atan

11. Reaping the Benefits
We can now add a method “driveTo” to drive to a specific spot in our coordinate system:
public static void driveTo(double xNew, double yNew) {
double angle = findAngle(x, y, xNew, yNew);
double dist = findDist(x, y, xNew, yNew);
// find out which angle to turn based on current heading
// and computed angle in which the new point is:
int turnAngle = (int)(angle – heading) % 360;
turn(turnAngle);
travel(dist); }

12. Using the driveTo method
We can use driveTo to drive to specific points in an imaginary coordinate system, while returning to our starting point simply by calling driveTo(0,0)
For example, to drive in a square we’d say (after calibrating the parameters, see next slide):
public static main(String[] args) {
driveTo(100,0);
driveTo(100,100);
driveTo(0, 100);
driveTo(0, 0); }

13. Calibration The calculations depend on your robot making exact turns
distance does not have to be exact, but
turns have to be as exact as possible
Calibrate your robot before running it, especially if its environment has changed
Replace any code in the main method by a call to “turn(360)” and execute
Adjust TURN_FACTOR until the robot makes a perfect 360 degree turn
Similarly adjust TRAVEL_FACTOR until a call to “travel(100)” results in a distance of 1 meter traveled

14. Random Movements
Make sure you calibrated the constants (previous slide).
Create a counting loop for 4 random movements, then return “home”
public static main(String[] args) {
int count = 0;
while (count < 4)
// Creating two random numbers between 20 and 80
double x = 60*Math.random() + 20;
double y = 60*Math.random() + 20;
// Driving to that random location
driveTo(x,y);
count = count + 1; }
// Returning “home” to the origin
driveTo(0, 0);