Skip to content. | Skip to navigation

Personal tools

lcu/tilt.cpp

lcu/tilt.cpp

lcu/tilt.cpp

/////////////////////////////////////////////////////////////////////////////
// TILT.CPP
//
//  Last updated: 4/24/2005
//
//  Part of the source code for the Remote M3 Program
//
//  Contains routines for motion and status of M3 Mirror Tilt motors and LVDTs
/////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////
// TiltInterrupt is used for background motion (called by clock interrupt)
void TiltInterrupt()
{
  int i;

  // Loop through all three tilt motors
  for (i=0;i<=2;i++)
  {
    do	// Use a do loop to allow easy breaks on errors
    {
      // Do we need to do an emergency stop?
      if (Tilt.abortmove[i])
      {
        // Stop as soon as our motion profile allows it
        if (Tilt.movesteps[i] > Tilt.profileindex[i])
          Tilt.movesteps[i] = Tilt.profileindex[i];
        // Jump to the necessary deceleration point in the motion profile
        Tilt.profileindex[i] = Tilt.movesteps[i];
        // Don't go faster than max velocity
        if (Tilt.profileindex[i] >= Tilt.MaxProfile)
          Tilt.profileindex[i] = Tilt.MaxProfile - 1;
        Tilt.pulsecount[i] = 1;

        // Remember why we aborted
        Tilt.tempresult[i] = Tilt.abortmove[i];
        Tilt.abortmove[i] = 0;

        // Stop now if movesteps is already 0
        if (Tilt.movesteps[i] == 0)
        {
          Tilt.moveresult[i] = Tilt.tempresult[i];
          Tilt.Motion[i] = TMStop;
          break;
        }
      }

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

          // Are we done?
          if (Tilt.movesteps[i] == 0)
          {
            Tilt.moveresult[i] = Tilt.tempresult[i];
            Tilt.Motion[i] = TMStop;
            break;
          }

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

        // Don't exceed maximum velocity
        if (Tilt.profileindex[i] >= Tilt.MaxProfile)
          Tilt.profileindex[i] = Tilt.MaxProfile - 1;
      }
    } while (0);
  }
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::Init - Initializes tilt variables, motion profile
void CTilt::Init()
{
  int i;
  int minpulse;             // Minimum number of FastestSteps for the given max velocity
  unsigned long 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;                   // Flag for completion of motion profile

  // Extracted from M3.INI
  // Tilt Stepper initial stepper pulse in milliseconds
  InitialPulse = 50;
  // Tilt Stepper maximum velocity in steps/sec
  MaxVelocity = 50.0;
  // Tilt Stepper acceleration in steps/sec/sec
  Acceleration = 100.0;
  // Steps per millimeter ratio (steps/mm)
  // to convert stepper motor steps into millimeters of height
  // (each of three considered the same)
  Kstepmm = 118.110;
  // Constants used in the steps to mm conversion.
  // Defined by the geometry of the M3, all in millimeters
  KKA = 381.0;
  KKB = 136.73;
  KKC = 216.03;

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

  // Mark the brake status
  BrakeFlag = 0;

  // Initialize all status variables for all three tilts
  for (i=0;i<=2;i++)
  {
    Motion[i] = TMStop;
    Moving[i] = 0;
    Dir[i] = 0;
    Stp[i] = 0;
    Mon[i] = 0;
    Pwr[i] = 0;
    Cur[i] = 0;
    Brk[i] = 0;
    MonStep[i] = 0;
    // Initialize interrupt flags
    abortmove[i] = 0;
    movesteps[i] = 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;
  // Don't let a fast computer give divide-by-zeros
  if (total < 1)
    total = 1;
	// 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
    Profile[i] = 1.0 / (Acceleration * total * FastestStep);

    ///////////////////////////////////////////////////
    // Find possible velocities near our target velocity
    Vt = Acceleration * total;
    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);
    }
    // Keep track of total error
    error += V - Vt;
    /////////////////////////////////////////////////////

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

  // We're not servoing at startup
  for (i=0;i<=2;i++)
    ServoFlag[i] = 0;
  ServoAllFlag = 0;
  ServoStepTimer = 0;
  // Start with all three tilt brakes engaged
  ClrDigBit(5);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::Update - Update all motion, status
