Skip to content. | Skip to navigation

Personal tools

remote/tilt.cpp

remote/tilt.cpp

remote/tilt.cpp

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

/////////////////////////////////////////////////////////////////////////////
// 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;
		Brk[i] = -1;
		//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);
    }
		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;

	// Initialize analogcount, which identifies which LVDT reading is
	// being displayed
	analogcount = 0;

	for (i=0;i<=3;i++)
		OldAnalogData[i] = -999.99;
  // Steps per millimeter ratio (steps/mm)
	// for tilt fine adjustement stepper motors
	// (each of three considered the same)
	Kstepmm = 118.110;
  // Constants defined by the geometry of the M3, all in millimeters
  KKA = 381.0;
  KKB = 136.73;
  KKC = 216.03;
  // We're not servoing at startup
  ServoFlag = 0;
*/
}

/////////////////////////////////////////////////////////////////////////////
// CTilt::Update - Update all motion, status, displays
void CTilt::Update()
{
	int i;
	char s[100];
/*
	int e;								// Stores error codes
  int ServoDone;				// Check to see if servo's done
	float err;						// Servo error
	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())
            {
	            BrakeTimer[i] = msTimer;
	            MoveStep[i] = 1;
            }
            else
            	MoveStep[i] = 2;
          }
        } 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;
					// 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
  if (ServoFlag)
  {
  	// Did the servo time out?
    if (Secs - ServoTimer > TiltServoTimeout)
    	ServoFlag = 0;
    // Did the servo reach its destination?
    else
    {
	  	ServoDone = 1;
		  for (i=0;i<=2;i++)
	    	if (fabs(ServoTarget[i]-AnalogData[i]) >= TiltServoError)
	      	ServoDone = 0;
  	  if (ServoDone)
    		ServoFlag = 0;
    }
    // OK, make any moves we need (if possible)
    if (ServoFlag)
		  for (i=0;i<=2;i++)
      {
      	err = ServoTarget[i]-AnalogData[i];
        if ((fabs(err) >= TiltServoError) && !Moving[i])
        	MoveSteps(err*TiltServoRatio,i,1);
      }
  }

*/
	// 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);
	}
/*
  // for now all 3 brakes are wired togheter,
  // so brake status for 3 tilts is the same
  if (OldBrk[0] != Brk[0])
*/
		DisplayBrake();
/*

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

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

/////////////////////////////////////////////////////////////////////////////
// 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 (Brk[0])
		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 = 168;
	int x1,x2,y1,y2,data,datay;

	sprintf(s,"%7.3lf",AnalogData[0]);
	data = round(AnalogData[0]);
/*
	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*0,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:
		{
*/
	sprintf(s,"%7.3lf",AnalogData[1]);
	data = round(AnalogData[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:
		{
*/
	sprintf(s,"%7.3lf",AnalogData[2]);
	data = round(AnalogData[2]);
			//PutString16(s,180,236,y,BLACK,SHECMEDGRAY);
			PutString16(s,260,316,y+16*2,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;
	}
*/
	sprintf(s,"%7.3lf",AnalogData[3]);
	PutString16(s,260,316,y+16*3,BLACK,SHECMEDGRAY);

}

/////////////////////////////////////////////////////////////////////////////
// CTilt::DisplayMotion - Display motion status of given tilt stepper
void CTilt::DisplayMotion(int tilt)
// Tilt = 0 - 2, 0 = A, 1 = C, 2 = B
{
	int x = 148,y = 168;
	if (tilt != 0)
		y = y + 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 = 32 + 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);
}

Generated by GNU Enscript 1.6.5.2.
Document Actions