Main Page | Packages | Class Hierarchy | Class List | Directories | File List | Namespace Members | Class Members | File Members | Related Pages

Handler.java

Go to the documentation of this file.
00001 /*
00002  * Handler.java
00003  *
00004  * Copyright (C) 2005 Project SQUID, http://www.cs.helsinki.fi/group/squid/
00005  *
00006  * This file is part of Ikayaki.
00007  *
00008  * Ikayaki is free software; you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation; either version 2 of the License, or
00011  * (at your option) any later version.
00012  *
00013  * Ikayaki is distributed in the hope that it will be useful,
00014  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00016  * GNU General Public License for more details.
00017  *
00018  * You should have received a copy of the GNU General Public License
00019  * along with Ikayaki; if not, write to the Free Software
00020  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00021  */
00022 
00023 package ikayaki.squid;
00024 
00025 import ikayaki.Settings;
00026 import ikayaki.util.LastExecutor;
00027 
00028 import java.util.concurrent.SynchronousQueue;
00029 import java.util.concurrent.TimeUnit;
00030 
00036 public class Handler implements SerialIOListener {
00037 
00041     private SynchronousQueue<String> answerQueue = new SynchronousQueue<String>();
00042 
00047     private LastExecutor workQueue = new LastExecutor(0, false);
00048 
00052     private final int POLL_TIMEOUT = 60;
00053 
00057     protected SerialIO serialIO;
00058 
00062     private int ACCELERATION;
00063 
00067     private int DECELERATION;
00068 
00076     private int VELOCITY;
00077 
00081     private int MEASUREMENT_VELOCITY;
00082 
00086     private int SAMPLE_LOAD_POSITION;
00087 
00091     private int TRANSVERSE_YAF_POSITION;
00092 
00096     private int AXIAL_AF_POSITION;
00097 
00101     private int BACKGROUND_POSITION;
00102 
00106     private int MEASUREMENT_POSITION;
00107 
00111     private int ROTATION_VELOCITY;
00112 
00116     private int ROTATION_ACCELERATION;
00117 
00121     private int ROTATION_DECELERATION;
00122 
00123     private int HANDLER_ROTATION;
00124 
00128     private int currentMotor = -1;
00129 
00134     private int currentPosition = 0;
00135 
00139     private int currentRotation = 0;
00140 
00144     private int currentVelocity = 0;
00145 
00149     private int estimatedPositionStart = 0;
00150 
00154     private long estimatedPositionStartTime = 0;
00155 
00156     private int estimatedPositionEnd = 0;
00157 
00158     private int estimatedRotationStart = 0;
00159     private long estimatedRotationStartTime = 0;
00160     private int estimatedRotationEnd = 0;
00161 
00165     private boolean waitingForMessage = false;
00166 
00171     protected Handler() throws SerialIOException {
00172         serialIO = SerialIO.openPort(new SerialParameters(Settings.getHandlerPort(), 1200, 0, 0, 8, 1, 0));
00173         serialIO.addSerialIOListener(this);
00174         updateSettings();
00175     }
00176 
00180     protected void setUp() throws SerialIOException {
00181         // put the system online
00182         setOnline();
00183 
00184         // seek the home position, so we can know where we are
00185         seekHome();
00186     }
00187 
00192     protected void updateSettings() {
00193         ACCELERATION = Settings.getHandlerAcceleration();
00194         DECELERATION = Settings.getHandlerDeceleration();
00195         AXIAL_AF_POSITION = Settings.getHandlerAxialAFPosition();
00196         BACKGROUND_POSITION = Settings.getHandlerBackgroundPosition();
00197         SAMPLE_LOAD_POSITION = Settings.getHandlerSampleLoadPosition();
00198         MEASUREMENT_POSITION = Settings.getHandlerMeasurementPosition();
00199         MEASUREMENT_VELOCITY = Settings.getHandlerMeasurementVelocity();
00200         TRANSVERSE_YAF_POSITION = Settings.getHandlerTransverseYAFPosition();
00201         ROTATION_VELOCITY = Settings.getHandlerRotationVelocity();
00202         ROTATION_ACCELERATION = Settings.getHandlerRotationAcceleration();
00203         ROTATION_DECELERATION = Settings.getHandlerRotationDeceleration();
00204         VELOCITY = Settings.getHandlerVelocity();
00205         HANDLER_ROTATION = Settings.getHandlerRotation();
00206     }
00207 
00211     public boolean isMoving() {
00212         return estimatedPositionStart != estimatedPositionEnd;
00213     }
00214 
00218     public boolean isRotating() {
00219         return estimatedRotationStart != estimatedRotationEnd;
00220     }
00221 
00228     public int getPosition() {
00229         return currentPosition;
00230     }
00231 
00237     public int getRotation() {
00238         double angle = (double) (currentRotation) / HANDLER_ROTATION * 360.0;
00239         return (int) (Math.round(angle)) % 360;
00240     }
00241 
00246     protected void setPosition(int position) {
00247         if (currentPosition == Integer.MAX_VALUE || currentPosition == Integer.MIN_VALUE) {
00248             estimatedPositionStart = getEstimatedPosition();
00249         } else {
00250             estimatedPositionStart = currentPosition;
00251         }
00252         estimatedPositionStartTime = System.currentTimeMillis();
00253         currentPosition = position;
00254         estimatedPositionEnd = currentPosition;
00255         System.err.println("Start Move:" +
00256                 " \tstartTime=" + estimatedPositionStartTime +
00257                 " \tstart=" + estimatedPositionStart +
00258                 " \tend=" + estimatedPositionEnd);
00259 
00260     }
00261 
00266     protected void setRotation(int rotationSteps) {
00267         estimatedRotationStart = currentRotation;
00268         estimatedRotationStartTime = System.currentTimeMillis();
00269         currentRotation = rotationSteps;
00270         estimatedRotationEnd = currentRotation;
00271 
00272         // rotations are always in the same direction, even when rotating back to 0
00273         while (estimatedRotationEnd < estimatedRotationStart) {
00274             estimatedRotationEnd += HANDLER_ROTATION;
00275         }
00276         System.err.println("Start Rotate:" +
00277                 " \tstartTime=" + estimatedRotationStartTime +
00278                 " \tstart=" + estimatedRotationStart +
00279                 " \tend=" + estimatedRotationEnd);
00280 
00281     }
00282 
00286     private void fireMovementStopped() {
00287         System.err.println("Stop Move:" +
00288                 " \ttravel Time=" + (System.currentTimeMillis() - estimatedPositionStartTime) +
00289                 " \tstart=" + estimatedPositionStart +
00290                 " \tend=" + estimatedPositionEnd);
00291         if (currentPosition == Integer.MAX_VALUE || currentPosition == Integer.MIN_VALUE) {
00292             int pos = getEstimatedPosition();
00293             estimatedPositionStart = pos;
00294             estimatedPositionEnd = pos;
00295         } else {
00296             estimatedPositionStart = currentPosition;
00297             estimatedPositionEnd = currentPosition;
00298         }
00299 
00300 
00301     }
00302 
00306     private void fireRotationStopped() {
00307         System.err.println("Stop Rotate:" +
00308                 " \ttravel Time=" + (System.currentTimeMillis() - estimatedRotationStartTime) +
00309                 " \tstart=" + estimatedRotationStart +
00310                 " \tend=" + estimatedRotationEnd);
00311         estimatedRotationStart = currentRotation;
00312         estimatedRotationEnd = currentRotation;
00313     }
00314 
00320     public int getEstimatedPosition() {
00321 //        System.err.println("MOVE:" +
00322 //                "\t  isMoving=" + isMoving() +
00323 //                " \tstartTime=" + estimatedPositionStartTime +
00324 //                " \tstart=" + estimatedPositionStart +
00325 //                " \tend=" + estimatedPositionEnd);
00326         if (!isMoving()) {
00327             return estimatedPositionEnd;
00328         }
00329 
00330         // TODO: maybe currentVelocity is not in steps per second?
00331         double timeSpent = (System.currentTimeMillis() - estimatedPositionStartTime) / 1000.0;    // in seconds
00332         int pos = estimatedPositionStart + (int) (currentVelocity * timeSpent);
00333 
00334         // TODO: calculate the acceleration and deceleration corrections
00335         /* acceleration correction */
00336         /*
00337         double accTime = (double) currentVelocity / (double) ACCELERATION;
00338         if (timeSpent > accTime) {
00339             pos -= (1050422 * (accTime * accTime)) / (ACCELERATION);
00340         } else {
00341             pos = estimatedPositionStart + (int) ((1050422 * (timeSpent * timeSpent)) / (double) (ACCELERATION));
00342         }
00343 */
00344 
00345         // we started from PositionStart and if we are already on the other side of PositionEnd, stop at PositionEnd
00346         if ((estimatedPositionStart < estimatedPositionEnd) != (pos < estimatedPositionEnd)) {
00347             return estimatedPositionEnd;
00348         } else {
00349             return pos;
00350         }
00351     }
00352 
00358     public int getEstimatedRotation() {
00359 //        System.err.println("ROTATE:" +
00360 //                "\tisRotating=" + isRotating() +
00361 //                " \tstartTime=" + estimatedRotationStartTime +
00362 //                " \tstart=" + estimatedRotationStart +
00363 //                " \tend=" + estimatedRotationEnd);
00364         if (!isRotating()) {
00365             return getRotation();
00366         }
00367 
00368         // TODO: maybe currentVelocity is not in steps per second?
00369         double timeSpent = (System.currentTimeMillis() - estimatedRotationStartTime) / 1000.0;    // in seconds
00370         int rotation = estimatedRotationStart + (int) ((20 * currentVelocity) * timeSpent);
00371 
00372         // prevent going over the end limit
00373         if (rotation > estimatedRotationEnd) {
00374             rotation = estimatedRotationEnd;
00375         }
00376 
00377         double angle = (double) (rotation) / HANDLER_ROTATION * 360.0;
00378         // no need to calculate acceleration, error minimal
00379         return (int) (Math.round(angle)) % 360;
00380     }
00381 
00387     public boolean isOK() {
00388         return (serialIO != null);
00389     }
00390 
00395     protected void seekHome() throws SerialIOException {
00396 
00397         // seek home position
00398         selectMovement();
00399         if (currentPosition != Integer.MAX_VALUE) {
00400             slewToLimit(true);
00401         }
00402 
00403         setMotorNegative();
00404         setPosition(0);
00405         serialIO.writeMessage("H1,");
00406         waitForMessage();
00407         fireMovementStopped();
00408 
00409         // seek home rotation
00410         selectRotation();
00411         setMotorPositive();
00412         setRotation(0);
00413         serialIO.writeMessage("H1,");
00414         waitForMessage();
00415         fireRotationStopped();
00416     }
00417 
00422     public void moveToSampleLoad() {
00423         workQueue.execute(new Runnable() {
00424             public void run() {
00425                 try {
00426                     moveToPosition(SAMPLE_LOAD_POSITION);
00427                 } catch (SerialIOException e) {
00428                     e.printStackTrace();
00429                 }
00430             }
00431         });
00432     }
00433 
00437     public void moveToDegausserZ() {
00438         workQueue.execute(new Runnable() {
00439             public void run() {
00440                 try {
00441                     moveToPosition(AXIAL_AF_POSITION);
00442                 } catch (SerialIOException e) {
00443                     e.printStackTrace();
00444                 }
00445             }
00446         });
00447     }
00448 
00453     public void moveToDegausserY() {
00454         workQueue.execute(new Runnable() {
00455             public void run() {
00456                 try {
00457                     moveToPosition(TRANSVERSE_YAF_POSITION);
00458                 } catch (SerialIOException e) {
00459                     e.printStackTrace();
00460                 }
00461             }
00462         });
00463     }
00464 
00465 
00469     public void moveToMeasurement() {
00470         workQueue.execute(new Runnable() {
00471             public void run() {
00472                 try {
00473                     moveToPosition(MEASUREMENT_POSITION);
00474                 } catch (SerialIOException e) {
00475                     e.printStackTrace();
00476                 }
00477             }
00478         });
00479     }
00480 
00484     public void moveToBackground() {
00485         workQueue.execute(new Runnable() {
00486             public void run() {
00487                 try {
00488                     moveToPosition(BACKGROUND_POSITION);
00489                 } catch (SerialIOException e) {
00490                     e.printStackTrace();
00491                 }
00492             }
00493         });
00494     }
00495 
00499     public void moveToLeftLimit() {
00500         workQueue.execute(new Runnable() {
00501             public void run() {
00502                 try {
00503                     moveToPosition(Integer.MIN_VALUE);
00504                 } catch (SerialIOException e) {
00505                     e.printStackTrace();
00506                 }
00507             }
00508         });
00509     }
00510 
00514     public void moveToRightLimit() {
00515         workQueue.execute(new Runnable() {
00516             public void run() {
00517                 try {
00518                     moveToPosition(Integer.MAX_VALUE);
00519                 } catch (SerialIOException e) {
00520                     e.printStackTrace();
00521                 }
00522             }
00523         });
00524     }
00525 
00533     protected void moveToPosition(int position) throws SerialIOException {
00534         selectMovement();
00535 
00536         if (position == getPosition()) {
00537             return;                 // do not move if we are already there
00538 
00539         } else if (position == Integer.MIN_VALUE) {
00540             slewToLimit(false);     // slew to left limit
00541 
00542         } else if (position == Integer.MAX_VALUE) {
00543             slewToLimit(true);      // slew to right limit
00544 
00545         } else {
00546 
00547             // if we are at a limit, we must recalibrate the home position
00548             if (getPosition() == Integer.MIN_VALUE || getPosition() == Integer.MAX_VALUE) {
00549                 seekHome();
00550             }
00551 
00552             // obey speed limit and go to the specified position
00553             if (getPosition() == BACKGROUND_POSITION) {
00554 
00555                 // currently in background position - speed depends on where we are going
00556                 // move to the target position and stop there
00557                 if (position > BACKGROUND_POSITION) {
00558                     moveSteps(position - getPosition(), MEASUREMENT_VELOCITY);
00559                 } else {
00560                     moveSteps(position - getPosition(), VELOCITY);
00561                 }
00562 
00563             } else if (getPosition() < BACKGROUND_POSITION) {
00564 
00565                 // currently in fast speed area
00566                 if (position > BACKGROUND_POSITION) {
00567                     // must change the speed at BG position
00568                     moveSteps(BACKGROUND_POSITION - getPosition(), VELOCITY);
00569                     moveToPosition(position);   // continue movement from BG position
00570                 } else {
00571                     // keep the same speed all the way
00572                     moveSteps(position - getPosition(), VELOCITY);
00573                 }
00574 
00575             } else if (getPosition() > BACKGROUND_POSITION) {
00576 
00577                 // currently in slow speed area
00578                 if (position < BACKGROUND_POSITION) {
00579                     // must change the speed at BG position
00580                     moveSteps(BACKGROUND_POSITION - getPosition(), MEASUREMENT_VELOCITY);
00581                     moveToPosition(position);   // continue movement from BG position
00582                 } else {
00583                     // keep the same speed all the way
00584                     moveSteps(position - getPosition(), MEASUREMENT_VELOCITY);
00585                 }
00586 
00587             } else {
00588                 assert false;
00589             }
00590         }
00591     }
00592 
00600     protected void moveSteps(int steps, int velocity) throws SerialIOException {
00601         if (steps < -16777215 || steps > 16777215) {
00602             throw new IllegalArgumentException("steps = " + steps);
00603         }
00604         selectMovement();
00605         setVelocity(velocity);
00606         if (steps >= 0) {
00607             setMotorPositive();
00608         } else {
00609             setMotorNegative();
00610         }
00611 
00612         setPosition(getPosition() + steps);
00613         serialIO.writeMessage("N" + Math.abs(steps));
00614         go();
00615 
00616         waitForMessage();
00617         fireMovementStopped();
00618     }
00619 
00620     protected void slewToLimit(boolean toRight) throws SerialIOException {
00621         setVelocity(VELOCITY);
00622         if (toRight) {
00623             setMotorPositive();
00624             setPosition(Integer.MAX_VALUE);
00625         } else {
00626             setMotorNegative();
00627             setPosition(Integer.MIN_VALUE);
00628         }
00629         performSlew();
00630         waitForMessage();
00631         fireMovementStopped();
00632     }
00633 
00640     public void rotateTo(final int rotationAngle) {
00641         workQueue.execute(new Runnable() {
00642             public void run() {
00643                 int angle = rotationAngle % 360;
00644                 int steps = (int) (((double) angle) / 360.0 * HANDLER_ROTATION);
00645 
00646                 try {
00647                     selectRotation();
00648                     setMotorPositive();
00649 
00650                     // re-seek home always rotating to zero, otherwise use the counter
00651                     if (angle == 0) {
00652                         setRotation(0);
00653                         serialIO.writeMessage("H1,");
00654                     } else {
00655                         int relativeSteps = steps - currentRotation;
00656                         while (relativeSteps < 0) {
00657                             relativeSteps += HANDLER_ROTATION;
00658                         }
00659                         relativeSteps = relativeSteps % HANDLER_ROTATION;
00660 
00661                         setRotation(currentRotation + relativeSteps);
00662                         serialIO.writeMessage("N" + relativeSteps);
00663                         go();
00664                     }
00665                     waitForMessage();
00666                     fireRotationStopped();
00667 
00668                 } catch (SerialIOException e) {
00669                     e.printStackTrace();
00670                 }
00671             }
00672         });
00673     }
00674 
00681     public void join() throws InterruptedException {
00682         workQueue.join();
00683     }
00684 
00688     protected void setOnline() throws SerialIOException {
00689         serialIO.writeMessage("@0,");
00690     }
00691 
00696     protected void selectMovement() throws SerialIOException {
00697         if (currentMotor == 0) {
00698             return;
00699         }
00700         currentMotor = 0;
00701         serialIO.writeMessage("O1,0,");
00702         setVelocity(VELOCITY);
00703         setAcceleration(ACCELERATION);
00704         setDeceleration(DECELERATION);
00705     }
00706 
00711     protected void selectRotation() throws SerialIOException {
00712         if (currentMotor == 1) {
00713             return;
00714         }
00715         currentMotor = 1;
00716         serialIO.writeMessage("O1,1,");
00717         setVelocity(ROTATION_VELOCITY);
00718         setAcceleration(ROTATION_ACCELERATION);
00719         setDeceleration(ROTATION_DECELERATION);
00720     }
00721 
00728     protected void setAcceleration(int acceleration) throws SerialIOException {
00729         if (acceleration < 0 || acceleration > 127) {
00730             throw new IllegalArgumentException("acceleration = " + acceleration);
00731         }
00732         serialIO.writeMessage("A" + acceleration + ",");
00733     }
00734 
00741     protected void setDeceleration(int deceleration) throws SerialIOException {
00742         if (deceleration < 0 || deceleration > 127) {
00743             throw new IllegalArgumentException("deceleration = " + deceleration);
00744         }
00745         serialIO.writeMessage("D" + deceleration + ",");
00746     }
00747 
00754     protected void setVelocity(int velocity) throws SerialIOException {
00755         if (velocity < 50 || velocity > 8500) {
00756             throw new IllegalArgumentException("velocity = " + velocity);
00757         }
00758         serialIO.writeMessage("M" + velocity + ",");
00759         if (currentVelocity < 0) {
00760             currentVelocity = -velocity;
00761         } else {
00762             currentVelocity = velocity;
00763         }
00764     }
00765 
00770     protected void stopExecution() throws SerialIOException {
00771         serialIO.writeMessage("Q,");
00772     }
00773 
00778     protected void performSlew() throws SerialIOException {
00779         selectMovement();
00780         serialIO.writeMessage("S,");
00781     }
00782 
00786     protected void setMotorPositive() throws SerialIOException {
00787         serialIO.writeMessage("+");
00788         currentVelocity = Math.abs(currentVelocity);
00789     }
00790 
00794     protected void setMotorNegative() throws SerialIOException {
00795         serialIO.writeMessage("-");
00796         currentVelocity = -Math.abs(currentVelocity);
00797     }
00798 
00802     protected void go() throws SerialIOException {
00803         serialIO.writeMessage("G,");
00804     }
00805 
00814     protected void waitForMessage() throws SerialIOException {
00815         // blocks all messages for handler
00816         serialIO.writeMessage("F%,");
00817 
00818         // just polls for messages, we might get old messages waiting there? Use take message.
00819         //this.serialIO.writeMessage("%,");
00820         waitingForMessage = true;
00821         try {
00822             answerQueue.take();
00823         } catch (InterruptedException e) {
00824             e.printStackTrace();
00825         }
00826         waitingForMessage = false;
00827     }
00828 
00840     protected String verify(char registry) throws SerialIOException {
00841         serialIO.writeMessage("V" + registry + ",");
00842 
00843         waitingForMessage = true;
00844         String answer = null;
00845         try {
00846             answer = (String) answerQueue.take();
00847         } catch (InterruptedException e) {
00848             e.printStackTrace();
00849         }
00850         waitingForMessage = false;
00851         return answer;
00852     }
00853 
00864     protected char takeMessage() throws SerialIOException {
00865         serialIO.writeMessage("%,");
00866 
00867         waitingForMessage = true;
00868         String answer = null;
00869         try {
00870             answer = (String) answerQueue.poll(POLL_TIMEOUT, TimeUnit.SECONDS);
00871         } catch (InterruptedException e) {
00872             e.printStackTrace();
00873         }
00874         waitingForMessage = false;
00875         return answer.charAt(0);
00876     }
00877 
00878     public void serialIOEvent(SerialIOEvent event) {
00879         String message = event.getCleanMessage();
00880         if (!waitingForMessage) {
00881             System.err.println("Recieved a message that nobody waited for: " + message);
00882             return;
00883         }
00884 
00885         try {
00886             answerQueue.put(message);
00887         } catch (InterruptedException e) {
00888             System.err.println("Interrupted Handler message event");
00889             e.printStackTrace();
00890         } catch (NullPointerException e) {
00891             System.err.println("Null from SerialEvent in Handler");
00892             e.printStackTrace();
00893         }
00894     }
00895 }

Generated on Fri May 6 16:00:33 2005 for Squid by  doxygen 1.4.1