Hi Tom,
I have gathered all (i think) the information you asked for. I included the raw data for all 3 axes , the code that generates the tach (it constantly loops in thread 1 after setting up the axes. Within that loop I also check a few bits to make sure all switches are in the correct state.)
I believe the machine (Fanuc 5m) originally has the following capabilities : 400 IPM , 0.0001 inch precision, unsure about acceleration?
The drives are indeed of Velocity type, with the dac sending voltage straight to the Velocity Control Unit VCMD wire.
The tach is generated based on position and time, with the dac connected to the TSA wire.
I got the data from a 40k steps move and a 75k steps move in all axes.
You might noticed some comments in the tach code are in french , as I am French Canadian , we tend to mix both languages as we do things.. Although perfectly fluent in English , sometimes my comprehension of more technical things (especially things I'm not familiar with) takes some time..
If there's anything more you need, let me know. If you have documentation I should read to grasp all the tuning concept better, I'd definitely look into it.
I am unsure how you plan to plot errors ? By looking at the graph or the raw data?
Your help is greatly appreciated, as always!
Code: Select all
#include "KMotionDef.h"
// Constantes a modifier
#define POSITION_PER_ROTATION 8000
#define RESOLUTION_OUTPUT_PWM 2048
#define VOLT_PER_RPM 0.003 // 1000 PRM for 3v
// Constante utilise durant le programme
#define INCREMENT_PER_VOLT (RESOLUTION_OUTPUT_PWM/10) // e.g. 2048 increments divise sur 10v
// Get the position of the encoder of channel 0 (Axis X)
double initPosX = ch0->Position;
// Get the time since startup in seconds (chiffre a virgule, ex. 0.009 secondes)
double initTimeX = Time_sec();
// Get the position of the encoder of channel 1 (Axis Y)
double initPosY = ch1->Position;
// Get the time since startup in seconds (chiffre a virgule, ex. 0.009 secondes)
double initTimeY = Time_sec();
// Get the position of the encoder of channel 1 (Axis Z)
double initPosZ = ch2->Position;
// Get the time since startup in seconds (chiffre a virgule, ex. 0.009 secondes)
double initTimeZ = Time_sec();
// Initiliaze the variable used in the main loop for Axis X
double tLastExecutionX = initTimeX, tCurrentExecutionX = initTimeX;
double posLastExecutionX = initPosX, posCurrentExecutionX = initPosX;
// Initiliaze the variable used in the main loop for Axis Y
double tLastExecutionY = initTimeY, tCurrentExecutionY = initTimeY;
double posLastExecutionY = initPosY, posCurrentExecutionY = initPosY;
// Initiliaze the variable used in the main loop for Axis Z
double tLastExecutionZ = initTimeZ, tCurrentExecutionZ = initTimeZ;
double posLastExecutionZ = initPosZ, posCurrentExecutionZ = initPosZ;
while(1)
{
// Get the time and the position at the start of the execution window of the thread for Axis X
posCurrentExecutionX = ch0->Position;
tCurrentExecutionX = Time_sec();
// Get the time and the position at the start of the execution window of the thread for Axis Y
posCurrentExecutionY = ch1->Position;
tCurrentExecutionY = Time_sec();
// Get the time and the position at the start of the execution window of the thread for Axis Z
posCurrentExecutionZ = ch2->Position;
tCurrentExecutionZ = Time_sec();
// Compute the current RPM of the encoder for Axis X
// RPM = step/sec * R/step * sec/min
double stepPerSecX = (posCurrentExecutionX-posLastExecutionX)/(tCurrentExecutionX - tLastExecutionX);
double encoderRPMX = (stepPerSecX / POSITION_PER_ROTATION) * 60;
// Compute the current RPM of the encoder for Axis Y
// RPM = step/sec * R/step * sec/min
double stepPerSecY = (posCurrentExecutionY-posLastExecutionY)/(tCurrentExecutionY - tLastExecutionY);
double encoderRPMY = (stepPerSecY / POSITION_PER_ROTATION) * 60;
// Compute the current RPM of the encoder for Axis Z
// RPM = step/sec * R/step * sec/min
double stepPerSecZ = (posCurrentExecutionZ-posLastExecutionZ)/(tCurrentExecutionZ - tLastExecutionZ);
double encoderRPMZ = (stepPerSecZ / POSITION_PER_ROTATION) * 60;
// Compute the required dacValue for Axis X
// voltage = RPM * v/RPM * increment/v
int dacValueX = (int)(encoderRPMX * VOLT_PER_RPM * INCREMENT_PER_VOLT);
// Compute the required dacValue for Axis Y
// voltage = RPM * v/RPM * increment/v
int dacValueY = (int)(encoderRPMY * VOLT_PER_RPM * INCREMENT_PER_VOLT);
// Compute the required dacValue for Axis Z
// voltage = RPM * v/RPM * increment/v
int dacValueZ = (int)(encoderRPMZ * VOLT_PER_RPM * INCREMENT_PER_VOLT);
// Write the dac value in the required output for Axis X
DAC(3, dacValueX);
// Write the dac value in the required output for Axis Y
DAC(4, dacValueY);
// Write the dac value in the required output for Axis Z
DAC(5, dacValueZ);
// Set the variable for the new execution window of the thread for Axis X
posLastExecutionX = posCurrentExecutionX;
tLastExecutionX = tCurrentExecutionX;
// Set the variable for the new execution window of the thread for Axis Y
posLastExecutionY = posCurrentExecutionY;
tLastExecutionY = tCurrentExecutionY;
// Set the variable for the new execution window of the thread for Axis Y
posLastExecutionZ = posCurrentExecutionZ;
tLastExecutionZ = tCurrentExecutionZ;
}
