-
Notifications
You must be signed in to change notification settings - Fork 0
/
Slave.c
452 lines (360 loc) · 10.4 KB
/
Slave.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
#include "CO_MAIN.H"
#include "Slave.h"
#include "IOutils.h"
#include "Ultraljud.h"
#define RTR_DIS bytes.B1.bits.b2
#define STD_DIS bytes.B1.bits.b3
#define PDO_DIS bytes.B1.bits.b4
// These are mapping constants for TPDO1
// starting at 0x1A00 in the dictionary
rom unsigned long uTPDO1Map = 0x60000108;
rom unsigned long uRPDO1Map = 0x62000108;
rom unsigned long uPDO1Dummy = 0x00000008;
unsigned char uIOinFilter; // 0x6003 filter
unsigned char uIOinPolarity; // 0x6002 polarity
unsigned char uIOinIntChange; // 0x6006 interrupt on change
unsigned char uIOinIntRise; // 0x6007 interrupt on positive edge
unsigned char uIOinIntFall; // 0x6008 interrupt on negative edge
unsigned char uIOinIntEnable; // 0x6005 enable interrupts
unsigned char uIOinDigiInOld; //
// Static data refered to by the dictionary
rom unsigned char rMaxIndex1 = 1;
rom unsigned char rMaxIndex2 = 8;
rom unsigned char uDemoTPDO1Len = 2;
unsigned char uLocalXmtBuffer[8]; // Local buffer for TPDO1
unsigned char uLocalRcvBuffer[8]; // local buffer fot RPDO1
UNSIGNED8 uDemoState; // Bits used to control various states
unsigned char uDemoSyncCount; // Counter for synchronous types
unsigned char uDemoSyncSet; // Internal TPDO type control
unsigned short distance = 0;
unsigned char warning = 0x01;
unsigned short limit = 0;
unsigned char limitMode = 0x01;
unsigned char distanceMode = 0x00;
unsigned char Hold = 0;
unsigned char TriggeredBySwitch = 0x00;
unsigned char TriggeredByDistance = 0x00;
unsigned char State = 0x00;
enum ApplicationState APPLICATION_STATE = INITIALIZATION;
void Slave_Init(void)
{
// Convert to MCHP
mTOOLS_CO2MCHP(0xC0000182L);
// Store the COB
mTPDOSetCOB(1, mTOOLS_GetCOBID());
// Convert to MCHP
mTOOLS_CO2MCHP(mCOMM_GetNodeID().byte + 0xC0000300L);
// Store the COB
mTPDOSetCOB(2, mTOOLS_GetCOBID());
// Set the pointer to the buffers
mTPDOSetTxPtr(1, (unsigned char *)(&warning));
mTPDOSetTxPtr(2, (unsigned char *)(&distance));
// RPDO1
mTOOLS_CO2MCHP(0xC0000181L);
mRPDOSetCOB(1, mTOOLS_GetCOBID());
mRPDOSetRxPtr(1, (unsigned char *)(&State));
// Set the length
mTPDOSetLen(1, 1);
mTPDOSetLen(2, 2);
}
void Slave_ProcessEvents(void)
{
ReadDistance(&distance);
LED = !SWITCH;
if(mRPDOIsGetRdy(1))
{
mAppGoToPOSTMANEUVERstate();
mRPDORead(1);
}
if(limitMode && (APPLICATION_STATE == PREMANEUVER) )
{
if((distance < limit) && !Hold && !TriggeredBySwitch)
{
TriggeredByDistance = 1;
Hold = 1;
mAppGoToMANEUVERstate();
//SEND PDO!!!
if(mTPDOIsPutRdy(1))
mTPDOWritten(1);
}
else if(!SWITCH && !Hold && !TriggeredByDistance)
{
TriggeredBySwitch = 1;
Hold = 1;
mAppGoToMANEUVERstate();
//SEND PDO!!!
if(mTPDOIsPutRdy(1))
mTPDOWritten(1);
}
if( (distance > limit) && TriggeredByDistance )
{
Hold = 0;
TriggeredByDistance = 0;
}
else if(SWITCH && TriggeredBySwitch)
{
Hold = 0;
TriggeredBySwitch = 0;
}
}
if(distanceMode)
{
if(mTPDOIsPutRdy(2))
mTPDOWritten(2);
}
}
/*********************************************************************
* Function: void CO_COMMSyncEvent(void)
*
* PreCondition: none
*
* Input: none
*
* Output: none
*
* Side Effects: none
*
* Overview: This is a simple demonstration of a SYNC event
* handling function.
*
* Note:
********************************************************************/
void CO_COMMSyncEvent(void)
{
// Process only if in a synchronous mode
if ((uDemoSyncSet == 0) && (uDemoState.bits.b2))
{
// Reset the synchronous transmit and transfer to async
uDemoState.bits.b2 = 0;
uDemoState.bits.b0 = 1;
}
else
if ((uDemoSyncSet >= 1) && (uDemoSyncSet <= 240))
{
// Adjust the sync counter
uDemoSyncCount--;
// If time to generate sync
if (uDemoSyncCount == 0)
{
// Reset the sync counter
uDemoSyncCount = uDemoSyncSet;
// Start the PDO transmission
uDemoState.bits.b0 = 1;
}
}
}
/*********************************************************************
* Function: void CO_COMM_RPDO1_COBIDAccessEvent(void)
*
* PreCondition: none
*
* Input: none
*
* Output: none
*
* Side Effects: none
*
* Overview: This is a simple demonstration of a RPDO COB access
* handling function.
*
* Note: This function is called from the dictionary.
********************************************************************/
void CO_COMM_RPDO1_COBIDAccessEvent(void)
{
switch (mCO_DictGetCmd())
{
case DICT_OBJ_READ: // Read the object
// Translate MCHP COB to CANopen COB
mTOOLS_MCHP2CO(mRPDOGetCOB(1));
// Return the COBID
*(unsigned long *)(uDict.obj->pReqBuf) = mTOOLS_GetCOBID();
break;
case DICT_OBJ_WRITE: // Write the object
// Translate the COB to MCHP format
mTOOLS_CO2MCHP(*(unsigned long *)(uDict.obj->pReqBuf));
// If the request is to stop the PDO
if ((*(UNSIGNED32 *)(&mTOOLS_GetCOBID())).PDO_DIS)
{
// And if the COB received matches the stored COB and type then close
if (!((mTOOLS_GetCOBID() ^ mRPDOGetCOB(1)) & 0xFFFFEFFFL))
{
// but only close if the PDO endpoint was open
if (mRPDOIsOpen(1)) {mRPDOClose(1);}
// Indicate to the local object that this PDO is disabled
(*(UNSIGNED32 *)(&mRPDOGetCOB(1))).PDO_DIS = 1;
}
else {mCO_DictSetRet(E_PARAM_RANGE);} //error
}
// Else if the TPDO is not open then start the TPDO
else
{
// And if the COB received matches the stored COB and type then open
if (!((mTOOLS_GetCOBID() ^ mRPDOGetCOB(1)) & 0xFFFFEFFFL))
{
// but only open if the PDO endpoint was closed
if (!mRPDOIsOpen(1)) {mRPDOOpen(1);}
// Indicate to the local object that this PDO is enabled
(*(UNSIGNED32 *)(&mRPDOGetCOB(1))).PDO_DIS = 0;
}
else {mCO_DictSetRet(E_PARAM_RANGE);} //error
}
break;
}
}
/*********************************************************************
* Function: void CO_COMM_TPDO1_COBIDAccessEvent(void)
*
* PreCondition: none
*
* Input: none
*
* Output: none
*
* Side Effects: none
*
* Overview: This is a simple demonstration of a TPDO COB access
* handling function.
*
* Note: This function is called from the dictionary.
********************************************************************/
void CO_COMM_TPDO1_COBIDAccessEvent(void)
{
switch (mCO_DictGetCmd())
{
case DICT_OBJ_READ: // Read the object
// Translate MCHP COB to CANopen COB
mTOOLS_MCHP2CO(mTPDOGetCOB(1));
// Return the COBID
*(unsigned long *)(uDict.obj->pReqBuf) = mTOOLS_GetCOBID();
break;
case DICT_OBJ_WRITE: // Write the object
// Translate the COB to MCHP format
mTOOLS_CO2MCHP(*(unsigned long *)(uDict.obj->pReqBuf));
// If the request is to stop the PDO
if ((*(UNSIGNED32 *)(&mTOOLS_GetCOBID())).PDO_DIS)
{
// And if the COB received matches the stored COB and type then close
if (!((mTOOLS_GetCOBID() ^ mTPDOGetCOB(1)) & 0xFFFFEFFFL))
{
// but only close if the PDO endpoint was open
if (mTPDOIsOpen(1)) {mTPDOClose(1);}
// Indicate to the local object that this PDO is disabled
(*(UNSIGNED32 *)(&mTPDOGetCOB(1))).PDO_DIS = 1;
}
else {mCO_DictSetRet(E_PARAM_RANGE);} //error
}
// Else if the TPDO is not open then start the TPDO
else
{
// And if the COB received matches the stored COB and type then open
if (!((mTOOLS_GetCOBID() ^ mTPDOGetCOB(1)) & 0xFFFFEFFFL))
{
// but only open if the PDO endpoint was closed
if (!mTPDOIsOpen(1)) {mTPDOOpen(1);}
// Indicate to the local object that this PDO is enabled
(*(UNSIGNED32 *)(&mTPDOGetCOB(1))).PDO_DIS = 0;
}
else {mCO_DictSetRet(E_PARAM_RANGE);} //error
}
break;
}
}
/*********************************************************************
* Function: void CO_COMM_TPDO1_TypeAccessEvent(void)
*
* PreCondition: none
*
* Input: none
*
* Output: none
*
* Side Effects: none
*
* Overview: This is a simple demonstration of a TPDO type access
* handling function.
*
* Note: This function is called from the dictionary.
********************************************************************/
void CO_COMM_TPDO1_TypeAccessEvent(void)
{
unsigned char tempType;
switch (mCO_DictGetCmd())
{
//case DICT_OBJ_INFO: // Get information about the object
// The application should use this to load the
// structure with legth, access, and mapping.
// break;
case DICT_OBJ_READ: // Read the object
// Write the Type to the buffer
*(uDict.obj->pReqBuf) = uDemoSyncSet;
break;
case DICT_OBJ_WRITE: // Write the object
tempType = *(uDict.obj->pReqBuf);
if ((tempType >= 0) && (tempType <= 240))
{
// Set the new type and resync
uDemoSyncCount = uDemoSyncSet = tempType;
}
else
if ((tempType == 254) || (tempType == 255))
{
uDemoSyncSet = tempType;
}
else {mCO_DictSetRet(E_PARAM_RANGE);} //error
break;
}
}
/*********************************************************************
* Function: void CO_PDO1LSTimerEvent(void)
*
* PreCondition: none
*
* Input: none
*
* Output: none
*
* Side Effects: none
*
* Overview: none
*
* Note: none
********************************************************************/
void CO_PDO1LSTimerEvent(void)
{
}
void CO_PDO2LSTimerEvent(void)
{
}
void CO_PDO3LSTimerEvent(void)
{
}
void CO_PDO4LSTimerEvent(void)
{
}
/*********************************************************************
* Function: void CO_PDO1TXFinEvent(void)
*
* PreCondition: none
*
* Input: none
*
* Output: none
*
* Side Effects: none
*
* Overview: none
*
* Note: none
********************************************************************/
void CO_PDO1TXFinEvent(void)
{
}
void CO_PDO2TXFinEvent(void)
{
}
void CO_PDO3TXFinEvent(void)
{
}
void CO_PDO4TXFinEvent(void)
{
}