Skip to content. | Skip to navigation

Personal tools

adc.cpp

adc.cpp

adc.cpp

/////////////////////////////////////////////////////////////////////////////
// ADC.CPP
//
//  Last updated: 6/29/2006
//
//  Part of the source code for the M3 Program
//
//  Contains routines for motion and status of the ADC Rotators
/////////////////////////////////////////////////////////////////////////////

void ADCInterrupt()
{
  CADC* ADC;

  ADC = &ADCA;

  // Do ADCA and ADCB
  while (ADC != NULL)
  {
    do      // Use a do loop to allow easy breaks on errors
    {
      // Do we need to do an emergency stop?
      if (ADC->abortmove)
      {
        // Stop as soon as our motion profile allows it
        if (ADC->movesteps > ADC->profileindex)
          ADC->movesteps = ADC->profileindex;
        // Jump to the necessary deceleration point in the motion profile
        ADC->profileindex = ADC->movesteps;
        // Don't go faster than max velocity
        if (ADC->profileindex >= ADC->MaxProfile)
          ADC->profileindex = ADC->MaxProfile - 1;
        ADC->pulsecount = 1;

        // Remember why we aborted
        ADC->tempresult = ADC->abortmove;
        ADC->abortmove = 0;

        // Stop now if movesteps is already 0
        if (ADC->movesteps == 0)
        {
          ADC->moveresult = ADC->tempresult;
          ADC->Motion = ADCMStop;
          break;
        }
      }

      // Should we be moving things?
      if (ADC->movesteps > 0)
      {
        // On the first loop for the current step, set the step bit
        if (ADC->pulsecount == 1)
        {
          ADC->pulsecount++;
          SetDigBit(ADC->StpBit);
        }
        // Delay until pulse needs to go back low (halfway through current step)
        else if (ADC->pulsecount == ADC->Profile[ADC->profileindex] >> 1)
        {
          ADC->pulsecount++;
          ClrDigBit(ADC->StpBit);
        }
        // At the end of the current step, update the motion profile,
        // and motor phase tracking
        else if (ADC->pulsecount == ADC->Profile[ADC->profileindex])
        {
          // Get ready for next step
          ADC->pulsecount = 1;
          ADC->movesteps--;
          
          if (ADC->Motion == ADCMForward)
            ADC->position++;
          else if (ADC->Motion == ADCMReverse)
            ADC->position--;
          
          // Are we done?
          if (ADC->movesteps == 0)
          {
            ADC->moveresult = ADC->tempresult;
            ADC->Motion = ADCMStop;
            break;
          }

          // Accelerate
          if (ADC->profileindex < ADC->movesteps)
            ADC->profileindex++;
          // Decelerate or maintain max speed (depending on steps left)
          else
            ADC->profileindex = ADC->movesteps;
          // Update recorded motor phase
          if (GetDigBit(ADC->MonBit))
            ADC->MonStep = 0;
          else
          {
            ADC->MonStep += ADC->direction;
            if (ADC->MonStep > 9)
              ADC->MonStep -= 10;
            if (ADC->MonStep < 0)
              ADC->MonStep += 10;
          }
        }
        // Keep looping for current step
        else
          ADC->pulsecount++;

        // Don't exceed maximum velocity
        if (ADC->profileindex >= ADC->MaxProfile)
          ADC->profileindex = ADC->MaxProfile - 1;

        // Read index
        ADC->Index = GetDigBit(ADC->IndexBit);

        if (ADC->MoveType == ADCMTHome)
        {
          // Count steps if we are homing, to check for lost steps later
          if ((ADC->pulsecount == 1) && ADC->movesteps)
            ADC->positionbeforeindex++;
          // Check to see if we hit an index in the proper direction
          // to count as being "homed".  The homing direction
          // is clockwise for the outer ADC, and counterclockwise
          // for the inner ADC.  This prevents position errors due to
          // the tensioning springs.
          if (ADC->Index && !ADC->indexflag)
          {
            if (ADC->Motion == ADCMReverse)
            {
              ADC->Homed = 1;
              // Note the position in steps before the index was active
              // This debug output is used to find the total number of steps
              // for a rotation, and the index width
              //Log->Add("Pos Before Index: %ld  Steps Left: %ld",BRIEF,ADC->position,ADC->movesteps);
              ADC->adchomereadsteps = ADC->positionbeforeindex;
              ADC->position = 0;
            }
            // If we're supposed to stop right after we hit the index (like
            // when we're homing), do so.
            if (ADC->submove == ADCSStopOnIndex)
              ADC->movesteps = ADC->profileindex;
          }
          // Note the position in steps after the index went inactive
          //else if (!ADC->Index && ADC->indexflag)
          //  Log->Add("Pos After Index: %ld  Steps Left: %ld",2,ADC->position,ADC->movesteps);
        }
        ADC->indexflag = ADC->Index;
      }
    } while (0);
    // Loop through ADCs
    if (ADC == &ADCA)
      ADC = &ADCB;
    else
      ADC = NULL;
  }
}

/////////////////////////////////////////////////////////////////////////////
// CADC::Init - Initialize ADC object
void CADC::Init(int dirBit,int stpBit,int monBit,
                int indexBit,int y1,int y2,int cw,int r1,int r2,float indexAngle)
// dirBit,stpBit,monBit, and indexBit are the associated digital I/O bits for
// this ADC's stepper.
// y1 and y2 are the on-screen y coords for this ADC's index and stepper
// displays.
// cw is a 1 if a forward step gives clockwise motion for this ADC.
// r1 and r2 are the inner and outer radii of the cartoon index mark for this
// ADC.
// indexAngle is the angle in degrees to the fixed index point for this ADC,
// with 0 being along the screen's +X access and counting up clockwise.
{
  int i;
  int minpulse;                   // Minimum number of FastestSteps for the given max velocity
  double total;                   // Used for motion profile calculations
  double error;                         // Used for motion profile calculations (cumulative error)
  double err,errp,errm; // Test errors for small changes in a Profile entry
  double Vt;                                    // Target velocity
  double V,Vp,Vm;                       // Newly calculated velocities
  int Done = 0;                         // Flag for completion of motion profile

  // Copy all fixed data
  DirBit = dirBit;
  StpBit = stpBit;
  MonBit = monBit;
  IndexBit = indexBit;
  Y1 = y1;
  Y2 = y2;
  CW = cw;
  R1 = r1;
  R2 = r2;
  IndexAngle = indexAngle;

  // Allocate an array for the stepper motion profile
  Profile = (int *)malloc(MotionProfile * sizeof(int));
  FastestStep = 1.0 / RealClockRate; // Shortest pulse width in seconds

  // Initialize all status variables
  Motion = ADCMStop;
  OldMotion = ADCMStop;
  Moving = 0;
  MoveType = ADCMTNone;
  Position = 0;
  ADCHomeExpectedSteps = 0;
  Homed = 0;
  OldHomed = -1;
  OldPosition = -1;
  Index = 0;
  OldIndex = -1;
  // Initialize ADCs operation mode
  ADCRotatorsMode = ADCSManual;
  OldADCRotatorsMode = -1;
  Dir = 0;
  Stp = 0;
  Mon = 0;
  Pwr = 0;
  Cur = 0;
  MonStep = 0;

  // Initialize interrupt flags
  abortmove = 0;
  movesteps = 0;
  position = 0;
  positionbeforeindex = 0;
  adchomereadsteps = 0;
  
  // Initialize motion profile
  i = 1;
  // Initial pulse length (Convert from milliseconds to ticks)
  Profile[0] = InitialPulse / 1000.0 / FastestStep;
  // total = total time in seconds the profile has used so far
  total = Profile[0] * FastestStep;
  // minpulse is the lower limit on our motion profile; when the profile
  // pulse width gets shorter than minpulse, we're at our maximum velocity.
  minpulse = 1.0 / (FastestStep * MaxVelocity);
  // minpulse must be at least 2 to toggle the stepper bit on and then off.
  if (minpulse < 2) minpulse = 2;

  Done = 0;
  error = 0;
  while (!Done)
  {
    // Calculate stepper acceleration pulse profile
    Vt = Acceleration * total;
    Profile[i] = 1.0 / (Acceleration * total * FastestStep);
    // Find possible velocities near our target velocity
    V = round(1.0 / (Profile[i] * FastestStep));
    Vp = round(1.0 / ((Profile[i]+1) * FastestStep));
    Vm = round(1.0 / ((Profile[i]-1) * FastestStep));
    // Dither velocities if necessary for a smooth ramp-up
    if (i > 1)
    {
      // Find new velocity that minimizes accumulated error
      err = fabs(error + (V - Vt));
      errp = fabs(error + (Vp - Vt));
      errm = fabs(error + (Vm - Vt));
      if ((errp < err) && (errp < errm))
        Profile[i]++;
      else if ((errm < err) && (errm < errp))
        Profile[i]--;

      V = 1.0 / (Profile[i] * FastestStep);
    }
    if (Profile[i] < minpulse)
    {
      Profile[i] = minpulse;
      Done = 1;
    }
    // Add to total time for next calculation
    total += Profile[i] * FastestStep;
    // Keep track of total error
    error += V - Vt;
    i++;
    if (i == MotionProfile)
      Done = 1;
  }
  // When accelerating a stepper, go from Profile[0] .. Profile[MaxProfile-1]
  MaxProfile = i;
}

/////////////////////////////////////////////////////////////////////////////
// CADC::Update - Update all motion and status
void CADC::Update()
{
  // Save old status values, and display on-screen only if they change
  int OldDir = Dir,OldStp = Stp,OldMon = Mon,OldPwr = Pwr,OldCur = Cur;

  // Read index
  Index = GetDigBit(IndexBit);
  // Read stepper bits
  Dir = GetDigBit(DirBit);
  Stp = GetDigBit(StpBit);
  Mon = GetDigBit(MonBit);
  Pwr = !GetParBit(4);
  Cur = GetParBit(3);

  // Update any high level motion in progress
  // (a MoveSteps, Home, or MovePosition)
  if (Moving)
  {
    switch (MoveType)
    {
      case ADCMTSteps:
      {
        switch (MoveStep)
        {
          // MoveSteps: Wait for stepper power to come on
          case 0:
          {
            if (msTimer - ADCStepperTime >= 1000)
            {
              ADCStepperPowerOnStage2();
              MoveStep = 1;
            }
          } break;
          // MoveSteps: Begin move
          case 1:
          {
            subMoveSteps(tempsteps,0);
            MoveStep = 2;
          } break;
          // MoveSteps: Wait for move to complete
          case 2:
          {
            if (moveresult >= 0)
            {
              MoveType = ADCMTNone;
              Moving = 0;
              // Keep power on
              //ADCStepperPowerOff(1);
              if (moveresult > 0)
                EDSLog.Add(40,"ADC Rotator: User interrupted motion");
            }
          } break;
        }
      } break; // End of ADCMTSteps case
      case ADCMTPosition:
      {
        switch (MoveStep)
        {
          // MovePosition: Wait for stepper power to come on
          case 0:
          {
            if (msTimer - ADCStepperTime >= 1000)
            {
              ADCStepperPowerOnStage2();
              MoveStep = 1;
            }
          } break;
          // MovePosition: Begin move
          case 1:
          {
            subMoveSteps(tempsteps,0);
            MoveStep = 2;
          } break;
          // MovePosition: Wait for move to complete
          case 2:
          {
            if (moveresult >= 0)
            {
              MoveType = ADCMTNone;
              Moving = 0;
              // Keep power on
              //ADCStepperPowerOff(1);
              if (moveresult > 0)
                EDSLog.Add(40,"ADC Rotator: User interrupted motion");
            }
          } break;
        }
      } break; // End of ADCMTPosition case
      case ADCMTHome:
      {
        switch (MoveStep)
        {
          // Home: Wait for stepper power to come on
          case 0:
          {
            if (msTimer - ADCStepperTime >= 1000)
            {
              ADCStepperPowerOnStage2();
              MoveStep = 1;
            }
          } break;
          // Home: Are we on an index already?
          case 1:
          {
            if (Index != 0)
            {
              // We're already on an index, move off a bit and back on
              subMoveSteps(ADCIndexMoveOff,0);
              //subMoveSteps(-ADCIndexMoveOff*sign,0);
              MoveStep = 2;
            }
            else
            {
              // Run to the index
              subMoveSteps(-ADCMinDistance,1);
              //subMoveSteps(ADCMinDistance*sign,1);
              MoveStep = 3;
            }
          } break;
          // Home: Wait to get off of index
          case 2:
          {
            if (moveresult >= 0)
            {
              if (moveresult > 0)
              {
                EDSLog.Add(40,"ADC Rotator: User interrupted motion");
                MoveType = ADCMTNone;
                // Keep power on
                //ADCStepperPowerOff(1);
                Moving = 0;
                break;
              }
              MoveStep = 3;
              subMoveSteps(-4000,1);
              //subMoveSteps(4000*sign,1);
            }
          } break;
          // Home: Wait to pass index
          case 3:
          {
            if (moveresult >= 0)
            {
              if (moveresult > 0)
                EDSLog.Add(40,"ADC Rotator: User interrupted motion");
              else if (!Homed)
                EDSLog.Add(41,"ADC Rotator: Couldn't find index");
              else if (Homed)
              {
                EDSLog.Add(941,"ADC Rotator Homed");
                EDSLog.Add(942,"Steps taken %ld, expecting %ld",abs(adchomereadsteps),ADCHomeExpectedSteps);
              }
              // Done with the Home
              MoveType = ADCMTNone;
              // Keep power on
              //ADCStepperPowerOff(1);
              Moving = 0;
            }
          } break;
        }
      } break; // End of ADCMTHome case
    }
  }

  // Keep our position in 0-StepsPerRev steps (0-360 degrees)
  Position = position % StepsPerRevolution;
  if (Position < 0)
    Position += StepsPerRevolution;

  // Display anything that has changed
  if (OldMotion != Motion)
    DisplayMotion();
  if (OldIndex != Index)
    DisplayIndex();
  if ((OldPosition != Position) || (OldHomed != Homed))
  {
    DisplayPosition();
    Log->Add("CADC::Update(%d): Position (steps): %d",VERBOSE,__LINE__,Position);
  }
  if ((OldDir != Dir) || (OldStp != Stp) || (OldMon != Mon) ||
      (OldPwr != Pwr) || (OldCur != Cur))
    DisplayStepper();
  if (OldADCRotatorsMode != ADCRotatorsMode)
    DisplayMode();

  // Record displayed values
  OldPosition = Position;
  OldHomed = Homed;
  OldMotion = Motion;
  OldIndex = Index;
  OldADCRotatorsMode = ADCRotatorsMode;
}

/////////////////////////////////////////////////////////////////////////////
// CADC::DisplayAll - Redraw all status info on-screen
void CADC::DisplayAll()
{
  DisplayMotion();
  DisplayIndex();
  DisplayPosition();
  DisplayMode();
}

/////////////////////////////////////////////////////////////////////////////
// CADC::DisplayMotion - Display motion status bits on-screen
void CADC::DisplayMotion()
{
        int x = 148,y = Y1;

        switch (Motion)
        {
                case ADCMStop:
                        PutChar16(CLeftRight,x,y,BLACK,SHECMEDGRAY);break;
                case ADCMForward:
                        PutChar16(CRight,x,y,SHECGREEN,SHECMEDGRAY);break;
                case ADCMReverse:
                        PutChar16(CLeft,x,y,SHECGREEN,SHECMEDGRAY);break;
                default:
                        PutChar16(CLeftRight,x,y,SHECRED,SHECMEDGRAY);break;
        }
}

/////////////////////////////////////////////////////////////////////////////
// CADC::DisplayIndex - Display index status bit on-screen
void CADC::DisplayIndex()
{
        char idxchar;

        if (CW)
                idxchar = 'A';
        else
                idxchar = 'B';
        if (Index)
                PutChar16(idxchar,228,Y1,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16(idxchar,228,Y1,BLACK,SHECMEDGRAY);
}

/////////////////////////////////////////////////////////////////////////////
// CADC::DisplayPosition - Display ADC lenses position on-screen
void CADC::DisplayPosition()
{
  char s[100];
  double deg,min;
  int color;

  if (!Homed)
  {
    color = SHECRED;
    PutString16("Unknown",260,340,Y1,color,SHECMEDGRAY);
    min = modf(double(360.0 * OldPosition / StepsPerRevolution),&deg);
    DrawCartoon(deg,SHECMEDGRAY);
    min = modf(double(360.0 * Position / StepsPerRevolution),&deg);
    DrawCartoon(deg,BLACK);
  }
  else
  {
    if (ADCRotatorsMode == ADCSAuto)
      color = BLACK;
    else
      color = SHECGREEN;
    min = modf(double(360.0 * OldPosition / StepsPerRevolution),&deg);
    DrawCartoon(deg,SHECMEDGRAY);
    min = modf(double(360.0 * Position / StepsPerRevolution),&deg);
    sprintf(s,"%3.0lfø %02.0lf'",deg,min*60.0);
    PutString16(s,260,340,Y1,color,SHECMEDGRAY);
    DrawCartoon(deg,BLACK);
  }
}

/////////////////////////////////////////////////////////////////////////////
// CADC::DisplayMode - Display ADC lenses motion mode on-screen
void CADC::DisplayMode()
{
  char s[100];
  int color;

  if (ADCRotatorsMode == ADCSAuto)
    PutString16("auto)",195-ADCBR2-1-5+32,195-ADCBR2-1-5+72,10,BLACK,SHECMEDGRAY);
  else
    PutString16("man )",195-ADCBR2-1-5+32,195-ADCBR2-1-5+72,10,BLACK,SHECMEDGRAY);
}
        
/////////////////////////////////////////////////////////////////////////////
// CADC::DisplayStepper - Display all stepper status bits
void CADC::DisplayStepper()
{
        if (Dir)
                PutChar16('þ',468,Y2,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',468,Y2,BLACK,SHECMEDGRAY);
        if (Stp)
                PutChar16('þ',500,Y2,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',500,Y2,BLACK,SHECMEDGRAY);
        if (Mon)
                PutChar16('þ',532,Y2,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',532,Y2,BLACK,SHECMEDGRAY);
        if (Pwr)
                PutChar16('þ',564,Y2,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',564,Y2,BLACK,SHECMEDGRAY);
        if (Cur)
                PutChar16('þ',596,Y2,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',596,Y2,BLACK,SHECMEDGRAY);
}

/////////////////////////////////////////////////////////////////////////////
// CADC::DrawCartoon - Draw the ADC cartoon at given Angle(degrees) in Color
void CADC::DrawCartoon(int Angle,int Color)
{
  int x,y,xoff,yoff;

  x = ADCX;
  y = ADCY;
  if (!CW)
    Angle = -Angle;
  for (xoff=-1;xoff<=1;xoff++)
    for (yoff=-1;yoff<=1;yoff++)
      Line(x+xoff+R1*cos((Angle+IndexAngle)*PI/180.0),y+yoff+R1*sin((Angle+IndexAngle)*PI/180.0),
           x+xoff+R2*cos((Angle+IndexAngle)*PI/180.0),y+yoff+R2*sin((Angle+IndexAngle)*PI/180.0),
           Color);
}

/////////////////////////////////////////////////////////////////////////////
// CADC::Home - Move the ADC rotator until we hit an index
int CADC::Home()
// Returns a 0 on success, 1 if a move is already in progress,
// 2 if we need to switch to manual.
{

  if (Moving)
  {
    EDSLog.Add(43,"ADC Rotator Home ignored, move in progress");
    return(1);
  }
  if (ADCRotatorsMode == ADCSAuto)
  {
    EDSLog.Add(44,"ADC Rotator Home ignored, motion in Automatic");
    return(2);
  }

  // Not necessary
  // The inner ADC has CW set to 1, but we want to home it counter-clockwise
  // to avoid errors from spring backlash, so set our motion sign to -1.
  //if (CW)
  //  sign = -1;
  // The outer ADC has CW set to 0, but we want to home it clockwise
  // to avoid errors from spring backlash, so set our motion sign to 1.
  //else
  //  sign = 1;

  // Reset homed
  Homed = 0;

  // Tell everyone we're moving
  Moving = 1;
  MoveType = ADCMTHome;
  if (ADCStepperPowerOn())
  {
    MoveStep = 0;
    ADCStepperTime = msTimer;
  }
  else
    MoveStep = 1;

  // Store the counts the software thinks are left to trigger the home sensor
  // If we're at the index mark, position = 0, so 
  // the steps taken should be twice ADCIndexMoveOff (see homing sequence)
  if (Index)
    ADCHomeExpectedSteps = 2 * ADCIndexMoveOff;
  else
    ADCHomeExpectedSteps = Position;

  positionbeforeindex = 0;

  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// CADC::MoveSteps - Move the turret the given number of steps
int CADC::MoveSteps(int steps, int StopOnIndex)
// Returns a 0 on success, 1 if a move is already in progress
// Steps is positive for forward motion and negative for reverse motion
// If StopOnIndex == 1, stop after hitting an index
{
  // Don't move if steps is 0
  if (steps == 0)
    return(0);

  if (Moving)
  {
    EDSLog.Add(43,"ADC Rotator move ignored, move in progress");
    return(1);
  }

  // Tell everyone we're moving
  Moving = 1;
  MoveType = ADCMTSteps;
  if (ADCStepperPowerOn())
  {
    MoveStep = 0;
    ADCStepperTime = msTimer;
  }
  else
    MoveStep = 1;
  tempsteps = steps;
  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// CADC::MovePosition - Move to the give position (in degrees from index)
int CADC::MovePosition(double targetposition)
// Returns a 0 on success, 1 if a move is already in progress,
// 2 if we need to home first
// targetposition is an angle in degrees, measured from the index.
// (Clockwise = positive)
{
  int newtarget;  // Used to calculate new target position in steps
  int steps;

  if (Moving)
  {
    EDSLog.Add(43,"ADC Rotator move ignored, move in progress");
    return(1);
  }

  if (!Homed)
  {
    EDSLog.Add(42,"ADC Rotator motion ignored, Home required");
    // We need to home
    return(2);
  }

  // Convert to steps
  newtarget = StepsPerRevolution * targetposition / 360.0;
  // Keep our target between 0 - StepsPerRevolution steps
  newtarget = newtarget % StepsPerRevolution;
  if (newtarget < 0)
    newtarget += StepsPerRevolution;

  // We're already at the commanded position
  if (Position == newtarget)
    return(0);

  steps = newtarget - Position;
  
  // Don't go the long way around
  if (abs(steps) > (StepsPerRevolution / 2))
  {
    if (steps > 0)
      steps -= StepsPerRevolution;
    else
      steps += StepsPerRevolution;
  }

  // Tell everyone we're moving
  Moving = 1;
  MoveType = ADCMTPosition;
  if (ADCStepperPowerOn())
  {
    MoveStep = 0;
    ADCStepperTime = msTimer;
  }
  else
    MoveStep = 1;
  tempsteps = steps;
  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// CADC::AbortMove - Abort any move in progress
void CADC::AbortMove()
{
  // Tell ADCInterrupt to stop anything in progress
  abortmove = 1;
}

/////////////////////////////////////////////////////////////////////////////
// CADC::subMoveSteps - Move the turret the given number of steps
void CADC::subMoveSteps(int steps, int StopOnIndex)
// Steps is positive for forward motion and negative for reverse motion
// If StopOnIndex == 1, stop after hitting an index
{
  // Forward motion
  if (steps > 0)
  {
    Motion = ADCMForward;
    if (CW)
    {
      ClrDigBit(DirBit);
      direction = -1;
    }
    else
    {
      SetDigBit(DirBit);
      direction = 1;
    }
  }
  // Reverse motion
  else
  {
    Motion = ADCMReverse;
    if (CW)
    {
      SetDigBit(DirBit);
      direction = 1;
    }
    else
    {
      ClrDigBit(DirBit);
      direction = -1;
    }
  }
  // Tell interrupt routine to start moving
  moveresult = -1;                                      // This will change on move completion
  tempresult = ADCRSuccess;       // Return code for success
  abortmove = 0;                                                // Clear abort flag
  pulsecount = 1;
  profileindex = 0;
  indexflag = Index;
  // Tell interrupt what kind of motion we want
  if (StopOnIndex)
    submove = ADCSStopOnIndex;
  else
    submove = ADCSSteps;
  movesteps = abs(steps);         // Interrupt starts going when it sees
                                  // movesteps != 0
}

/////////////////////////////////////////////////////////////////////////////
// MovePositionCombined - Move both ADCs to the given position (in degrees)
int MovePositionCombined(double targetposition)
// Returns a 0 on success, 1 if a move is already in progress,
// 2 if we need to home first
// targetposition is an angle in degrees, measured from the index.
// ADCA goes to +targetposition, ADCB to -targetposition
{

  if (ADCA.Moving || ADCB.Moving)
  {
    EDSLog.Add(43,"ADC Rotator move ignored, move in progress");
    return(1);
  }
  if (!ADCA.Homed || !ADCB.Homed)
  {
    EDSLog.Add(42,"ADC Rotator motion ignored, Home required");
    // We need to home
    return(2);
  }

  ADCA.MovePosition(targetposition);
  ADCB.MovePosition(targetposition);
  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// ADCRotatorsAngle - Calculate ADC rotators angle as a function of elevation angle
// After the angle is obtained, both ADCs are commanded to rotate: 
// ADCA to + and ADCB to - the angle. The formula was provided by Matt
int ADCRotatorsAngle()
// Returns a 0 on success
// Elevation is coming from TCS through message J5.
// Constants:
// Dispersion Function (arcsec) = dispfunc = 1.915834711;
// ADC Dispersion (arcsec) = adcfunc = 2.50287440585;
// adcfunc/dispfunc = 0.7654537944;
{
  double ADCAngle,K1;

  // Don't do anything if not in auto mode or ADC Turret is not at the ADC position
  if ((ADCRotatorsMode == ADCSManual) || (ADCTurret->Position != ATPIndex))
    return(0);

  // Don't move if previous motion has not finished yet
  if (ADCA.Moving || ADCB.Moving)
    return(0);

  K1 = 180.0/PI;

  // Check elevation angle limits
  if (telel < 37.5)
    ADCAngle = 90.0;
  else if (telel > 90.0)
    ADCAngle = 0.0;
  else
    ADCAngle = K1 * asin(0.7654537944 * tan((90.0 - telel)/K1));
  
  MovePositionCombined(ADCAngle);
  Log->Add("telel = %lf",VERBOSE,telel);
  
  return(0);
}

Generated by GNU Enscript 1.6.5.2.
Document Actions