void CTilt::Update()
{
  int i;
  int ServoDone[3];     // Check to see if servo's done
  float err;            // Servo error
  float P;							// Proportional term in the control action = KP*err

  // Update any motion
  for (i=0;i<=2;i++)
  {
    // Update if this tilt motor is moving
    if (Moving[i])
    {
    	switch(MoveStep[i])
      {
        // Wait for stepper power to come on
        case 0:
        {
          if ((Timer - StepperTimer[i]) >= (1000*CKF))
          {
            StepperPowerOnStage2();
            // Do we need to disengage the brakes?
            if (ReleaseBrakes())
            	MoveStep[i] = 2;
            else
            {
	            BrakeTimer[i] = Timer;
	            MoveStep[i] = 1;
            }
          }
        } break;
        // Wait for brakes to release
        case 1:
        {
        	if ((Timer - BrakeTimer[i]) > (500*CKF))
          	MoveStep[i] = 2;
        } break;
        // Begin Move
        case 2:
        {
          // Tell interrupt routine to start moving
          moveresult[i] = -1;					// This will change on move completion
	        tempresult[i] = TRSuccess;	// Return code for success
          abortmove[i] = 0;						// Clear abort flag
          pulsecount[i] = 1;
          profileindex[i] = 0;
					// Prepare for motion
	        // "Out" motion
	        if (tempsteps[i] > 0)
	        {
		        ClrDigBit(TBitDir[i]);
		        Motion[i] = TMOut;
		        direction[i] = -1;
	        }
	        // "In" motion
	        else
	        {
		        SetDigBit(TBitDir[i]);
		        Motion[i] = TMIn;
		        direction[i] = 1;
	        }
					// Interrupt starts going when it sees movesteps != 0
	        movesteps[i] = abs(tempsteps[i]);
          MoveStep[i] = 3;
        } break;
        // Wait for move to complete
        case 3:
        {
          if (moveresult[i] >= 0)
          {
            if (moveresult[i] > 0)
						  EDSLog.Add(50,"Tilts: User interrupted motion");
            if (EngageBrakes(1))
            {
            	Moving[i] = 0;
            	StepperPowerOff(1);
            }
            else
            {
            	BrakeTimer[i] = Timer;
	            MoveStep[i] = 4;
            }
          }
        } break;
        // Wait for brakes to engage
        case 4:
				{
        	if ((Timer - BrakeTimer[i]) > (500*CKF))
          {
            Moving[i] = 0;
            StepperPowerOff(1);
          }
        } break;
      }
    }
  }

  // Update the servo loop
  for (i=0;i<=2;i++)
  {
    if (ServoFlag[i] && ((Timer - ServoStepTimer) > (1000*CKF)))
    {
  	  // Did the servo time out?
      if ((Timer - ServoTimer[i]) > (TiltServoTimeout*CKF))
			{
			  EDSLog.Add(51,"Tilt%c: Servo timed out",'A'+i);
    	  ServoFlag[i] = 0;
      }
			// Did the servo reach its destination?
      else
      {
        // The mesa card data is stored in this order: LVDTA,LVDTB,LVDTC
      	err = ServoTarget[i] - AnalogData[i];
	  	  ServoDone[i] = 1;
    	  if (fabs(err) >= TiltServoError)
      	  ServoDone[i] = 0;
  	    if (ServoDone[i])
				{
        	EDSLog.Add(951,"Tilt%c servo completed",'A'+i);
    		  ServoFlag[i] = 0;
        }
      }
      // OK, make any moves we need (if possible)
      if (ServoFlag[i])
		  {
        if ((fabs(err) >= TiltServoError) && !Moving[i])
        {
          P = TiltServoRatio * err;
          if (P > TiltMax)
						MoveSteps(TiltMax,i,1);
          else if (P < -TiltMax)
						MoveSteps(-TiltMax,i,1);
          else if (fabs(P) < 7.0)
          {
					  if (P > 0)
							MoveSteps(7,i,1);
          	else if (P < 0)
							MoveSteps(-7,i,1);
          }
					else
						MoveSteps(P,i,1);
          ServoStepTimer = Timer;
        }
      }
    }
  }

  // Update all tilts servo
  if (ServoAllFlag)
  {
    switch(ServoAllStep)
    {
    	case 0:
      {
        // Added this to avoid M3 to fall (brakes off for some time)
        if ((Timer - ServoAllTimer) > (3000*CKF))
        	ServoAllStep = 1;
      } break;
      case 1:
      {
				Servo(0,ServoTarget[0]);
        ServoAllStep = 2;
      } break;
      case 2:
      {
    		if (ServoDone[0] && !Moving[0])
				{
					//EDSLog.Add(951,"TiltA servo completed");
					Servo(1,ServoTarget[1]);
        	ServoAllStep = 3;
        }
      } break;
      case 3:
      {
    		if (ServoDone[1] && !Moving[1])
				{
					//EDSLog.Add(952,"TiltB servo completed");
					Servo(2,ServoTarget[2]);
        	ServoAllStep = 4;
        }
      } break;
      case 4:
      {
    		if (ServoDone[2] && !Moving[2])
				{
					//EDSLog.Add(953,"TiltC servo completed");
        	ServoAllFlag++;
          if (ServoAllFlag == 2)
					{
						EDSLog.Add(954,"Servo first pass completed");
            ServoAllTimer = Timer;
          	ServoAllStep = 0;
          }
					else if (ServoAllFlag == 3)
          {
						EDSLog.Add(955,"Servo second pass completed");
          	ServoAllFlag = 0;
          }
        }
      } break;
    }
  }

  // Read analog inputs (LVDTs = tilt positions)
  ReadAllAnalog();

	// Update stepper and motion displays
  for (i=0;i<=2;i++)
  {
    Dir[i] = GetDigBit(TBitDir[i]);
    Stp[i] = GetDigBit(TBitStep[i]);
    Mon[i] = !GetDigBit(TBitMon[i]);
    Pwr[i] = GetDigBit(TBitPwr[i]);
    Cur[i] = GetDigBit(TBitCur[i]);
    Brk[i] = !GetDigBit(TBitBrk[i]);
  }
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::MoveSteps - Move given number of steps on tilt motor specified
int CTilt::MoveSteps(int steps,int tilt,int ServoOK)
// Returns a 0 on success, 1 on user interrupt
// Steps is positive for outward motion (moving the mirror away from
// its frame) and negative for inward motion.
// tilt = 0 - 2, 0 = A, 1 = B, 2 = C
// If ServoOK is 1, move is allowed even if the servo is active.
{
  //int lvdtslimit = 0;  // Tells when we went pass the LVDTs soft limits
  // Should we be checking these?
  // Check analog data out of soft limits
  //ReadAllAnalog();
  //if ( ((AnalogData[2] > -6.0)  && (AnalogData[2] < 7.0)) &&
  //		 ((AnalogData[1] > -7.0)  && (AnalogData[1] < 6.0)) &&
  //		 ((AnalogData[0] > -11.0) && (AnalogData[0] < 5.0)) )
  //if (lvdtslimit)
  //EDSLog.Add(52,"Tilt: LVDTs soft limit reached");

  // Don't move if steps is 0
  if (steps == 0)
    return(0);

  if (abs(steps) > TiltMax)
  {
    EDSLog.Add(53,"Tilt Move: Max %d steps allowed",TiltMax);
    return(1);
  }

  if (Moving[tilt] || (!ServoOK && ServoFlag[tilt]))
	{
		EDSLog.Add(54,"Tilt move ignored, move in progress");
		return(1);
	}

	// The setting for TBitDir[tilt] was moved to Update().
	// Somehow the Mycom stepper driver "forgets" the setting for these bits
	// after 1.5 secs and returns to default (-steps).
  // This means that there were problems only for +steps.

  tempsteps[tilt] = steps;

  // Tell everyone we're moving
  Moving[tilt] = 1;
  if (StepperPowerOn())
  {
	  MoveStep[tilt] = 0;
    StepperTimer[tilt] = Timer;
  }
  else
  {
	  // Do we need to disengage the brakes?
    if (ReleaseBrakes())
      MoveStep[tilt] = 2;
    else
    {
	    BrakeTimer[tilt] = Timer;
	    MoveStep[tilt] = 1;
    }
  }

  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::AbortMove - Abort any move in progress
void CTilt::AbortMove()
{
	int i;
	// Tell TiltInterrupt to stop anything in progress
  for (i=0;i<=2;i++)
  {
		abortmove[i] = 1;
    ServoFlag[i] = 0;
  }
  ServoAllFlag = 0;
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::ReleaseBrakes - Release tilt brakes
int CTilt::ReleaseBrakes()
// Returns a 0 if caller must wait 500 msec for release to occur,
// 1 if no wait necessary.
{
	// Release tilt brakes
	SetDigBit(5);
  BrakeFlag++;
  if (BrakeFlag > 1)
		return(1);
  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::EngageBrakes - Engage tilt brakes if no tilts are moving
int CTilt::EngageBrakes(int MotionOK)
// Returns a 0 if caller must wait 500 msec for engage to occur,
// 1 if no wait necessary.
// If MotionOK = 0, brakes will not be engaged if tilts are moving.
{
	if (!MotionOK && (Tilt.Moving[0] || Tilt.Moving[1] || Tilt.Moving[2]))
  	return(1);
  BrakeFlag--;
  if (BrakeFlag <= 0)
  {
  	BrakeFlag = 0;
	  // Engage tilt brakes
	  ClrDigBit(5);
		return(0);
  }
  return(1);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::Servo - Servo tilt to the given LVDT position
int CTilt::Servo(int tilt,float position)
// Servo the tilt (A,B or C) to the given LVDT position
// Returns a 0 if move was started, 1 if a move is already in progress
{
  if (Moving[tilt] || ServoFlag[tilt])
	{
		EDSLog.Add(55,"Tilt move ignored, move in progress");
		return(1);
	}

	ServoTimer[tilt] = Timer;
  ServoTarget[tilt] = position;
  ServoFlag[tilt] = 1;

  return(0);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::ServoAll - Servo 3 tilts to the given LVDTs positions
int CTilt::ServoAll(float positionA,float positionB,float positionC)
// Servo the tilts to the given LVDTs positions
// Returns a 0 if move was started, 1 if a move is already in progress
{
  if (Moving[0] || Moving[1] || Moving[2] ||
			ServoFlag[0] || ServoFlag[1] || ServoFlag[2] ||
			ServoAllFlag)
	{
		EDSLog.Add(56,"Tilt move ignored, move in progress");
		return(1);
	}

  ServoAllTimer = Timer;
  ServoTarget[0] = positionA;
  ServoTarget[1] = positionB;
  ServoTarget[2] = positionC;
  ServoAllFlag = 1;
  ServoAllStep = 0;

  return(0);
}


Generated by GNU Enscript 1.6.5.2.
Document Actions