Hi, Tom!
Now this axis works good, we tuned it.
Now we trying to set up MPG like this:
We trying MPG examples in C programs folder, but nothing succeeded. Last tested is MPGServiceSmoothHardwareEnc. compiles good.
For testing we changed axis to 4 (MPG connected as on picture)
Also we changed ENABLE_MPG, HANDWHEEL_EN, SELECT adn FACTOR bits to virtual.
As well we can see what encoder works after connecting to Kanalog (see picture):
Code:
#include "KMotionDef.h"
#define DIR_BIT0_POS 144
#define DIR_BIT0_NEG 145
#define MPG_INPUT_AXIS 4
#define TAU 0.08 // smoothness factor (Low Pass Time constant seconds)
#define FINAL_TIME 1.0 // Set final dest after this amount of time with no change
#define ENABLE_MPG 1056
#define SELECTX 1024
#define SELECTY 1025
#define SELECTZ 1026
#define SELECTA 1027
#define FACTOR1 1040
#define FACTOR10 1041
#define FACTOR100 1042
#define FACTOR200 1043
#define HANDWHEEL_EN 1056
main()
{
int BitA,Change1=0,Change2=0, DiffX2;
int PosNoWrap, NewPos, Pos=0, wraps;
double Factor=0;
SetBit(152);
SetBit(157);
SetBit(158);
ch0->InputMode=ENCODER_MODE;
ch0->OutputMode=DAC_SERVO_MODE;
ch0->Vel=100000;
ch0->Accel=2e+006;
ch0->Jerk=2e+006;
ch0->P=1;
ch0->I=0;
ch0->D=0;
ch0->FFAccel=0;
ch0->FFVel=0;
ch0->MaxI=200;
ch0->MaxErr=40000;
ch0->MaxOutput=2000;
ch0->DeadBandGain=1;
ch0->DeadBandRange=0;
ch0->InputChan0=0;
ch0->InputChan1=0;
ch0->OutputChan0=0;
ch0->OutputChan1=0;
ch0->MasterAxis=-1;
ch0->LimitSwitchOptions=0x100;
ch0->LimitSwitchNegBit=0;
ch0->LimitSwitchPosBit=0;
ch0->SoftLimitPos=1e+009;
ch0->SoftLimitNeg=-1e+009;
ch0->InputGain0=1;
ch0->InputGain1=1;
ch0->InputOffset0=0;
ch0->InputOffset1=0;
ch0->OutputGain=1;
ch0->OutputOffset=0;
ch0->SlaveGain=1;
ch0->BacklashMode=BACKLASH_OFF;
ch0->BacklashAmount=0;
ch0->BacklashRate=0;
ch0->invDistPerCycle=0.00025;
ch0->Lead=0;
ch0->MaxFollowingError=10000000;
ch0->StepperAmplitude=100;
ch0->iir[0].B0=0.027497;
ch0->iir[0].B1=0.027497;
ch0->iir[0].B2=0.000000;
ch0->iir[0].A1=0.945006;
ch0->iir[0].A2=0.000000;
ch0->iir[1].B0=1.000000;
ch0->iir[1].B1=0.000000;
ch0->iir[1].B2=0.000000;
ch0->iir[1].A1=0.000000;
ch0->iir[1].A2=0.000000;
ch0->iir[2].B0=1.000000;
ch0->iir[2].B1=0.000000;
ch0->iir[2].B2=0.000000;
ch0->iir[2].A1=0.000000;
ch0->iir[2].A2=0.000000;
ch1->InputMode=ENCODER_MODE;
ch1->OutputMode=DAC_SERVO_MODE;
ch1->Vel=100000;
ch1->Accel=2e+006;
ch1->Jerk=2e+006;
ch1->P=1;
ch1->I=0;
ch1->D=0;
ch1->FFAccel=0;
ch1->FFVel=0;
ch1->MaxI=2000;
ch1->MaxErr=40000;
ch1->MaxOutput=2000;
ch1->DeadBandGain=1;
ch1->DeadBandRange=0;
ch1->InputChan0=1;
ch1->InputChan1=1;
ch1->OutputChan0=1;
ch1->OutputChan1=1;
ch1->MasterAxis=-1;
ch1->LimitSwitchOptions=0x100;
ch1->LimitSwitchNegBit=0;
ch1->LimitSwitchPosBit=0;
ch1->SoftLimitPos=1e+009;
ch1->SoftLimitNeg=-1e+009;
ch1->InputGain0=-1;
ch1->InputGain1=-1;
ch1->InputOffset0=0;
ch1->InputOffset1=0;
ch1->OutputGain=1;
ch1->OutputOffset=0;
ch1->SlaveGain=1;
ch1->BacklashMode=BACKLASH_OFF;
ch1->BacklashAmount=0;
ch1->BacklashRate=0;
ch1->invDistPerCycle=0.00025;
ch1->Lead=0;
ch1->MaxFollowingError=10000000;
ch1->StepperAmplitude=100;
ch1->iir[0].B0=0.002819;
ch1->iir[0].B1=0.002819;
ch1->iir[0].B2=0.000000;
ch1->iir[0].A1=0.994361;
ch1->iir[0].A2=0.000000;
ch1->iir[1].B0=1.000000;
ch1->iir[1].B1=0.000000;
ch1->iir[1].B2=0.000000;
ch1->iir[1].A1=0.000000;
ch1->iir[1].A2=0.000000;
ch1->iir[2].B0=1.000000;
ch1->iir[2].B1=0.000000;
ch1->iir[2].B2=0.000000;
ch1->iir[2].A1=0.000000;
ch1->iir[2].A2=0.000000;
ch2->InputMode=ENCODER_MODE;
ch2->OutputMode=DAC_SERVO_MODE;
ch2->Vel=100000;
ch2->Accel=1e+006;
ch2->Jerk=1e+006;
ch2->P=1;
ch2->I=0;
ch2->D=0;
ch2->FFAccel=0;
ch2->FFVel=0;
ch2->MaxI=2000;
ch2->MaxErr=40000;
ch2->MaxOutput=2000;
ch2->DeadBandGain=1;
ch2->DeadBandRange=0;
ch2->InputChan0=2;
ch2->InputChan1=2;
ch2->OutputChan0=2;
ch2->OutputChan1=2;
ch2->MasterAxis=-1;
ch2->LimitSwitchOptions=0x100;
ch2->LimitSwitchNegBit=0;
ch2->LimitSwitchPosBit=0;
ch2->SoftLimitPos=1e+009;
ch2->SoftLimitNeg=-1e+009;
ch2->InputGain0=-1;
ch2->InputGain1=-1;
ch2->InputOffset0=0;
ch2->InputOffset1=0;
ch2->OutputGain=-1;
ch2->OutputOffset=0;
ch2->SlaveGain=1;
ch2->BacklashMode=BACKLASH_OFF;
ch2->BacklashAmount=0;
ch2->BacklashRate=0;
ch2->invDistPerCycle=0.00025;
ch2->Lead=0;
ch2->MaxFollowingError=10000000;
ch2->StepperAmplitude=100;
ch2->iir[0].B0=0.002819;
ch2->iir[0].B1=0.002819;
ch2->iir[0].B2=0.000000;
ch2->iir[0].A1=0.994361;
ch2->iir[0].A2=0.000000;
ch2->iir[1].B0=1.000000;
ch2->iir[1].B1=0.000000;
ch2->iir[1].B2=0.000000;
ch2->iir[1].A1=0.000000;
ch2->iir[1].A2=0.000000;
ch2->iir[2].B0=1.000000;
ch2->iir[2].B1=0.000000;
ch2->iir[2].B2=0.000000;
ch2->iir[2].A1=0.000000;
ch2->iir[2].A2=0.000000;
ch3->InputMode=ENCODER_MODE;
ch3->OutputMode=NO_OUTPUT_MODE;
ch3->Vel=100000;
ch3->Accel=100000;
ch3->Jerk=2e+07;
ch3->P=0.05;
ch3->I=0;
ch3->D=1;
ch3->FFAccel=0;
ch3->FFVel=0;
ch3->MaxI=2000;
ch3->MaxErr=400000000000;
ch3->MaxOutput=2000;
ch3->DeadBandGain=1;
ch3->DeadBandRange=0;
ch3->InputChan0=3;
ch3->InputChan1=0;
ch3->OutputChan0=3;
ch3->OutputChan1=0;
ch3->MasterAxis=-1;
ch3->LimitSwitchOptions=0x100;
ch3->LimitSwitchNegBit=0;
ch3->LimitSwitchPosBit=0;
ch3->SoftLimitPos=1e+09;
ch3->SoftLimitNeg=-1e+09;
ch3->InputGain0=1;
ch3->InputGain1=1;
ch3->InputOffset0=0;
ch3->InputOffset1=0;
ch3->OutputGain=1;
ch3->OutputOffset=0;
ch3->SlaveGain=1;
ch3->BacklashMode=BACKLASH_OFF;
ch3->BacklashAmount=0;
ch3->BacklashRate=0;
ch3->invDistPerCycle=1;
ch3->Lead=0;
ch3->MaxFollowingError=10000000000;
ch3->StepperAmplitude=100;
ch3->iir[0].B0=0.00141172;
ch3->iir[0].B1=0.00141172;
ch3->iir[0].B2=0.000000;
ch3->iir[0].A1=0.997177;
ch3->iir[0].A2=0.000000;
ch3->iir[1].B0=1.000000;
ch3->iir[1].B1=0.000000;
ch3->iir[1].B2=0.000000;
ch3->iir[1].A1=0.000000;
ch3->iir[1].A2=0.000000;
ch3->iir[2].B0=1.000000;
ch3->iir[2].B1=0.000000;
ch3->iir[2].B2=0.000000;
ch3->iir[2].A1=0.000000;
ch3->iir[2].A2=0.000000;
EnableAxis(0);
EnableAxis(1);
EnableAxis(2);
EnableAxis(3);
DefineCoordSystem(0,1,2,3);
// If you're using fixed outputs (I.e. Kanalog output optos/FET relay drivers), you don't need to set bit direction, so the following two lines can be removed, but you will still need to modify the defines at the top of the file
SetBitDirection(DIR_BIT0_POS,1); // You will need to rename DIR_BIT0 to DIR_BIT0_POS at the top of the file
SetBitDirection(DIR_BIT0_NEG,1); // You will need to add a define for DIR_BIT0_NEG at the top of the file
for (;;) //loop forever
{
WaitNextTimeSlice();
OutputSignMag(0,DIR_BIT0_POS,DIR_BIT0_NEG); // This calls, and passes the required values to the function
}
}
// write the servo output value to the relative DAC, and activate/deactivate direction bits depending on negative/positive output
void OutputSignMag() //(function and args)
{
if (chan[3].Enable)
{
if (chan[3].Output >= 0)
{
DAC(3, (int)chan[3].Output*-1); // Set DAC output, using channel output
ClearBit(DIR_BIT0_NEG); // Clear negative direction bit/output
SetBit(DIR_BIT0_POS); // Set positive direction bit/output //(set value '1' to 'dirbit' )
}
else // else our channel output is negative
{
DAC(3, (int)chan[3].Output); // As we need an always positive DAC, we multiply by -1
ClearBit(DIR_BIT0_POS); // Clear positive direction bit/output
SetBit(DIR_BIT0_NEG); // Set negative direction bit/output
}
}
else // else the channel is disabled so set DAC to zero, and disable both outputs
{
DAC(3, 0);
ClearBit(DIR_BIT0_POS);
ClearBit(DIR_BIT0_NEG);
}
}
void ServiceMPG()
{
static int Pos, FirstTime=TRUE;
static int InMotion=FALSE,Axis,LastAxis=-1;
static double LastChangeTime=0,Target,Factor=0;
int Change1, NewPos;
if (FirstTime)
{
Pos = chan[MPG_INPUT_AXIS].Position;
FirstTime=FALSE;
}
NewPos = chan[MPG_INPUT_AXIS].Position;
Change1 = NewPos - Pos;
Pos = NewPos;
if (!ReadBit(HANDWHEEL_EN) || JOB_ACTIVE) // if not button pressed or Job Active ignore the encoder.
Change1 = 0;
else if (ReadBit(FACTOR1)) // is X1 selected?
Factor = 4;
else if (ReadBit(FACTOR10)) // is X10 selected?
Factor = 20;
else if (ReadBit(FACTOR100)) // is X100 selected?
Factor = 40;
else if (ReadBit(FACTOR200)) // is X200 selected?
Factor = 400;
else
Factor = 0.0;
if (ReadBit(SELECTX)) // is x selected?
Axis=0;
else if (ReadBit(SELECTY)) // is y selected?
Axis=1;
else if (ReadBit(SELECTZ)) // is z selected?
Axis=2;
else if (ReadBit(SELECTA)) // is A selected?
Axis=3;
// check if the Axis just changed or we have been
// converging to the target for a long time
if (Axis != LastAxis ||
(InMotion && Time_sec() > LastChangeTime+FINAL_TIME))
{
if (InMotion)
Move(LastAxis,Target); //finalize any motion
LastAxis = Axis;
InMotion = FALSE;
}
if (Change1) // did we move?
{
if (!InMotion) Target = chan[Axis].Dest;
Target += Change1 * Factor;
MoveExp(Axis,Target,TAU); // note: contains a WaitNextTimeSlice
LastChangeTime = Time_sec();
InMotion=TRUE;
}
else
{
WaitNextTimeSlice();
}
}