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

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



                                                                               CHAPTER 7  ■  NAVIGATION  243



                                    // in case it overshoots direction twice
                                    if (toggle) {
                                        // reset
                                        toggle = false;
                                        // reduce turn time by 250ms
                                        turnSize = turnSize - 250;
                                    }
                                    // turn for a second left
                                    if (relHeading < 180 && relHeading > 15) {
                                        if (lastTurn == 'R') {
                                            toggle = true;
                                        }
                                        drive.pivotLeft(turnSize);
                                        // record what turn
                                        lastTurn = 'L';
                                        // turn for a second right
                                    } else if (relHeading >= 180 && relHeading < 345) {
                                        // records toggle
                                        if (lastTurn == 'L') {
                                            toggle = true;
                                        }
                                        drive.pivotRight(turnSize);
                                        lastTurn = 'R';
                                    } else if (relHeading >= 345) {
                                        drive.pivotRight(250);
                                    } else if (relHeading <= 15) {
                                        drive.pivotLeft(250);
                                    }
                                }
                                // set back to default speed
                                drive.setSpeed(DEFAULT_SPEED);
                            }

                            // adjust for angle measured to absolute angle
                            public static int getRealAngle(int theta) {


                                int phi = 0;
                                double ratio = 0.0;
                                // if in 1st quadrant
                                if (theta > 0 && theta < 90) {
                                    // 1. get % of the total range
                                    // 2. get range
                                    // 3. multiply range by percentage, add it to current north reading.
                                    phi = (int) ((theta / 90.0) * (REL_EAST - REL_NORTH)) + REL_NORTH;
                                }
   257   258   259   260   261   262   263   264   265   266   267