Page 261 - The Definitive Guide to Building Java Robots
P. 261

Preston_5564C07.fm  Page 242  Monday, September 26, 2005  5:38 AM



                 242    CHAPTER 7  ■  NAVIGATION



                            // instance variables
                            public int surface = SURFACE_CEMENT;
                            private JMotion drive;
                            private NavStamp navStamp;

                            public Navigation(JSerialPort serialPort) throws Exception {
                                // drive with default speed
                                drive = new SpeedDiffDrive(serialPort);
                                drive.setSpeed(DEFAULT_SPEED);
                                // stamp for sensors
                                navStamp = new NavStamp(serialPort);
                            }

                            // change heading
                            public void changeHeading(int newHeading) throws Exception {
                                // this will calculate a real angle from a relative measure of
                                // the coord axis.
                                newHeading = getRealAngle(newHeading);
                                int accuracy = 2; // degrees
                                // autoadjust speed depending on the surface
                                if (surface == SURFACE_CEMENT) {
                                    // slow so don't overshoot 15 degrees at 1sec intervals
                                    drive.setSpeed(12);
                                } else {
                                    // moves slower on carpet
                                    drive.setSpeed(20);
                                }
                                // used to record lats turn
                                int lastTurn = 0;
                                boolean toggle = false;
                                int turnSize = 1000;
                                while (true) {
                                    // get compass
                                    int currentHeading = navStamp.getCompass();
                                    // get relative heading from compass to where you want to go
                                    int relHeading = currentHeading - newHeading;

                                    // adjust for negative
                                    if (relHeading < 0) {
                                        relHeading = 360 + relHeading;
                                    }
                                    // if within bounds, stop
                                    if (relHeading <= accuracy || relHeading >= 360 - accuracy) {
                                        drive.stop();
                                        break;
                                    }
   256   257   258   259   260   261   262   263   264   265   266