1

/*

     2

     3

Template C code used to produce target Ethercat C CIA402 code

     4

     5

Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT

     6

                         RTES Lab : CRKim, JBLee, youcu

     7

                         Higen Motor : Donggu Kang

     8

     9

Distributed under the terms of the GNU Lesser General Public License as

    10

published by the Free Software Foundation; either version 2 of the License, or

    11

(at your option) any later version.

    12

    13

See COPYING file for copyrights details.

    14

    15

*/

    16

    17

#include "ecrt.h"

    18

    19

#include "beremiz.h"

    20

#include "iec_types_all.h"

    21

    22

#include "accessor.h"

    23

#include "POUS.h"

    24

    25

/* From CiA402, page 27

    26

    27

        Table 30 - State coding

    28

    Statusword      |      PDS FSA state

    29

xxxx xxxx x0xx 0000 | Not ready to switch on

    30

xxxx xxxx x1xx 0000 | Switch on disabled

    31

xxxx xxxx x01x 0001 | Ready to switch on

    32

xxxx xxxx x01x 0011 | Switched on

    33

xxxx xxxx x01x 0111 | Operation enabled

    34

xxxx xxxx x00x 0111 | Quick stop active

    35

xxxx xxxx x0xx 1111 | Fault reaction active

    36

xxxx xxxx x0xx 1000 | Fault

    37

*/

    38

    39

//ssh_add

    40

/* From CiA402, Page 63 Statusword for homing mode

    41

    42

		Table 106 - Definition of bit 10, bit 12, bit 13

    43

    44

xx00 x0xx xxxx xxxx | Homing procedure is in progress

    45

xx00 x1xx xxxx xxxx | Homing procedure is interrupted or not started

    46

xx01 x0xx xxxx xxxx | Homing is attained, but target is not reached

    47

xx01 x1xx xxxx xxxx | Homing procedure is completed successfully

    48

xx10 x0xx xxxx xxxx | Homing error occurred, velocity is not 0

    49

xx10 x1xx xxxx xxxx | Homing error occurred, velocity is 0

    50

xx11 xxxx xxxx xxxx | reserved

    51

*/

    52

    53

#define FSAFromStatusWord(SW) (SW & 0x006f)

    54

//ssh_add

    55

#define HomingStatusWord(SW) (SW & 0x3400)

    56

#define FaultFromStatusWord(SW) (SW & 0x0008)

    57

#define NotReadyToSwitchOn  0b00000000 FSA_sep 0b00100000

    58

#define SwitchOnDisabled    0b01000000 FSA_sep 0b01100000

    59

#define ReadyToSwitchOn     0b00100001

    60

#define SwitchedOn          0b00100011

    61

#define OperationEnabled    0b00100111

    62

#define QuickStopActive     0b00000111

    63

#define FaultReactionActive 0b00001111 FSA_sep 0b00101111

    64

#define Fault               0b00001000 FSA_sep 0b00101000

    65

    66

//ssh_add

    67

#define HomingInProgress	0b0000000000000000

    68

#define HomingNotRunning	0b0000010000000000

    69

#define HomingNotReached	0b0001000000000000

    70

#define Homing_Completed	0b0001010000000000

    71

#define HomingErrorInVelo	0b0010000000000000

    72

#define HomingErrorNotVelo	0b0010010000000000

    73

#define HomingReserved		0b0011000000000000 FSA_sep 0b0011010000000000

    74

    75

// StatusWord bits :

    76

#define SW_ReadyToSwitchOn     0x0001

    77

#define SW_SwitchedOn          0x0002

    78

#define SW_OperationEnabled    0x0004

    79

#define SW_Fault               0x0008

    80

#define SW_VoltageEnabled      0x0010

    81

#define SW_QuickStop           0x0020

    82

#define SW_SwitchOnDisabled    0x0040

    83

#define SW_Warning             0x0080

    84

#define SW_Remote              0x0200

    85

#define SW_TargetReached       0x0400

    86

#define SW_InternalLimitActive 0x0800

    87

    88

//ssh_add

    89

#define SW_HomingAttained		0x1000

    90

#define SW_HomingError			0x2000

    91

    92

// ControlWord bits :

    93

#define SwitchOn        0x0001

    94

#define EnableVoltage   0x0002

    95

#define QuickStop       0x0004

    96

#define EnableOperation 0x0008

    97

#define FaultReset      0x0080

    98

#define Halt            0x0100

    99

   100

//ssh_add

   101

//#define Homing_OperationStart 0x0010

   102

#define Homing_OperationStart_Origin 0x0010

   103

#define Homing_OperationStart_Edit 0x001F

   104

   105

IEC_INT beremiz__IW%(location)s = %(slave_pos)s;

   106

IEC_INT *__IW%(location)s = &beremiz__IW%(location)s;

   107

IEC_INT beremiz__IW%(location)s_402;

   108

IEC_INT *__IW%(location)s_402 = &beremiz__IW%(location)s_402;

   109

   110

%(MCL_headers)s

   111

   112

static IEC_BOOL __FirstTick = 1;

   113

   114

typedef enum {

   115

    mc_mode_none, // No motion mode

   116

    mc_mode_csp,  // Continuous Synchronous Positionning mode

   117

    mc_mode_csv,  // Continuous Synchronous Velocity mode

   118

    mc_mode_cst,  // Continuous Synchronous Torque mode

   119

} mc_axismotionmode_enum;

   120

   121

