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

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

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[]) }

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[]) }

Figuring new heading

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; }

Figuring new (x,y) location

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); }

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

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) ) * 180.0 / Math.PI; Note the use of Math.atan2, not the regular Math.atan

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); }

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); }

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

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);