| ApossC SDK V01.15

ECAT_3Ax_Epos4-Test_csp.mc
#include "..\..\..\SDK\SDK_ApossC.mc"
// Parameters for the SDK function
#define C_AXIS1 0 // Axis module number
#define C_AXIS2 1 // Axis module number
#define C_AXIS3 2 // Axis module number
#define C_AXIS1_POLARITY 0 // Definition of the polarity 0: Normal, 1: Inverse
#define C_AXIS2_POLARITY 0 // Definition of the polarity 0: Normal, 1: Inverse
#define C_AXIS3_POLARITY 0 // Definition of the polarity 0: Normal, 1: Inverse
#define C_DRIVE_BUSID1 1000001 // The driveBusId is 1000000 plus the EtherCAT slave position in the bus
#define C_DRIVE_BUSID2 1000002 // The driveBusId is 1000000 plus the EtherCAT slave position in the bus
#define C_DRIVE_BUSID3 1000003 // The driveBusId is 1000000 plus the EtherCAT slave position in the bus
#define C_EC_CYCLE_TIME 1 // Cycletime in milliseconds
#define C_EC_OFFSET 1 // Shift offset
#define C_PDO_NUMBER 1 // Used PDO number
// Encoder settings & axis user units (MACS)
#define C_AXIS_ENCRES 4*1600 // Resolution of the encoder for position feed back in increments (quadcounts)
#define C_AXIS_POSENCREV 1 // Number of revolutions of the motor
#define C_AXIS_POSENCQC C_AXIS_ENCRES // Number of quadcounts in POSENCREV revolutions
#define C_AXIS_POSFACT_Z 1 // Number of revolutions of the input shaft
#define C_AXIS_POSFACT_N 1 // Number of revolutions of the output shaft in POSFACT_Z revolutions of the input shaft
#define C_AXIS_FEEDREV 1 // Number of revolutions of the gear box output shaft
#define C_AXIS_FEEDDIST C_AXIS_ENCRES // Distance travelled (in user units) in FEEDREV revolutions of the gear box output shaft [mm]
// Axis Movement Parameter
#define C_AXIS_MAX_RPM 2000 // Maximum velocity in RPM
#define C_AXIS_VELRES 100 // Velocity resolution, Scaling used for the velocity and acceleration/deceleration commands, default
#define C_AXIS_RAMPTYPE RAMPTYPE_JERKLIMITED // Defines the ramptype
#define C_AXIS_RAMPMIN 800 // Maximum acceleration
#define C_AXIS_JERKMIN 1000 // Minimum time (ms) required before reaching the maximum acceleration
#define C_AXIS_TRACKERR 0 // There is also a following error on EPOS4, could be set to zero on the MACS
// Axis MACS control loop settings
// MACS position control is not used
#define C_AXIS_KPROP 0
#define C_AXIS_KINT 0
#define C_AXIS_KDER 0
#define C_AXIS_KILIM 0
#define C_AXIS_KILIMTIME 0
#define C_AXIS_BANDWIDTH 1000
#define C_AXIS_FFVEL 1000
#define C_AXIS_KFFAC 0
#define C_AXIS_KFFDEC 0
// Set the bus ids of the all axis in the network
long axis[] = {C_AXIS1, C_AXIS2, C_AXIS3};
long busIds[] = {C_DRIVE_BUSID1, C_DRIVE_BUSID2, C_DRIVE_BUSID3};
long numberOfAxis = arraylen(busIds);
long main(void) {
long slaveCount, i,retval, axisIndex;
long homingStateAx_0=0,homingStateAx_1=0,homingStateAx_2=0;
print("-----------------------------------------------------------");
print(" Test application EtherCAT Master with 3 EPOS4 drive");
print("-----------------------------------------------------------");
ErrorClear();
AmpErrorClear(C_AXIS1); // Clear error on EPOS4
AmpErrorClear(C_AXIS2); // Clear error on EPOS4
AmpErrorClear(C_AXIS3); // Clear error on EPOS4
InterruptSetup(ERROR, ErrorHandler);
ECatMasterCommand(0x1000, 0);
//----------------------------------------------------------------
// Application Setup
//----------------------------------------------------------------
slaveCount = sdkEtherCATMasterInitialize();
print("slavecount: ",slaveCount);
// initialising maxon drives
sdkEpos4_SetupECatSdoParam(C_DRIVE_BUSID1, C_PDO_NUMBER, C_AXIS1_POLARITY, EPOS4_OP_CSP );
sdkEpos4_SetupECatSdoParam(C_DRIVE_BUSID2, C_PDO_NUMBER, C_AXIS2_POLARITY, EPOS4_OP_CSP );
sdkEpos4_SetupECatSdoParam(C_DRIVE_BUSID3, C_PDO_NUMBER, C_AXIS3_POLARITY, EPOS4_OP_CSP );
sdkEtherCATMasterDoMapping();
for (i = 1; i <= 3; i++) {
sdkEtherCATSetupDC(i, C_EC_CYCLE_TIME, C_EC_OFFSET); // Setup EtherCAT DC (cycle_time [ms], offset [us]
}
// starting the ethercat
sdkEtherCATMasterStart();
// All axis have in this example the same parameters
for(axisIndex = 0; axisIndex < numberOfAxis; axisIndex++ )
{
// setup EtherCAT bus module for csp mode
sdkEpos4_SetupECatBusModule(axis[axisIndex], busIds[axisIndex], C_PDO_NUMBER, EPOS4_OP_CSP);
// setup virtual amplifier for csp mode
sdkEpos4_SetupECatVirtAmp(axis[axisIndex], C_AXIS_MAX_RPM, EPOS4_OP_CSP);
// setup irtual counter for csp mode
sdkEpos4_SetupECatVirtCntin(axis[axisIndex], EPOS4_OP_CSP);
// Movement parameters for the axis
sdkSetupAxisMovementParam( axis[axisIndex],
C_AXIS_VELRES,
C_AXIS_MAX_RPM,
C_AXIS_RAMPTYPE,
C_AXIS_RAMPMIN,
C_AXIS_JERKMIN,
C_AXIS_TRACKERR
);
// Definition of the user units
sdkSetupAxisUserUnits( axis[axisIndex],
C_AXIS_POSENCREV,
C_AXIS_POSENCQC,
C_AXIS_POSFACT_Z,
C_AXIS_POSFACT_N,
C_AXIS_FEEDREV,
C_AXIS_FEEDDIST
);
// Position control setup
sdkSetupPositionPIDControlExt( axis[axisIndex],
C_AXIS_KPROP,
C_AXIS_KINT,
C_AXIS_KDER,
C_AXIS_KILIM,
C_AXIS_KILIMTIME,
C_AXIS_BANDWIDTH,
C_AXIS_FFVEL,
C_AXIS_KFFAC,
C_AXIS_KFFDEC
);
}
//----------------------------------------------------------------
// End of Application Setup
//----------------------------------------------------------------
ErrorClear();
//----------------------------------------------------------------
// Homing start
//----------------------------------------------------------------
// Homing setup
for(axisIndex = 0; axisIndex < numberOfAxis; axisIndex++ )
{
SdoWrite( busIds[axisIndex], EPOS4_HOMING_METHOD, 0, EPOS4_HOMING_ACT_POSITION); // 0x6098 Set homing method to “-4" : Homing Method -4 (Current Threshold Negative Speed).”
SdoWrite( busIds[axisIndex], EPOS4_HOMING_SPEEDS, 1, 20); // Homing Speed / Speed for switch speed [velocity units]
SdoWrite( busIds[axisIndex], EPOS4_HOMING_SPEEDS, 2, 20); // Homing Speed / Speed for zero search [velocity units]
SdoWrite( busIds[axisIndex], EPOS4_HOMING_ACCELERATION, 0, 20); // Homing acceleration [acceleration units]
SdoWrite( busIds[axisIndex], EPOS4_HOME_OFFSET_MOVE_DISTANCE, 0, 6400); // Home offset move distance [position units]
SdoWrite( busIds[axisIndex], EPOS4_HOME_POSITION, 0, 0); // Home position [position units]
SdoWrite( busIds[axisIndex], EPOS4_CURRENT_THRESHOLD_FOR_HOMING_MODE, 0, 1500); // Current threshold for homing mode [mA]
print("...: EPOS4 Homing AxisNo: ",axis," - Homing methode: ",SdoRead(busIds[axisIndex], EPOS4_HOMING_METHOD,0));
// Disable MACS trackerror for homing
AXE_PARAM(axisIndex,POSERR)=0;
}
// Homing statemachine
retval=0;
while(retval!=7)
{
retval.i[0] = sdkEpos4_AxisHomingStart(C_AXIS1, C_DRIVE_BUSID1, EPOS4_OP_CSP, homingStateAx_0);
retval.i[1] = sdkEpos4_AxisHomingStart(C_AXIS2, C_DRIVE_BUSID2, EPOS4_OP_CSP, homingStateAx_1);
retval.i[2] = sdkEpos4_AxisHomingStart(C_AXIS3, C_DRIVE_BUSID3, EPOS4_OP_CSP, homingStateAx_2);
}
AxisControl(C_AXIS1,ON, C_AXIS2,ON, C_AXIS3,ON);
print("-----------------------------------------------------------");
print(" Movement in CSP Mode ");
print("----------------------------------------------------------- \n");
Vel(C_AXIS1, 40, C_AXIS2, 40, C_AXIS3, 40);
Acc(C_AXIS1, 30, C_AXIS2, 30, C_AXIS3, 30);
Dec(C_AXIS1, 30, C_AXIS2, 30, C_AXIS3, 30);
for(i=10;i>=0;i--)
{
print("Start, move to target position");
AxisPosAbsStart( C_AXIS1, 20000 , C_AXIS2, 20000, C_AXIS3, 20000 );
AxisWaitReached(C_AXIS1,C_AXIS2,C_AXIS3);
print("Target position is reached \n");
print("Start, back to start position");
AxisPosAbsStart( C_AXIS1, 0, C_AXIS2, 0, C_AXIS3, 0);
AxisWaitReached(C_AXIS1,C_AXIS2,C_AXIS3);
print("Start position is reached");
print(i, " repetitions to go \n");
}
AxisControl(AXALL, OFF);
RecordStop(0, 0);
print("Program done, Axis OFF ");
return(0);
}
void ErrorHandler(void)
{
long axeNbr = ErrorAxis();
long errNbr = ErrorNo();
long errInfoNbr = ErrorInfo();
long sdoAbortCode, eposErr;
AxisControl(AXALL,OFF);
switch(errNbr)
{
case F_AMP: eposErr = SdoRead(C_DRIVE_BUSID1,EPOS4_ERROR_CODE,0x00);
printf("Error Axis: %d, Epos4 Error 0x%lX: ", axeNbr , eposErr);
sdkEpos4_PrintErrorDescription(eposErr);
print();
AmpErrorClear(axeNbr); // Clear error on EPOS4
break;
case F_CANIO: print("ErrorNo: ",errNbr," SlaveAddr(info): ",errInfoNbr);
ECatMasterInfo(0x1000, 22, sdoAbortCode);
printf("SDO Abort Code EtherCAT 0x%lX\n", sdoAbortCode);
break;
default: print("ErrorNo: ",errNbr," info: ",errInfoNbr, " AxisNo: ", axeNbr);
}
ErrorClear();
print(""); print(" There is no error handlig → Exit()");
Exit(0);
}
sdkEpos4_SetupECatVirtCntin
long sdkEpos4_SetupECatVirtCntin(long axis, long operationMode)
Setup the virtual counter input for an Epos4 with EtherCat.
Definition: SDK_Amplifier_Epos4.mc:168
sdkEtherCATMasterStart
long sdkEtherCATMasterStart(void)
Start an EtherCAT Master.
Definition: SDK_Bussystem_EtherCat.mc:86
EPOS4_HOMING_SPEEDS
#define EPOS4_HOMING_SPEEDS
Definition: SDK_Amplifier_Epos4.mh:169
sdkSetupAxisUserUnits
long sdkSetupAxisUserUnits(long axis, long posencrev, long posencqc, long posfact_z, long posfact_n, long feedrev, long feeddist)
Setup to convert the resolution of the machine in user units.
Definition: SDK_Axis_Setup.mc:104
sdkEtherCATMasterInitialize
long sdkEtherCATMasterInitialize(void)
Initialization of an EtherCAT master.
Definition: SDK_Bussystem_EtherCat.mc:23
sdkEpos4_AxisHomingStart
long sdkEpos4_AxisHomingStart(long axis, long busId, long operationMode, long &homingState)
State machine function for performing a homing on an EPOS4.
Definition: SDK_Amplifier_Epos4.mc:607
POSERR
#define POSERR
Maximum tolerated position error.
Definition: SdoDictionary.mh:2444
EPOS4_OP_CSP
#define EPOS4_OP_CSP
Definition: SDK_Amplifier_Epos4.mh:130
sdkEpos4_SetupECatBusModule
long sdkEpos4_SetupECatBusModule(long axis, long busId, long pdoNumber, long operationMode)
Setup the ECAT bus module for an Epos4.
Definition: SDK_Amplifier_Epos4.mc:37
EPOS4_HOME_POSITION
#define EPOS4_HOME_POSITION
Definition: SDK_Amplifier_Epos4.mh:93
sdkEpos4_SetupECatVirtAmp
long sdkEpos4_SetupECatVirtAmp(long axis, long maxRpm, long operationMode)
Setup the virtual amplifier for an Epos4 with EtherCat.
Definition: SDK_Amplifier_Epos4.mc:101
EPOS4_HOMING_ACT_POSITION
#define EPOS4_HOMING_ACT_POSITION
Definition: SDK_Amplifier_Epos4.mh:154
sdkEpos4_PrintErrorDescription
void sdkEpos4_PrintErrorDescription(long errorCode)
Prints a error description for Epos4 errors.
Definition: SDK_Amplifier_Epos4.mc:735
EPOS4_HOME_OFFSET_MOVE_DISTANCE
#define EPOS4_HOME_OFFSET_MOVE_DISTANCE
Definition: SDK_Amplifier_Epos4.mh:94
AXE_PARAM
#define AXE_PARAM(modno, parno)
Axis Parameters: Setter.
Definition: SdoDictionary.mh:2293
sdkEpos4_SetupECatSdoParam
long sdkEpos4_SetupECatSdoParam(long busId, long pdoNumber, long axisPolarity, long operationMode)
Setup the Sdo parameter for an Epos4 with EtherCat.
Definition: SDK_Amplifier_Epos4.mc:209
EPOS4_HOMING_ACCELERATION
#define EPOS4_HOMING_ACCELERATION
Definition: SDK_Amplifier_Epos4.mh:170
sdkEtherCATSetupDC
long sdkEtherCATSetupDC(long slaveNo, long cycleTime_ms, long offset_us)
Sets the DC cycle for an individual slave.
Definition: SDK_Bussystem_EtherCat.mc:67
EPOS4_HOMING_METHOD
#define EPOS4_HOMING_METHOD
Definition: SDK_Amplifier_Epos4.mh:153
sdkSetupPositionPIDControlExt
long sdkSetupPositionPIDControlExt(long axis, long kprop, long kint, long kder, long kilim, long kilimtime, long bandwidth, long ffvel, long kffacc, long kffdec)
Set extended parameters for PID position control loop.
Definition: SDK_Axis_Setup.mc:65
sdkSetupAxisMovementParam
long sdkSetupAxisMovementParam(long axis, long velres, long maxRpm, long ramptype, long rampmin, long jerkmin, long poserr)
Defines parameters which are required for the calculation of ramps and velocities.
Definition: SDK_Axis_Setup.mc:136
EPOS4_CURRENT_THRESHOLD_FOR_HOMING_MODE
#define EPOS4_CURRENT_THRESHOLD_FOR_HOMING_MODE
Definition: SDK_Amplifier_Epos4.mh:95
sdkEtherCATMasterDoMapping
long sdkEtherCATMasterDoMapping(void)
Mapping of EtherCAT Master IO Image.
Definition: SDK_Bussystem_EtherCat.mc:46
EPOS4_ERROR_CODE
#define EPOS4_ERROR_CODE
Definition: SDK_Amplifier_Epos4.mh:119

Data Sheets | Released Software | Software Manuals | Hardware Manuals | Maxon Shop

Maxon Support Center