calibrate.c

/**
*  Polargraph Server for ATMEGA1280+
*  Written by Sandy Noble
*  Released under GNU License version 3.
*  http://www.polargraph.co.uk
*  http://code.google.com/p/polargraph/

Specific features for Polarshield / arduino mega.
Calibration.

Experimental calibration routine.  It is designed to work with
limit switches, triggered by the gondola being a fixed (known)
distance from the sprocket.

*/

/* =============================================================================
   Here is the calibration routines
   =============================================================================*/

extern AF_Stepper afMotorB;

void calibrate_doCalibration()
{
  Serial.println("Doing calibration.");
  Serial.println("Lifting Pen.");
  penlift_movePen(isPenUp ? upPosition : downPosition, upPosition, penLiftSpeed);

  Serial.print("machineWidth: "); Serial.print(machineWidth);
  Serial.print(",stepsPerMM: "); Serial.print(stepsPerMM);
  Serial.print(",pageWidth: "); Serial.println(pageWidth);

  motorA.setAcceleration(200000);
  motorB.setAcceleration(200000);

  releaseMotors();
  afMotorB.release();  // release doesn't actually release

  motorA.enableOutputs();
  motorA.setCurrentPosition(0);

  // get a baseline from the endstop detector in case it's already at the stop
  byte endStopSignal = digitalRead(ENDSTOP_X_MIN);
  Serial.print("Reading X Endstop:");
  Serial.println(endStopSignal);

  //motorA.setAcceleration(10000);
  if (endStopSignal == 0)
  {
    // it's already there! God what a mess!
    // what we'll do is wind forwards until it changes back
    Serial.println("Already at X Endstop, winding motorA forward");
    while (endStopSignal == 0)
    {
      motorA.move(stepMultiplier);
      while (motorA.distanceToGo() != 0)
        motorA.run();
      endStopSignal = digitalRead(ENDSTOP_X_MIN);
      Serial.println(endStopSignal);
    }
    Serial.println("motorA: at endstop, jumping a bit more");
    motorA.move(stepMultiplier * motorStepsPerRev);     // then jump a bit more - one rev
    while (motorA.distanceToGo() != 0)  motorA.run();
  }
  delay(400);

  Serial.println("motorA: wind backwards until we hit endstop X");
  // so wind backwards until hitting the stop.
  while (endStopSignal != 0)
  {
    motorA.move(-1);
    while (motorA.distanceToGo() != 0)
      motorA.run();
    endStopSignal = digitalRead(ENDSTOP_X_MIN);
  }
  Serial.print("motorA: at endstop X, saving position for motorA, resting at ");
  motorARestPoint = abs(motorA.currentPosition()) + (ENDSTOP_X_MIN_POSITION * stepsPerMM);
  Serial.println(motorARestPoint);
  Serial.print(", current position ");
  motorA.setCurrentPosition(ENDSTOP_X_MIN_POSITION * stepsPerMM);
  Serial.println(motorA.currentPosition());

  delay(1000);
  reportPosition();

  //Serial.print("moving motorA to ");
  //Serial.println(pageWidth/20);
  //motorA.moveTo(pageWidth/20);
  //motorA.setAcceleration(currentAcceleration);
  //while (motorA.distanceToGo() != 0)
  //  motorA.run();

  Serial.println("repeating process for motorB, moving motorA with it");
  // now do above for motorB
  motorB.enableOutputs();
  motorB.setCurrentPosition(0);
  //motorB.setAcceleration(10000);

  Serial.println("motorB: wind backwards until we hit endstop Y, move motorA with it");
  endStopSignal = digitalRead(ENDSTOP_Y_MIN); // get baseline read from endstop in case we're already there
  delay(400);
  while (endStopSignal != 0)
  {
    motorB.move(-1);
    motorA.move(1);
    while (motorB.distanceToGo() != 0) {
      motorB.run();
      motorA.run();
    }
    endStopSignal = digitalRead(ENDSTOP_Y_MIN);
  }
  Serial.print("motorB: at endstop Y, saving position for motorB, resting at ");
  motorBRestPoint = abs(motorB.currentPosition()) + (ENDSTOP_Y_MIN_POSITION * stepsPerMM);
  Serial.print(motorBRestPoint);
  Serial.print(", current position ");
  motorB.setCurrentPosition(ENDSTOP_Y_MIN_POSITION * stepsPerMM);
  Serial.println(motorB.currentPosition());
  reportPosition();

  //motorA.setMaxSpeed(currentMaxSpeed);
 // motorB.setMaxSpeed(currentMaxSpeed);
  //motorA.setAcceleration(currentAcceleration);
  //motorB.setAcceleration(currentAcceleration);

  float h = 100;
  float fudge = 7.28; // substitude penWidth for debugging
  Serial.print("using fudge: "); Serial.println(fudge);


  int d = sqrt(h*h+((float)machineWidth/2)*((float)machineWidth/2))-fudge ;
  int ds = d*16;

  Serial.print("move motorA and motorB to ");
  Serial.print(d);
  Serial.print(",");
  Serial.println(d);
  Serial.print("->");
  Serial.print(ds);
  Serial.print(",");
  Serial.println(ds);


  motorA.moveTo(ds);
  motorB.moveTo(ds);

  while (motorA.distanceToGo() != 0 || motorB.distanceToGo() != 0)
  {
    motorA.run();
    motorB.run();
  }

  Serial.println("motors at final position, calibrated");
  reportPosition();
  powerIsOn = true;
  isCalibrated = true;
}