1、“Coordinate System” ChallengeThe challenge this time is that you robot needs to be able to drive to arbitrary coordinates (x,y). It does so via state variables to track its current x- and y-coordinates and heading, using a function “goTo” to compute the correct heading and distance to the new (x,y)
2、coordinates. Note that the function I am using is spelled “goTo”, different from the reserved keyword “goto”.For example, to move to the above points in order we could use the main function as follows:public static void main(String args)goTo( 50, 50);goTo(-30, 60);goTo(-60,-20);goTo( 0, 0);Of course
3、 the program will need additional utility functions and state variables to do its work. Here is suggested starting point for your program: Create a new class “Traveler” in your NXT project in Eclipse Open that class and delete *everything* in it Copy the code below, and only that code Paste the copi
4、ed code into your Java file and saveIt should compile without error. Of course it wont work you will need to fill in the missing functions and perhaps define additional ones. But it should give you something to start. See more notes after this function, hopefully helpful.import lejos.nxt.*;/* This p
5、rogram allows the robot to keep track of its position* in the world. We assume that the robot is initially positioned * at the origin, facing north along the (imagined) y-axis (a 90 degree * angle to x-axis).* All distances are assumed to be in cm, all angles in degrees. All * angles are counted in
6、math orientation, i.e. counter-clockwise. * The main functions to use are:* * goTo(x,y) - moves robot from current position to (x,y)* */public class Traveler / constantsfinal static float CONVERT_TURN = 2.05f;final static int SPEED_CALIBRATE = 45;/ state variablesstatic Motor leftMotor = Motor.A;sta
7、tic Motor rightMotor = Motor.C;static CompassSensor compass = new CompassSensor(SensorPort.S4);static int xLocation = 0; / in cmstatic int yLocation = 0; / in cmstatic int heading = 90; / in degreespublic static void main(String args)calibrate(SPEED_CALIBRATE, CONVERT_TURN);goTo(10, 50);goTo(50, 70)
8、;goTo(60, 90);goTo(0,0);/* This function moves the robot to the new (x,y) * coordinates and updates its position and heading.*/public static void goTo(int x, int y)int angle = findAngleTo(x, y);int dist = findDistanceTo(x, y);turnWithCompass(angle); / as beforetravel(dist); / as before, but more acc
9、uratexLocation = x;yLocation = y;heading = heading + angle;/* This function computes the shortest angle (in * degrees) between the current location and heading* and the (x,y) coordinates. * * It will return an integer between -179 and 180.*/public static int findAngleTo(int x, int y)/ I leave it to
10、you to figure it out. Right now/ the function returns 0 so that it compiles .return 0;/* This function computes the distance between the* current location and the (x,y) coordinates.* * It will return a positive integer*/public static int findDistanceTo(int x, int y)int dX = x - xLocation;int dY = y
11、- yLocation;return (int)Math.round(float)Math.sqrt(dX*dX + dY*dY);public static void turn(int degree)/ rotate as previously by open loop computationpublic static void turnWithCompass(int degree)/ reset compass to “zero”/ rotate using “turn”/ check (Cartesian) compass heading:/ * if compass heading a
12、grees with desired angle, fine/ * if there is a discrepancy between measured angle and/ indented angle, compute error and rotate againpublic static void travel(int distance)/ use compass to check if youre going straight/ if not, proportionally adjust speed to wheels/ to correct/* Method to calibrate
13、 compass by rotating at least * twice very slowly. Requires two input variables:* * turnSpeed: int speed at which to turn. Must be slow* so that one full turn takes 20 secs* or more.* convert: float conversion factor to convert degrees* of wheel rotation into robot rotation* degrees. Depends on whee
14、l size and axis* length. Try aprrox. 2.0f and see if that* gives you two rotations.*/public static void calibrate(int speed, float convert) throws Exceptionint numDegrees = (int)Math.round(Math.abs(720)*convert);Sound.buzz();LCD.clearDisplay();LCD.drawString(“COMPASS CALIBRATE“, 0, 1);LCD.drawString
15、(“ ENTER to start“, 0, 3);LCD.drawString(“ ESC to skip“, 0, 4);Button.waitForPress();if (Button.ESCAPE.isPressed()return;LCD.drawString(“.calibrating.“, 1, 6);leftMotor.setSpeed(speed);rightMotor.setSpeed(speed);leftMotor.resetTachoCount();rightMotor.resetTachoCount();leftMotor.forward();rightMotor.
16、backward();compass.startCalibration();while (leftMotor.getTachoCount() = -numDegrees)if (leftMotor.getTachoCount() numDegrees)leftMotor.stop();if (rightMotor.getTachoCount() ENTER.“, 0, 7);Button.ENTER.waitForPressAndRelease();Button.ENTER.waitForPressAndRelease();You will need conversion functions
17、between radians and degrees, since your state variables (and turn function) will be using degrees, whereas the internal Java trig functions work with radians. You could try functions like:/* Converts the input (degrees) to radians*/public static float toRadians(int degree)return (float)(degree / 180
18、.0 * Math.PI);/* Converts the input (radians) to degrees*/public static int toDegrees(double radians)return (int)Math.round(float)(radians/Math.PI * 180.0);Thats not bad. As far as the function findAngleTo is concerned, lets look at the following picture: h is the current heading (in degrees) theta
19、is arctan(dy/dx) angle to turn is (h theta)There are a few problems: the arctan does not give the right angle, it needs to be adjusted according to which quadrant the point is in. Luckily, Java provides another trig function, which gives the exact right angle: Math.atan2(y, x) = angle of the vector
20、(x, y) to the x-axis (in radians)However, that angle is in radians so it needs to be converted to degrees before we can use it lets assume we did that so that theta is the angle in degrees. Then our turning angle will be either h theta or theta h, depending on which way we need to turn.As a final di
21、fficulty, we want to ensure the resulting turning angle is between -179 and 180, so that might require some further experimentation And be careful you need to make sure that the turn function later on takes care of negative angles by turning one way and positive angles by turning the other way!Final
22、ly, so far our solution is “open-loop”: any errors in turning and travelling will accumulate. Thus, it is important to be able to turn as exactly as you can. To accomplish that, use the compass sensor.I would suggest the following approach to get going: copy the above program into your project work
23、on the “findAngleTo” function and test it thoroughly to ensure it works. To test it, you could use the LCD screen to display computed values you do not really need to turn your robot (yet) implement the turn function from your previous program, but make sure to modify it so it can take positive and
24、negative angles implement the travel function, as before test if your robot moves where it should if successful, add the compass, modify “turn”, and see if you are more accurate now if successful, change the “travel” function to use the compass to ensure youre going straightEnough said start early and ask me if you have any questions!
