00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00182 setOnline();
00183
00184
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
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
00322
00323
00324
00325
00326 if (!isMoving()) {
00327 return estimatedPositionEnd;
00328 }
00329
00330
00331 double timeSpent = (System.currentTimeMillis() - estimatedPositionStartTime) / 1000.0;
00332 int pos = estimatedPositionStart + (int) (currentVelocity * timeSpent);
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346 if ((estimatedPositionStart < estimatedPositionEnd) != (pos < estimatedPositionEnd)) {
00347 return estimatedPositionEnd;
00348 } else {
00349 return pos;
00350 }
00351 }
00352
00358 public int getEstimatedRotation() {
00359
00360
00361
00362
00363
00364 if (!isRotating()) {
00365 return getRotation();
00366 }
00367
00368
00369 double timeSpent = (System.currentTimeMillis() - estimatedRotationStartTime) / 1000.0;
00370 int rotation = estimatedRotationStart + (int) ((20 * currentVelocity) * timeSpent);
00371
00372
00373 if (rotation > estimatedRotationEnd) {
00374 rotation = estimatedRotationEnd;
00375 }
00376
00377 double angle = (double) (rotation) / HANDLER_ROTATION * 360.0;
00378
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
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
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;
00538
00539 } else if (position == Integer.MIN_VALUE) {
00540 slewToLimit(false);
00541
00542 } else if (position == Integer.MAX_VALUE) {
00543 slewToLimit(true);
00544
00545 } else {
00546
00547
00548 if (getPosition() == Integer.MIN_VALUE || getPosition() == Integer.MAX_VALUE) {
00549 seekHome();
00550 }
00551
00552
00553 if (getPosition() == BACKGROUND_POSITION) {
00554
00555
00556
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
00566 if (position > BACKGROUND_POSITION) {
00567
00568 moveSteps(BACKGROUND_POSITION - getPosition(), VELOCITY);
00569 moveToPosition(position);
00570 } else {
00571
00572 moveSteps(position - getPosition(), VELOCITY);
00573 }
00574
00575 } else if (getPosition() > BACKGROUND_POSITION) {
00576
00577
00578 if (position < BACKGROUND_POSITION) {
00579
00580 moveSteps(BACKGROUND_POSITION - getPosition(), MEASUREMENT_VELOCITY);
00581 moveToPosition(position);
00582 } else {
00583
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
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
00816 serialIO.writeMessage("F%,");
00817
00818
00819
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 }