Skip to content. | Skip to navigation

Personal tools

tilt.cpp

tilt.cpp

tilt.cpp

/////////////////////////////////////////////////////////////////////////////
// TILT.CPP
//
//  Last updated: 10/16/2003
//
//  Part of the source code for the M3 Program
//
//  Contains routines for motion and status of M3 Mirror Tilt motors and LVDTs
/////////////////////////////////////////////////////////////////////////////

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, load-cell DGHs
void CTilt::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

  // Mark the brake status
  BrakeFlag = 0;
  OldBrake = -1;

  // Initialize all status variables for all three tilts
  for (i=0;i<=2;i++)
    {
      Motion[i] = TMStop;
      OldMotion[i] = TMStop;
      Moving[i] = 0;
      Dir[i] = 0;
      Stp[i] = 0;
      Mon[i] = 0;
      Pwr[i] = 0;
      Cur[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;
  // 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);
	}
      // 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;

  // Initialize analogcount, which identifies which LVDT reading is
  // being displayed
  analogcount = 0;
  // Start reading first DGH
  CurrentDGH = 0;
  // Initialize DGH modules.
  DGHs[0].Init('a');
  DGHs[1].Init('b');
  DGHs[2].Init('c');
  for (i=0;i<=3;i++)
    OldAnalogData[i] = -999.99;
  // Kstepmm,KKA,KKB and KKC are initialized from M3.INI
  // 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
  ClrParBit(6);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::Update - Update all motion, status, displays
void CTilt::Update()
{
  int i;
  char s[100];
  int e;            // Stores error codes
  int ServoDone[3]; // Check to see if servo's done
  float err;        // Servo error
  float P;          // Proportional term in the control action = KP*err
  int OldDir[3],OldStp[3],OldMon[3],OldPwr[3],OldCur[3];
  // Previous stepper values
  int preverrorcount;   // Previous number of errors for a DGH module

  // 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 (msTimer - StepperTimer[i] >= 1000)
		  {
		    StepperPowerOnStage2();
		    // Do we need to disengage the brakes?
		    if (ReleaseBrakes())
		      MoveStep[i] = 2;
		    else
		      {
			BrakeTimer[i] = msTimer;
			MoveStep[i] = 1;
		      }
		  }
	      } break;
	      // Wait for brakes to release
	    case 1:
	      {
        	if (msTimer - BrakeTimer[i] > 500)
		  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(500,"Tilts: User interrupted motion");
		    if (EngageBrakes(1))
		      {
			Moving[i] = 0;
			StepperPowerOff(1);
		      }
		    else
		      {
			BrakeTimer[i] = msTimer;
			MoveStep[i] = 4;
		      }
		  }
	      } break;
	      // Wait for brakes to engage
	    case 4:
	      {
        	if (msTimer - BrakeTimer[i] > 500)
		  {
		    Moving[i] = 0;
		    StepperPowerOff(1);
		  }
	      } break;
	    }
	}
    }

  // Update the servo loop
  for (i=0;i<=2;i++)
    {
      if ((ServoFlag[i]) && (msTimer - ServoStepTimer > 1000))
	{
  	  // Did the servo time out?
	  if (Secs - ServoTimer[i] > TiltServoTimeout)
	    {
	      EDSLog.Add(505,"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: LVDTC,LVDTB,LVDTA
	      err = ServoTarget[i] - AnalogData[2-i];
	      ServoDone[i] = 1;
	      if (fabs(err) >= TiltServoError)
		ServoDone[i] = 0;
	      if (ServoDone[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) < 2.0)
		    {
		      if (P > 0)
			MoveSteps(2,i,1);
		      else if (P < 0)
			MoveSteps(-2,i,1);
		    }
		  else
		    MoveSteps(P,i,1);
		  ServoStepTimer = msTimer;
		}
	    }
	}
    }

  // Update all tilts servo
  if (ServoAllFlag)
    {
      switch(ServoAllStep)
	{
    	case 0:
	  {
	    // Added this to avoid M3 to fall (brakes off for some time)
	    if (Secs - ServoAllTimer > 3)
	      ServoAllStep = 1;
	  } break;
	case 1:
	  {
	    Servo(0,ServoTarget[0]);
	    ServoAllStep = 2;
	  } break;
	case 2:
	  {
	    if (ServoDone[0] && !Moving[0])
	      {
		EDSLog.Add(981,"TiltA servo completed");
		Servo(1,ServoTarget[1]);
        	ServoAllStep = 3;
	      }
	  } break;
	case 3:
	  {
	    if (ServoDone[1] && !Moving[1])
	      {
		EDSLog.Add(982,"TiltB servo completed");
		Servo(2,ServoTarget[2]);
        	ServoAllStep = 4;
	      }
	  } break;
	case 4:
	  {
	    if (ServoDone[2] && !Moving[2])
	      {
		EDSLog.Add(983,"TiltC servo completed");
        	ServoAllFlag++;
		if (ServoAllFlag == 2)
		  {
		    EDSLog.Add(971,"Servo first pass completed");
		    ServoAllTimer = Secs;
		    ServoAllStep = 0;
		  }
		else if (ServoAllFlag == 3)
		  {
		    EDSLog.Add(972,"Servo second pass completed");
		    ServoAllFlag = 0;
		  }
	      }
	  } break;
	}
    }

  // Read Mesa analog card (LVDTs = tilt positions) and display
  ReadAllAnalog();
  // Display updated analog data if necessary
  if (OldAnalogData[analogcount] != AnalogData[analogcount])
    DisplayAnalogData();
  OldAnalogData[analogcount] = AnalogData[analogcount];
  analogcount++;
  if (analogcount > 3)
    analogcount = 0;

  // Update stepper and motion displays
  for (i=0;i<=2;i++)
    {
      OldDir[i] = Dir[i];
      Dir[i] = GetDigBit(TBitDir[i]);
      OldStp[i] = Stp[i];
      Stp[i] = GetDigBit(TBitStep[i]);
      OldMon[i] = Mon[i];
      Mon[i] = GetDigBit(TBitMon[i]);
      OldPwr[i] = Pwr[i];
      Pwr[i] = !GetParBit(5);
      OldCur[i] = Cur[i];
      Cur[i] = GetParBit(0);
      if (OldMotion[i] != Motion[i])
	DisplayMotion(i);
      OldMotion[i] = Motion[i];
      if ((OldDir[i] != Dir[i]) || (OldStp[i] != Stp[i]) || (OldMon[i] != Mon[i]) ||
	  (OldPwr[i] != Pwr[i]) || (OldCur[i] != Cur[i]))
	DisplayStepper(i);
    }

  // Update brake display
  if ((PARVALUE & 64) != OldBrake)
    {
      OldBrake = (PARVALUE & 64);
      DisplayBrake();
    }

  // Update DGH modules (mirror load cell readings)
  preverrorcount = DGHs[CurrentDGH].ErrorCount;
  e = DGHs[CurrentDGH].Update();
  if (e == -1)
    return;			// DGH module not finished updating
  if (e == 0)
    // DGH module read success
    {
      if (preverrorcount >= 3)
	EDSLog.Add(998,"DGH %c Error Messages Resumed",CurrentDGH+'A');
      sprintf(s,"%+7.3fV  ",DGHs[CurrentDGH].Reading);
      PutString16(s,468,548,168+16*CurrentDGH,BLACK,SHECMEDGRAY);               
    }
  else
    // DGH module read error
    {
      PutString16("Read Error",468,548,168+16*CurrentDGH,SHECRED,SHECMEDGRAY);
      if (DGHs[CurrentDGH].ErrorCount < 3)
	EDSLog.Add(50,"DGH %c Read Error",CurrentDGH+'A');
      else if (DGHs[CurrentDGH].ErrorCount == 3)
	EDSLog.Add(51,"DGH %c Error Messages Suspended",CurrentDGH+'A');
    }
  CurrentDGH++;
  // Continue scanning DGH's
  if (CurrentDGH > 2)
    CurrentDGH = 0;
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::DisplayAll - Display all motion, status
void CTilt::DisplayAll()
{
  int i;
  DisplayBrake();
  DisplayAnalogData();
  for (i=0;i<=2;i++)
    {
      DisplayMotion(i);
      DisplayStepper(i);
    }
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::DisplayBrake - Display brake status
void CTilt::DisplayBrake()
{
  // Display brake status
  if (!(PARVALUE & 64))
    PutChar16('þ',468,216,SHECGREEN,SHECMEDGRAY);
  else
    PutChar16('þ',468,216,BLACK,SHECMEDGRAY);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::DisplayAnalogData - Display analog LVDT readings
void CTilt::DisplayAnalogData()
{
  char s[20];
  int y = 296;
  int x1,x2,y1,y2,data,datay;

  sprintf(s,"%7.3lf",AnalogData[analogcount]);
  data = round(AnalogData[analogcount]);
  switch (analogcount) {
    // Tilt C  Ranges between -11 and 5 Volts aprox.
  case 0:
    {
      //PutString16(s,180,236,y+16*2,BLACK,SHECMEDGRAY);
      PutString16(s,260,316,y+16*2,BLACK,SHECMEDGRAY);
      // Display tilt in millimeters starting from 0.
      //sprintf(s,"%7.3lf",(11.0+AnalogData[0])*32.0/Kstepmm); //32 steps/volt obtained experimentally
      //PutString16(s,260,316,y+16*2,BLACK,SHECMEDGRAY);
      // Display a little bar graph, 1 pixel/volt
      x1 = TiltX + 17;
      x2 = TiltX + 25;
      y1 = TiltY + 16;
      y2 = TiltY + 34;
      //datay = TiltY + 25 - (data + 3);
      datay = TiltY + 25 - data;
      //if (data < -11)
      if (data < -9)
	Box(x1,y1,x2,y2,BLACK);
      //else if (data > 5)
      else if (data > 8)
	Box(x1,y1,x2,y2,SHECGREEN);
      else
	{
	  //Box(x1,y1,x2,datay,BLACK);
	  //Box(x1,datay+1,x2,y2,SHECGREEN);
	  Box(x1,y1,x2,datay-1,BLACK);
	  Box(x1,datay,x2,y2,SHECGREEN);
	}
    } break;
    // Tilt B ranges between -7 and 6 Volts aprox
  case 1:
    {
      //PutString16(s,180,236,y+16*1,BLACK,SHECMEDGRAY);
      PutString16(s,260,316,y+16*1,BLACK,SHECMEDGRAY);
      // Display tilt in millimeters starting from 0.
      //sprintf(s,"%7.3lf",(7.0+AnalogData[1])*40.0/Kstepmm); //40 steps/volt obtained experimentally
      //PutString16(s,260,316,y+16*1,BLACK,SHECMEDGRAY);
      // Display a little bar graph, 1 pixel/volt
      x1 = TiltX - 17;
      x2 = TiltX - 25;
      y1 = TiltY + 16;
      y2 = TiltY + 34;
      datay = TiltY + 25 - data;
      //if (data < -7)
      if (data < -9)
	Box(x1,y1,x2,y2,BLACK);
      //else if (data > 6)
      else if (data > 8)
	Box(x1,y1,x2,y2,SHECGREEN);
      else
	{
	  Box(x1,y1,x2,datay-1,BLACK);
	  Box(x1,datay,x2,y2,SHECGREEN);
	}
    } break;
    // Tilt A	// Ranges between -6 and 7 V aprox
  case 2:
    {
      //PutString16(s,180,236,y,BLACK,SHECMEDGRAY);
      PutString16(s,260,316,y+16*0,BLACK,SHECMEDGRAY);
      // Display tilt in millimeters starting from 0.
      //sprintf(s,"%7.3lf",(6.0+AnalogData[2])*40.0/Kstepmm); //40 steps/volt obtained experimentally
      //PutString16(s,260,316,y,BLACK,SHECMEDGRAY);
      // Display a little bar graph, 1 pixel/volt
      x1 = TiltX - 4;
      x2 = TiltX + 4;
      y1 = TiltY - 41;
      y2 = TiltY - 23;
      datay = TiltY - 32 - data;
      //if (data < -6)
      if (data < -9)
	Box(x1,y1,x2,y2,BLACK);
      //else if (data > 7)
      else if (data > 8)
	Box(x1,y1,x2,y2,SHECGREEN);
      else
	{
	  Box(x1,y1,x2,datay-1,BLACK);
	  Box(x1,datay,x2,y2,SHECGREEN);
	}
    } break;
  }
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::DisplayMotion - Display motion status of given tilt stepper
void CTilt::DisplayMotion(int tilt)
// Tilt = 0 - 2, 0 = A, 1 = B, 2 = C
{
  int x = 148,y = 296 + tilt * 16;

  switch (Motion[tilt])
    {
    case TMStop:
      PutChar16(CUpDown,x,y,BLACK,SHECMEDGRAY);break;
    case TMOut:
      PutChar16(CUp,x,y,SHECGREEN,SHECMEDGRAY);break;
    case TMIn:
      PutChar16(CDown,x,y,SHECGREEN,SHECMEDGRAY);break;
    default:
      PutChar16(CUpDown,x,y,SHECRED,SHECMEDGRAY);break;
    }
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::DisplayStepper - Display stepper status of given tilt
void CTilt::DisplayStepper(int tilt)
// Tilt = 0 - 2, 0 = A, 1 = B, 2 = C
{
  int y = 96 + 16 * tilt;

  if (Dir[tilt])
    PutChar16('þ',468,y,SHECGREEN,SHECMEDGRAY);
  else
    PutChar16('þ',468,y,BLACK,SHECMEDGRAY);
  if (Stp[tilt])
    PutChar16('þ',500,y,SHECGREEN,SHECMEDGRAY);
  else
    PutChar16('þ',500,y,BLACK,SHECMEDGRAY);
  if (Mon[tilt])
    PutChar16('þ',532,y,SHECGREEN,SHECMEDGRAY);
  else
    PutChar16('þ',532,y,BLACK,SHECMEDGRAY);
  if (Pwr[tilt])
    PutChar16('þ',564,y,SHECGREEN,SHECMEDGRAY);
  else
    PutChar16('þ',564,y,BLACK,SHECMEDGRAY);
  if (Cur[tilt])
    PutChar16('þ',596,y,SHECGREEN,SHECMEDGRAY);
  else
    PutChar16('þ',596,y,BLACK,SHECMEDGRAY);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::MoveSteps - Move the turret the given number of steps
int CTilt::MoveSteps(int steps,int tilt,int ServoOK)
// Returns a 0 on success, 1 on move already in progress
// 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.
{
  // 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)) )

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

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

  if (Moving[tilt] || (!ServoOK && ServoFlag[tilt]))
    {
      EDSLog.Add(500,"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] = msTimer;
    }
  else
    {
      // Do we need to disengage the brakes?
      if (ReleaseBrakes())
	MoveStep[tilt] = 2;
      else
	{
	  BrakeTimer[tilt] = msTimer;
	  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
  SetParBit(6);
  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
      ClrParBit(6);
      return(0);
    }
  return(1);
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::Servo - Servo tilt to the given LVDT position
int CTilt::Servo(int tilt,float position)
// Servo the tilt (A,C or B) 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(500,"Tilt move ignored, move in progress");
      return(1);
    }

  ServoTimer[tilt] = Secs;
  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(500,"Tilt move ignored, move in progress");
      return(1);
    }

  ServoAllTimer = Secs;
  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