Advertisement
Guest User

Untitled

a guest
Jun 23rd, 2013
66
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 4.04 KB | None | 0 0
  1. void calibrate_doCalibration()
  2. {
  3. Serial.println("Doing calibration.");
  4. Serial.print("machineWidth: "); Serial.print(machineWidth);
  5. Serial.print(",stepsPerMM: "); Serial.print(stepsPerMM);
  6. Serial.print(",pageWidth: "); Serial.println(pageWidth);
  7.  
  8. penlift_penUp();
  9. releaseMotors();
  10. motorA.enableOutputs();
  11. motorA.setCurrentPosition(0);
  12.  
  13. // get a baseline from the endstop detector in case it's already at the stop
  14. byte endStopSignal = digitalRead(ENDSTOP_X_MIN);
  15. Serial.print("Reading X Endstop:");
  16. Serial.println(endStopSignal);
  17.  
  18. motorA.setAcceleration(10000);
  19. if (endStopSignal == 0)
  20. {
  21. // it's already there! God what a mess!
  22. // what we'll do is wind forwards until it changes back
  23. Serial.println("Already at X Endstop, winding motorA forward");
  24. while (endStopSignal == 0)
  25. {
  26. motorA.move(stepMultiplier);
  27. while (motorA.distanceToGo() != 0)
  28. motorA.run();
  29. endStopSignal = digitalRead(ENDSTOP_X_MIN);
  30. Serial.println(endStopSignal);
  31. }
  32. Serial.println("motorA: at endstop, jumping a bit more");
  33. motorA.move(stepMultiplier * motorStepsPerRev); // then jump a bit more - one rev
  34. while (motorA.distanceToGo() != 0) motorA.run();
  35. }
  36. delay(400);
  37.  
  38. Serial.println("motorA: wind backwards until we hit endstop X");
  39. // so wind backwards until hitting the stop.
  40. while (endStopSignal != 0)
  41. {
  42. motorA.move(-1);
  43. while (motorA.distanceToGo() != 0)
  44. motorA.run();
  45. endStopSignal = digitalRead(ENDSTOP_X_MIN);
  46. }
  47. Serial.print("motorA: at endstop X, saving position for motorA, resting at ");
  48. motorARestPoint = abs(motorA.currentPosition()) + (ENDSTOP_X_MIN_POSITION * stepsPerMM);
  49. Serial.println(motorARestPoint);
  50. Serial.print(", current position ");
  51. motorA.setCurrentPosition(ENDSTOP_X_MIN_POSITION * stepsPerMM);
  52. Serial.println(motorA.currentPosition());
  53.  
  54. delay(1000);
  55. reportPosition();
  56.  
  57. //Serial.print("moving motorA to ");
  58. //Serial.println(pageWidth/20);
  59. //motorA.moveTo(pageWidth/20);
  60. //motorA.setAcceleration(currentAcceleration);
  61. //while (motorA.distanceToGo() != 0)
  62. // motorA.run();
  63.  
  64. Serial.println("repeating process for motorB, moving motorA with it");
  65. // now do above for motorB
  66. motorB.enableOutputs();
  67. motorB.setCurrentPosition(0);
  68. motorB.setAcceleration(10000);
  69.  
  70. // get a baseline from the endstop detector in case it's already at the stop
  71. endStopSignal = digitalRead(ENDSTOP_Y_MIN);
  72.  
  73. Serial.println("motorB: wind backwards until we hit endstop Y, move motorA with it");
  74. motorA.setAcceleration(10000); // make it slightly faster to allow for slack
  75. motorA.setSpeed(motorA.speed());
  76. motorB.setSpeed(motorB.speed()*.99);
  77.  
  78. delay(400);
  79. while (endStopSignal != 0)
  80. {
  81. motorB.move(-1);
  82. motorA.move(1);
  83. while (motorB.distanceToGo() != 0) {
  84. motorB.run();
  85. motorA.run();
  86. }
  87. endStopSignal = digitalRead(ENDSTOP_Y_MIN);
  88. }
  89. Serial.print("motorB: at endstop Y, saving position for motorB, resting at ");
  90. motorBRestPoint = abs(motorB.currentPosition()) + (ENDSTOP_Y_MIN_POSITION * stepsPerMM);
  91. Serial.print(motorBRestPoint);
  92. Serial.print(", current position ");
  93. motorB.setCurrentPosition(ENDSTOP_Y_MIN_POSITION * stepsPerMM);
  94. Serial.println(motorB.currentPosition());
  95.  
  96. reportPosition();
  97.  
  98. // now return to the place where you started.
  99. Serial.print("move motorA and motorB to ");
  100. Serial.print(motorARestPoint);
  101. Serial.print(",");
  102. Serial.println(motorBRestPoint);
  103.  
  104. motorB.setSpeed(motorA.speed());
  105. motorA.setMaxSpeed(currentMaxSpeed);
  106. motorB.setMaxSpeed(currentMaxSpeed);
  107. motorA.setAcceleration(currentAcceleration);
  108. motorB.setAcceleration(currentAcceleration);
  109.  
  110.  
  111. motorA.moveTo(motorARestPoint);
  112. motorB.moveTo(motorARestPoint);
  113.  
  114. while (motorA.distanceToGo() != 0 || motorB.distanceToGo() != 0)
  115. {
  116. motorA.run();
  117. motorB.run();
  118. }
  119.  
  120. Serial.println("motors at final position, calibrated");
  121. reportPosition();
  122. powerIsOn = true;
  123. isCalibrated = true;
  124.  
  125. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement