Skip to content. | Skip to navigation

Personal tools

aturret.cpp

aturret.cpp

aturret.cpp

/////////////////////////////////////////////////////////////////////////////
// ATURRET.CPP
//
//  Last updated: 5/3/2007
//
//  Part of the source code for the M3 Program
//
//  Contains routines for motion and status of the ADC (outer) turret
/////////////////////////////////////////////////////////////////////////////

void ADCTurretInterrupt()
{
        // Do we need to do an emergency stop?
        if (ADCTurret->abortmove)
  {
                // Don't go further than the emergency stop distance
                if (ADCTurret->movesteps > ADCTurret->EmergencyStopDistance)
                        ADCTurret->movesteps = ADCTurret->EmergencyStopDistance - 1;
                // Stop even sooner if our motion profile allows it
                if (ADCTurret->movesteps > ADCTurret->profileindex)
                        ADCTurret->movesteps = ADCTurret->profileindex;
                // Jump to the necessary deceleration point in the motion profile
                ADCTurret->profileindex = ADCTurret->movesteps;
                // Don't go faster than max velocity
                if (ADCTurret->profileindex >= ADCTurret->MaxProfile)
                        ADCTurret->profileindex = ADCTurret->MaxProfile - 1;
                ADCTurret->pulsecount = 1;

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

    // Stop now if movesteps is already 0
    if (ADCTurret->movesteps == 0)
    {
            ADCTurret->moveresult = ADCTurret->tempresult;
            return;
    }
  }

        // Should we be moving things?
        if (ADCTurret->movesteps > 0)
  {

          // On the first loop for the current step, set the step bit
          if (ADCTurret->pulsecount == 1)
          {
                  ADCTurret->pulsecount++;
                  SetDigBit(ATBitStep);
          }
          // Delay until pulse needs to go back low (halfway through current step)
          else if (ADCTurret->pulsecount == ADCTurret->Profile[ADCTurret->profileindex] >> 1)
          {
                  ADCTurret->pulsecount++;
                  ClrDigBit(ATBitStep);
          }
          // At the end of the current step, update the motion profile,
          // and motor phase tracking
          else if (ADCTurret->pulsecount == ADCTurret->Profile[ADCTurret->profileindex])
          {
        // Get ready for next step
                  ADCTurret->pulsecount = 1;
                  ADCTurret->movesteps--;

      // Are we done?
      if (ADCTurret->movesteps == 0)
      {
        ADCTurret->moveresult = ADCTurret->tempresult;
        return;
      }

      //we need to continue moving but index limit or reverse is active
      //
      if (!GetDigBit(ATBitIndex) && (ADCTurret->movesteps > 0) && (ADCTurret->Motion == ATMReverse) && (ADCTurret->MoveType == ATMTSteps))
      {
        ADCTurret->abortmove = ATRHitLimitIndex;             // We hit a limit, so abort
        ADCTurret->moveresult = ADCTurret->abortmove;
        return;
      }
      //Reverse Limit
      if (ADCTurret->CheckLimit(ATBitReverse) && (ADCTurret->movesteps > 0) && (ADCTurret->Motion == ATMForward) && (ADCTurret->MoveType == ATMTSteps))
      {
        ADCTurret->abortmove = ATRHitLimit;             // We hit a limit, so abort
        ADCTurret->moveresult = ADCTurret->abortmove;
        return;
      }

      if (ADCTurret->CheckLimit(ATBitReverse) && (ADCTurret->movesteps > 0) && (ADCTurret->Motion == ATMForward) && (ADCTurret->MoveType == ATMTPosition))
      {
        ADCTurret->abortmove = ATRHitLimit;             // We hit a limit, so abort
        ADCTurret->moveresult = ADCTurret->tempresult;
        return;
      }


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

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

    // If we're moving slowly to an index, make sure you can always
                // decelerate exactly to the target index.
    if ((ADCTurret->submove == ATSSlowIndex) &&
                                (ADCTurret->profileindex > ATHalfIndexWidth))
                        ADCTurret->profileindex = ATHalfIndexWidth;

          // Read indexes
          ADCTurret->Index = !GetDigBit(ATBitIndex);

                // If we're moving slowly to an index and we come to an index,
                // stop in half the width of an index.
                if ((ADCTurret->submove == ATSSlowIndex) &&
                                (ADCTurret->Index && !ADCTurret->indexflag))
                {
                        ADCTurret->indexflag = 1;               // We just found an index
                        ADCTurret->movesteps = ATHalfIndexWidth;
                }
  }
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::Init - Initialize all object variables and motion profile
void CADCTurret::Init()
{
        int i;
        int minpulse;                   // Minimum number of FastestSteps for the given max velocity
        dword 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

        // 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 = ATMStop;
        OldMotion = ATMStop;
  Moving = 0;
  MoveType = ATMTNone;
        Limit = ATLNone;
  OldLimit = -1;
        Position = ATPUnknown;
        OldPosition = -1;
        CartoonPosition = 0;
        OldCartoonPosition = 0;
        Index = 0;
        OldIndex = -1;
        Dir = 0;
        Stp = 0;
        Mon = 0;
        Pwr = 0;
        Cur = 0;
        MonStep = 0;

  // Initialize interrupt flags
  abortmove = 0;
  movesteps = 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;
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::Update - Update all ADC turret status, motion
void CADCTurret::Update()
{
  int e;

  // Save old status values, and display on-screen if they change
  int OldDir = Dir,OldStp = Stp,OldMon = Mon,OldPwr = Pwr,OldCur = Cur;

  // Read limit switches
  CheckLimit(0);
  // Read index
  Index = !GetDigBit(ATBitIndex);
  Limit = !GetDigBit(ATBitReverse);
  // Read stepper controller status bits
  Dir = GetDigBit(ATBitDir);
  Stp = GetDigBit(ATBitStep);
  Mon = GetDigBit(ATBitMon);
  Pwr = !GetParBit(5);
  Cur = GetParBit(1);

  // Update any high level motion in progress
  // (a MoveSteps, Home, or MovePosition)
  if (Moving)
  {
    switch (MoveType)
    {

      case ATMTSteps:
      {
        switch (MoveStep)
        {
          // MoveSteps: Wait for stepper power to come on
          case 0:
          {
            if (msTimer - StepperTime >= 1000)
            {
              StepperPowerOnStage2();
              MoveStep = 1;
            }
          } break;
          // MoveSteps: Begin the move
          case 1:
            if (ADCTurretPin.Motion == ATPMStop)
            {
              if (ADCTurretPin.Limit != ATPLUnlock)
              {
                MoveType = ATMTNone;
                Moving = 0;
                EDSLog.Add(22,"ADC Turret: Pin is not unlocked");
              }
              else
              {
                MoveStep = 2;
                SubStep = 0;
                subMoveSteps(tempsteps);
                MoveStep = 2;
              }
             } break;
          // MoveSteps: Wait for move to complete
          case 2:
          {
            e = subMoveSteps(0);
            if (e >= 0)
            {
              if (e == 1)
                EDSLog.Add(23,"ADC Turret: User interrupted motion");
              else if (e == 2)
                EDSLog.Add(33,"ADC Turret motion ignored, Hit a limit");
              else if (e == 4)
                EDSLog.Add(36,"ADC Turret motion ignored, already at index");
              MoveType = ATMTNone;
              Moving = 0;
              StepperPowerOff(1);
            }
          } break;
        }
      } break; // End of ATMTSteps case

      case ATMTHome:
      {
        switch (MoveStep)
        {
          // Home: Wait for stepper power to come on
          case 0:
          {
            if (msTimer - StepperTime >= 1000)
            {
              StepperPowerOnStage2();
              MoveStep = 1;
            }
          } break;
          // Home: Wait for ADC Turret Pin to unlock
          case 1:
          {
            if (ADCTurretPin.Motion == ATPMStop)
            {
              if (ADCTurretPin.Limit != ATPLUnlock)
              {
                MoveType = ATMTNone;
                Moving = 0;
                EDSLog.Add(22,"ADC Turret: Pin is not unlocked");
                //EDSLog.Add(22,"ADC Turret: Unable to unlock pin");
              }
              else
                MoveStep = 2;
            }
          } break;
          // Home: See if we're on an index
          case 2:
          {
            if (Index != 0)
            {
              // We're already on an index, move off a bit and back on to center
              SubStep = 0;
              subMoveSteps(2000);
              MoveStep = 3;
            }
            else
            {
              // We're not at the index, so run to the "ADC" index
              SubStep = 0;
              subMoveSlowToIndex(ATMinDistance); //-60000
              MoveStep = 4;
            }
          } break;
          // Home: Wait to get off of index
          case 3:
          {
            e = subMoveSteps(0);
            if (e > 0)  // Was there an error?
            {
              // The motion was interrupted by a user keypress
              if (e == 1)
                EDSLog.Add(23,"ADC Turret: User interrupted motion");
              else if (e == 2)
                EDSLog.Add(24,"ADC Turret: Hit wrong limit");
              else
                EDSLog.Add(25,"ADC Turret: Motion error");
              MoveType = ATMTNone;
              StepperPowerOff(1);
              Moving = 0;
            }
            else if (e == 0)
            {
              if (Index != 0) // Did we get off the index?
              {
                EDSLog.Add(27,"ADC Turret: Couldn't get off of index");
                MoveType = ATMTNone;
                StepperPowerOff(1);
                Moving = 0;
              }
              else
              {
                // OK, the move off the index is complete.
                // Move back to center of index.
                SubStep = 0;
                MoveStep = 4;
                subMoveSlowToIndex(-4000);
              }
            }
          } break;
          // Home: Wait for the move back to the center of the index
          case 4:
          {
            e = subMoveSlowToIndex(0);
            if (e > 0)  // Was there an error?
            {
              if (e == 1)
                EDSLog.Add(23,"ADC Turret: User interrupted motion");
              else if (e == 2)
                EDSLog.Add(24,"ADC Turret: Hit an unexpected limit");
              else if (e == 3)
                EDSLog.Add(27,"ADC Turret: Couldn't find index");
              else
                EDSLog.Add(25,"ADC Turret: Motion error");
              MoveType = ATMTNone;
              StepperPowerOff(1);
              Moving = 0;
            }
            else if (e == 0)
            {
              // Move to center of index is complete. Home successful.
              EDSLog.Add(901,"ADC Turret: Home successful");
              StepperPowerOff(1);
              //Lock the ADC Turret Pin
              ADCTurretPin.Lock(1);
              MoveStep = 5;
            }
          } break;
          // Home: Wait for lock to complete
          case 5:
          {
            if (ADCTurretPin.Motion == ATPMStop)
            {
              if (ADCTurretPin.Limit != ATPLLock)
                EDSLog.Add(28,"ADC Turret: Unable to lock pin");
              // All done.
              MoveType = ATMTNone;
              Moving = 0;
            }
          } break;
          // Wait for move to forward limit
        }
      } break; // End of ATMTHome case

      case ATMTPosition:
      {
        switch(MoveStep)
        {
          // MovePosition: Wait for stepper power to come on
          case 0:
          {
            if (msTimer - StepperTime >= 1000)
            {
              StepperPowerOnStage2();
              MoveStep = 1;
            }
          } break;
          // MovePosition: Wait for ADC Turret Pin to unlock
          case 1:
          {
            if (ADCTurretPin.Motion == ATPMStop)
            {
              if (ADCTurretPin.Limit != ATPLUnlock)
              {
                MoveType = ATMTNone;
                Moving = 0;
                EDSLog.Add(22,"ADC Turret: Pin is not unlocked");
                //EDSLog.Add(22,"ADC Turret: Unable to unlock pin");
              }
              // Are we moving forwards or backwards?
              else
              {
                // ADC (ATPIndex) --> NO ADC (ATPReverse)
                if (tempsteps > 0)
                {
                  SubStep = 0;
                  subMoveSteps(-ATMinDistance);
                  MoveStep = 2;
                }
                // NO ADC (ATPReverse) --> ADC (ATPIndex)
                else
                {
                  SubStep = 0;
                  subMoveSteps(-ATPosToPosDistance);
                  MoveStep = 3;
                }
              }
            }
          } break;
          // MovePosition: Wait for move to reverse limit
          case 2:
          {
            e = subMoveSteps(0);
            if (e < 0)  // Not done yet
              break;
            else if (e > 0)
            // Did we fail to hit the reverse limit?
            {
              if (e == 1)
                EDSLog.Add(23,"ADC Turret: User interrupted motion");
              else if (e == 2)
                EDSLog.Add(24,"ADC Turret: Hit wrong limit");
              else if (e == 3)
                EDSLog.Add(29,"ADC Turret: Bad index");
              else
                EDSLog.Add(25,"ADC Turret: Motion error");
              MoveType = ATMTNone;
              StepperPowerOff(1);
              Moving = 0;
            }
            else if ((e == 0) && (Limit == 1))
            {
             // Success, we're at the reverse limit
             EDSLog.Add(902,"ADC Turret: NO ADC Position Successful");
             MoveType = ATMTNone;
             StepperPowerOff(1);
             Moving = 0;
            }
          } break;
          // MovePosition: Wait to get close to index
          case 3:
          {
            e = subMoveSteps(0);
            if (e > 0)  // Was there an error?
            {
              if (e == 1)
                EDSLog.Add(23,"ADC Turret: User interrupted motion");
              else if (e == 2)
                EDSLog.Add(24,"ADC Turret: Hit an unexpected limit");
              else
                EDSLog.Add(25,"ADC Turret: Motion error");
              MoveType = ATMTNone;
              StepperPowerOff(1);
              Moving = 0;
            }
            else if (e == 0)
            {
              // Move to center of index.
              SubStep = 0;
              MoveStep = 4;
              subMoveSlowToIndex(-ATExtraDistance);
            }
          } break;
          // MovePosition: Wait for the move back to the center of the index
          case 4:
          {
            e = subMoveSlowToIndex(0);
            if (e > 0)  // Was there an error?
            {
              if (e == 1)
                EDSLog.Add(23,"ADC Turret: User interrupted motion");
              else if (e == 2)
                EDSLog.Add(24,"ADC Turret: Hit an unexpected limit");
              else if (e == 3)
                EDSLog.Add(27,"ADC Turret: Couldn't find index");
              else
                EDSLog.Add(25,"ADC Turret: Motion error");
              MoveType = ATMTNone;
              StepperPowerOff(1);
              Moving = 0;
            }
            else if (e == 0)
            {
              // Move to center of index is complete. Home successful.
              EDSLog.Add(903,"ADC Turret: ADC position successful");
              StepperPowerOff(1);
              // Lock the ADC Turret Pin
              ADCTurretPin.Lock(1);
              MoveStep = 5;
            }
          } break;
          // MovePosition: Wait for lock to complete
          case 5:
          {
            if (ADCTurretPin.Motion == ATPMStop)
            {
              if (ADCTurretPin.Limit != ATPLLock)
                EDSLog.Add(28,"ADC Turret: Unable to lock pin");
              // All done.
              MoveType = ATMTNone;
              Moving = 0;
            }
          } break;
        }
      } break; // End of ATMTPosition case
    }
  }

  // Figure out what position the turret and its cartoon should be in
  CartoonPosition = 0;
  if (Motion != ATMStop)
    Position = ATPMidway;   // We're between indexes, and moving
  else
  {
    // Can we figure out where we are?
    //if ((ADCTurretPin.Limit == ATPLLock) && (Index == 1))
    // for now we are not going to check the adc turret lock pin,
    // just the index will tell the position
    if ((Index == 1) && (Limit == 0))
    {
      Position = ATPIndex;  // ADC
      if (M3Turret->CartoonPosition != 0)
        CartoonPosition = M3Turret->CartoonPosition % 10 + 2;
    }
    else if ((Index == 0) && (Limit == 1))
    {
      Position = ATPReverse;  // No ADC
      if (M3Turret->CartoonPosition != 0)
        CartoonPosition = M3Turret->CartoonPosition % 10;
    }
    else
      Position = ATPUnknown;  // We're lost
  }

  // Display anything that has changed
  if (OldMotion != Motion)
    DisplayMotion();
  if (OldLimit != Limit)
    DisplayLimit();
  if (OldIndex != Index)
    DisplayIndex();
  if (OldPosition != Position)
    DisplayPosition();
  if ((OldDir != Dir) || (OldStp != Stp) || (OldMon != Mon) ||
      (OldPwr != Pwr) || (OldCur != Cur))
    DisplayStepper();
  if (OldCartoonPosition != CartoonPosition)
      DisplayCartoon();

  // Record displayed values
  OldLimit = Limit;
  OldPosition = Position;
  OldMotion = Motion;
  OldIndex = Index;
  OldCartoonPosition = CartoonPosition;
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::DisplayAll - Redraw all status info on-screen
void CADCTurret::DisplayAll()
{
  DisplayMotion();
  DisplayLimit();
  DisplayIndex();
  DisplayPosition();
  DisplayCartoon();
}

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

        switch (Motion)
        {
                case ATMStop:
                        PutChar16(CLeftRight,x,y,BLACK,SHECMEDGRAY);break;
                case ATMForward:
                        PutChar16(CRight,x,y,SHECGREEN,SHECMEDGRAY);break;
                case ATMReverse:
                        PutChar16(CLeft,x,y,SHECGREEN,SHECMEDGRAY);break;
                default:
                        PutChar16(CLeftRight,x,y,SHECRED,SHECMEDGRAY);break;
        }
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::DisplayLimit - Display limit status bits on-screen
void CADCTurret::DisplayLimit()
{
        int x = 188,y = 232;

        /*switch (Limit)
        {
                case ATLNone:
                        PutChar16(CLeftRight,x,y,BLACK,SHECMEDGRAY);break;
                case ATLReverse:
                        PutChar16(CLeft,x,y,SHECGREEN,SHECMEDGRAY);break;
                default:
                        PutChar16(CLeftRight,x,y,SHECRED,SHECMEDGRAY);break;
        } */
        if (Limit)
        {
                        PutChar16(CLeft,x,y,SHECGREEN,SHECMEDGRAY);
                        //break;
        }
        else
        {
                        PutChar16(CLeftRight,x,y,BLACK,SHECMEDGRAY);
                        //break;
        }                
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::DisplayIndex - Display index status bit on-screen
void CADCTurret::DisplayIndex()
{
        int x = 228,y = 232;

        if (Index)
                PutChar16('A',x,y,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('A',x,y,BLACK,SHECMEDGRAY);
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::DisplayPosition - Display ADC turret position on-screen
void CADCTurret::DisplayPosition()
{
        int x1 = 260,x2 = 340,y = 232;
        switch (Position)
        {
                case ATPUnknown:
                        PutString16("Unknown",x1,x2,y,SHECRED,SHECMEDGRAY);break;
                case ATPMidway:
                        PutString16("Midway",x1,x2,y,BLACK,SHECMEDGRAY);break;
                case ATPReverse:
                        PutString16("No ADC",x1,x2,y,BLACK,SHECMEDGRAY);break;
                case ATPIndex:
                        PutString16("ADC",x1,x2,y,BLACK,SHECMEDGRAY);break;
        }
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::DisplayStepper - Display all stepper status bits
void CADCTurret::DisplayStepper()
{
        if (Dir)
                PutChar16('þ',468,48,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',468,48,BLACK,SHECMEDGRAY);
        if (Stp)
                PutChar16('þ',500,48,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',500,48,BLACK,SHECMEDGRAY);
        if (Mon)
                PutChar16('þ',532,48,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',532,48,BLACK,SHECMEDGRAY);
        if (Pwr)
                PutChar16('þ',564,48,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',564,48,BLACK,SHECMEDGRAY);
        if (Cur)
                PutChar16('þ',596,48,SHECGREEN,SHECMEDGRAY);
        else
                PutChar16('þ',596,48,BLACK,SHECMEDGRAY);
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::DisplayCartoon - Erase last cartoon and draw current one
void CADCTurret::DisplayCartoon()
{
        DrawCartoon(OldCartoonPosition,SHECMEDGRAY);
        DrawCartoon(CartoonPosition,BLACK);
        OldCartoonPosition = CartoonPosition;
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::DrawCartoon - Draw the ADC cartoon at given Position in Color
void CADCTurret::DrawCartoon(int Position,int Color)
{
        int i,j;


        if ((Position >= 1) && (Position <= 7))
        {
                i = Position - 1;
                // Draw AT cartoons
                for (j=0;j<=3;j++)
                        Line(ATurretCoords[i][j*2],ATurretCoords[i][j*2+1],
                                         ATurretCoords[i][((j+1)%4)*2],ATurretCoords[i][((j+1)%4)*2+1],
                                         Color);
                Line(ATurretCoords[i][8],ATurretCoords[i][9],
                                 ATurretCoords[i][10],ATurretCoords[i][11],
                                 Color);
                Circle(75,75,60,BLACK); // Redraw outer circle on cartoon
        }
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::Home - Find the position of the ADC turret
int CADCTurret::Home()
// Returns a 0 on success or if move was started,
// 1 on other moves in progress, 2 on mirror not down
{
  Position = ATPUnknown;
  // See if we can immediately figure out where we are
  // (if the turret is locked at its index or at the No ADC limit)
  if ((ADCTurretPin.Limit == ATPLLock) && (Index == 1))
  {
    Position = ATPIndex;
    EDSLog.Add(36,"ADC Turret: Motion Ignored, locked");
  }
  else if ((Index == 0) && (Limit == 1))
    Position = ATPReverse;

  if (Position != ATPUnknown)
    return(0);

  // We need to do a full home

  // Don't move if there are any other moves taking place
  if (Moving || (M3Turret->Moving) || (Mirror.MMoving) || (Mirror.LMoving) ||
     (ADCTurretPin.Moving) || (M3TurretPin.Moving))
  {
    EDSLog.Add(500,"ADC Turret: Home aborted, other moves in progress");
    return(1);
  }

  // Check to make sure mirror is in right
  // position (down).
  if (!(Mirror.Limit & MLDown))
  {
    EDSLog.Add(21,"ADC Turret: Home aborted, mirror not down");
    return(2);
  }

  // Ok, we're going to have to do some moving
  Moving = 1;
  MoveType = ATMTHome;

  if (StepperPowerOn())
  {
    StepperTime = msTimer;
    MoveStep = 0;
  }
  else
    MoveStep = 1;

  // Home started successfully
  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::MoveSteps - Move the turret the given number of steps
int CADCTurret::MoveSteps(int steps)
// Returns a 0 on success or if move was started,
// 1 on other moves in progress, 2 on mirror not down,
// 3 if the turret is locked.
// 4 if limit index was found
// 5 if Reverse limit was found
// Steps is positive for forward motion and negative for reverse motion
{
  // Don't move if steps is 0
  if (steps == 0)
    return(0);

  // Don't move if there are any other moves taking place
  if (Moving || (M3Turret->Moving) || (Mirror.MMoving) || (Mirror.LMoving) ||
     (ADCTurretPin.Moving) || (M3TurretPin.Moving))
  {
    EDSLog.Add(500,"ADC Turret: Move aborted, other moves in progress");
    return(1);
  }

  // Don't move if mirror not down
  if (!(Mirror.Limit & MLDown))
  {
    EDSLog.Add(32,"ADC Turret: Move aborted, mirror not down");
    return(2);
  }

  // Don't move if the turret is locked in position
  if ((ADCTurretPin.Limit != ATPLUnlock) || (ADCTurretPin.Motion != ATPMStop))
  {
    EDSLog.Add(31,"ADC Turret: Move aborted, turret is Locked");
    return(3);
  }

  // checking limits
  // Index Limit (ADC position)
  if (!GetDigBit(ATBitIndex) && (steps < 0))
  {
     ADCTurret->abortmove = ATRHitLimitIndex; // We hit a index, so abort
     EDSLog.Add(36,"ADC Turret motion ignored, already at index");
     return(4);
   }
  //Reverse Limit
  if (ADCTurret->CheckLimit(ATBitReverse) && (steps >  0))
  {
     ADCTurret->abortmove = ATRHitLimit; // We hit a limit, so abort
     EDSLog.Add(33,"ADC Turret motion ignored, already at limit");
     return(5);
  }

  // Tell everyone we're moving
  Moving = 1;
  MoveType = ATMTSteps;
  tempsteps = steps;
  if (StepperPowerOn())
  {
    StepperTime = msTimer;
    MoveStep = 0;
  }
  else
    MoveStep = 1;

  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::MovePosition - Move to one of the turret's two positions
int CADCTurret::MovePosition(int targetposition)
// Returns a 0 on success or if move was started,
// 1 on other moves in progress, 2 on mirror not down,
// 3 if we need to home first.
{
  // Are we already at the commanded position?
  if (Position == targetposition)
    return(0);

  // Don't move if there are any other moves taking place
  if (Moving || (M3Turret->Moving) || (Mirror.MMoving) || (Mirror.LMoving) ||
     (ADCTurretPin.Moving) || (M3TurretPin.Moving))
  {
    EDSLog.Add(500,"ADC Turret: Move aborted, other moves in progress");
    return(1);
  }

  // Don't move if mirror not down
  if (!(Mirror.Limit & MLDown))
  {
    EDSLog.Add(32,"ADC Turret: Move aborted, mirror not down");
    return(2);
  }

  // Don't move because we're lost and need a Home operation
  if (Position == MTPUnknown)
  {
    EDSLog.Add(35,"ADC Turret: Move aborted, Home required first");
    return(3);
  }

  // We're going to move at full speed until just before our target.
  /* if (Position < targetposition)
  // We're going backward to the index
    tempsteps = -1;
  // We're running forward until we hit the limit
  else
    tempsteps = 1;*/

  //if we want to move to adc but, that is the actual position, the adc turret will not move

  if ((Position == ATPIndex || Position == ATPMidway) && (targetposition == ATPReverse))
    tempsteps = 1;  // we want to move to NO ADC position
  else if ((Position == ATPReverse || Position == ATPMidway) && (targetposition == ATPIndex))
    tempsteps = -1; // we want to move to ADC position

  // Tell everyone we're moving
  Moving = 1;
  MoveType = ATMTPosition;
  if (StepperPowerOn())
  {
    StepperTime = msTimer;
    MoveStep = 0;
  }
  else
    MoveStep = 1;

  return(0);
}

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

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::subMoveSteps - Move the turret the given number of steps
int CADCTurret::subMoveSteps(int steps)
// Returns a 0 on success, 1 on user interrupt, 2 on hit a limit
// Steps is positive for forward motion and negative for reverse motion
{
  switch (SubStep)
  {
        // Begin motion
        case 0:
    {
            // Forward motion
            if (steps > 0)
            {
                    ClrDigBit(ATBitDir);
                    //limitbit = ATBitForward;
                    Motion = ATMForward;
                    direction = -1;
            }
            // Reverse motion
            else
            {
                    SetDigBit(ATBitDir);
                    limitbit = ATBitReverse;
                    Motion = ATMReverse;
                    direction = 1;
            }

      // Tell interrupt routine to start moving
      moveresult = -1;                                  // This will change on move completion
      tempresult = ATRSuccess;  // Return code for success
      abortmove = 0;                                            // Clear abort flag
      pulsecount = 1;
      profileindex = 0;
      submove = ATSSteps;                               // Tell interrupt what kind of motion we want
      movesteps = abs(steps);             // Interrupt starts going when it sees
                                                                                                            // movesteps != 0
      SubStep++;
    } break;
    // Check for move completion
    case 1:
    {
        if (moveresult >= 0)
      {
        Motion = ATMStop;
        return(moveresult);
      }
    } break;
  };
  return(-1);   // Move not complete
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::subMoveSlowToIndex - Move slowly to the center of the next index
int CADCTurret::subMoveSlowToIndex(int steps)
// Returns a 0 on success, 1 on user interrupt, 2 on hit a limit,
// 3 if we somehow aren't on an index when we should be.
// This routine moves slowly to the next index, and stops in the center
// of the index when it gets there.  Pass it the number of steps it should
// go looking for the next index before giving up, with positive steps
// being forward and negative reverse.
// Most error checking (other moves in progress, mirror not down, etc) done
// in the higher level routines.
{
  switch (SubStep)
  {
    // Begin motion
    case 0:
    {
      // You're not allowed to start on top of an index
      if (Index)
      {
        SubStep = 2;
        break;
      }
      // Forward motion
      if (steps > 0)
      {
        ClrDigBit(ATBitDir);
        //limitbit = ATBitForward;
        Motion = ATMForward;
        direction = -1;
      }
      // Reverse motion
      else
      {
        SetDigBit(ATBitDir);
        limitbit = ATBitReverse;
        Motion = ATMReverse;
        direction = 1;
      }

      // Tell interrupt routine to start moving
      moveresult = -1;           // This will change on move completion
      tempresult = ATRSuccess;  // Return code for success
      abortmove = 0;            // Clear abort flag
      pulsecount = 1;
      profileindex = 0;
      indexflag = 0;
      submove = ATSSlowIndex;   // Tell interrupt what kind of motion we want
      movesteps = abs(steps);   // Interrupt starts going when it sees
                                // movesteps != 0
      SubStep++;
    } break;
    // Check for move completion
    case 1:
    {
      if (moveresult == ATRUserAbort)
      {
        Motion = ATMStop;
        return(moveresult);
      }
      if (moveresult >= 0)
      {
        Motion = ATMStop;
        if (indexflag == 0)
          return(ATRNoIndex); // We didn't land on our index.
        // Figure out our location
        if ((Index == 1))
          Position = ATPIndex;
        else
        {
          Position = ATPUnknown;
          return(ATRNoIndex);  // Somehow we overshot or misread the index
        }
        // Success
        return(0);
      }
    } break;
    // Error, we started on top of an index
    case 2:
    {
      return(MTRNoIndex);
    } break;
  }
  return(-1);   // Move not complete
}

/////////////////////////////////////////////////////////////////////////////
// CADCTurret::CheckLimit - Check a limit, and debounce
int CADCTurret::CheckLimit(int limitbit)
// Returns a 0 on limit not hit, 1 on limit hit
// Debouncing was necessary because of noise in the limit signals during motion
{
        // Is reverse limit triggered?
        if (!GetDigBit(ATBitReverse))
  {
                if (RevLimitCount < ATDebounceLimit)
            RevLimitCount++;
  }
  else
  {
        // Did we kill some noise?
    if ((RevLimitCount > 0) && (RevLimitCount < ATDebounceLimit))
        Log->Add("ADC Turret Reverse Limit debounced",1);
        RevLimitCount = 0;
  }

  // Record limit status
        Limit = 2 * (RevLimitCount >= ATDebounceLimit) +
                                        FwdLimitCount >= ATDebounceLimit;

  // Report the limit bit of interest
  if ((limitbit == ATBitReverse) && (RevLimitCount >= ATDebounceLimit))
  {
        Limit = ATLReverse;
        return(1);
  }
  return(0);


}

Generated by GNU Enscript 1.6.5.2.
Document Actions