typedef struct {

   122

   IEC_BOOL Power;

   123

   IEC_BOOL CommunicationReady;

   124

   IEC_UINT NetworkPosition;

   125

   IEC_BOOL ReadyForPowerOn;

   126

   IEC_BOOL PowerFeedback;

   127

   IEC_DINT ActualRawPosition;

   128

   IEC_DINT ActualRawVelocity;

   129

   IEC_DINT ActualRawTorque;

   130

   IEC_DINT RawPositionSetPoint;

   131

   IEC_DINT RawVelocitySetPoint;

   132

   IEC_DINT RawTorqueSetPoint;

   133

   mc_axismotionmode_enum AxisMotionMode;

   134

   IEC_LREAL ActualVelocity;

   135

   IEC_LREAL ActualPosition;

   136

   IEC_LREAL ActualTorque;

   137

}axis_s;

   138

   139

typedef struct {

   140

%(entry_variables)s

   141

    axis_s* axis;

   142

} __CIA402Node;

   143

   144

#define AxsPub __CIA402Node_%(location)s

   145

   146

static __CIA402Node AxsPub;

   147

   148

%(extern_located_variables_declaration)s

   149

   150

%(fieldbus_interface_declaration)s

   151

   152

int __init_%(location)s()

   153

{

   154

    __FirstTick = 1;

   155

%(init_entry_variables)s

   156

    return 0;

   157

}

   158

   159

void __cleanup_%(location)s()

   160

{

   161

}

   162

   163

void __retrieve_%(location)s()

   164

{

   165

	if (__FirstTick) {

   166

		*__IW%(location)s_402 = __MK_Alloc_AXIS_REF();

   167

		AxsPub.axis = 

   168

            __MK_GetPublic_AXIS_REF(*__IW%(location)s_402);

   169

		AxsPub.axis->NetworkPosition = beremiz__IW%(location)s;

   170

%(init_axis_params)s

   171

%(fieldbus_interface_definition)s

   172

		__FirstTick = 0;

   173

	}

   174

   175

	// Default variables retrieve

   176

	AxsPub.axis->CommunicationReady = 

   177

        *(AxsPub.StatusWord) != 0;

   178

#define FSA_sep || FSA ==

   179

    {

   180

        uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord));

   181

        AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn;

   182

        AxsPub.axis->PowerFeedback = FSA == OperationEnabled;

   183

    }

   184

#undef FSA_sep 

   185

%(default_variables_retrieve)s

   186

   187

	// Extra variables retrieve

   188

%(extra_variables_retrieve)s

   189

}

   190

   191

void __publish_%(location)s()

   192

{

   193

	IEC_BOOL power = 

   194

        ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) 

   195

        && AxsPub.axis->Power;

   196

    uint16_t CW = *(AxsPub.ControlWord);

   197

   198

#define FSA_sep : case

   199

	// CIA402 node state transition computation

   200

	switch (FSAFromStatusWord(*(AxsPub.StatusWord))) {

   201

	    case SwitchOnDisabled :

   202

            CW &= ~(SwitchOn | FaultReset);

   203

            CW |= EnableVoltage | QuickStop;

   204

	    	break;

   205

	    case ReadyToSwitchOn :

   206

	    case OperationEnabled :

   207

	    	if (!power) {

   208

                CW &= ~(FaultReset | EnableOperation);

   209

                CW |= SwitchOn | EnableVoltage | QuickStop;

   210

	    		break;

   211

	    	}

   212

	    case SwitchedOn :

   213

	    	if (power) {

   214

                CW &= ~(FaultReset);

   215

                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;

   216

	    	}

   217

	    	break;

   218

			//ssh_check

   219

//	    case Fault :

   220

//            /* TODO reset fault only when MC_Reset */

   221

//	    	AxsPub.axis->DriveFault = 1;

   222

//            CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);

   223

//            CW |= FaultReset;

   224

//	    	break;

   225

	    default:

   226

	    	break;

   227

	}

   228

	//ssh_add

   229

	if(FaultFromStatusWord(*(AxsPub.StatusWord)) == SW_Fault)

   230

		AxsPub.axis->DriveFault = 1;

   231

	else{

   232

		AxsPub.axis->DriveFault = 0;

   233

		AxsPub.axis->DriveFaultReset = 0;

   234

	}

   235

	if(AxsPub.axis->DriveFaultReset){

   236

		CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);

   237

		CW |= FaultReset;

   238

	}

   239

   240

	//ssh_add

   241

	switch (HomingStatusWord(*(AxsPub.StatusWord))) {

   242

		case HomingInProgress:

   243

			break;

   244

		case HomingNotRunning:

   245

			break;

   246

		case HomingNotReached:

   247

			break;

   248

		case Homing_Completed:

   249

			if(!AxsPub.axis->HomingCompleted)

   250

				AxsPub.axis->HomingCompleted = 1;

   251

			break;

   252

		case HomingErrorInVelo:

   253

		case HomingErrorNotVelo:

   254

			if(!AxsPub.axis->HomingCompleted)

   255

				AxsPub.axis->HomingCompleted = 1;

   256

			break;

   257

		case HomingReserved:

   258

			break;

   259

	}

   260

#undef FSA_sep 

   261

   262

	//ssh_add

   263

%(modeofop_homing_method)s

   264

   265

	*(AxsPub.ControlWord) = CW;

   266

   267

   268

	// CIA402 node modes of operation computation according to axis motion mode

   269

%(modeofop_computation_mode)s

   270

   271

	// Default variables publish

   272

%(default_variables_publish)s

   273

   274

	// Extra variables publish

   275

%(extra_variables_publish)s

   276

}