//****************** Profile Position test ********************
//21APR10

//reset network
Send 000h  2  81h 00h
Wait 1000

//**** setup RX PDO1 (disabled)
Send 601h  8  23h 00h 14h 01h 01h 02h 00h 80h   //PDOs disabled
WaitID 500 581h

//**** setup RX PDO2 (disabled)
Send 601h  8  23h 01h 14h 01h 01h 03h 00h 80h   //PDOs disabled
WaitID 500 581h

//**** setup RX PDO3 (control word & Interpolated target position)
Send 601h  8  23h 02h 14h 01h 01h 04h 00h 80h
WaitID 50 581h
Send 601h  8  2Fh 02h 16h 00h 00h 00h 00h 00h   //clear mapping
WaitID 50 581h
Send 601h  8  23h 02h 16h 01h 10h 00h 40h 60h	//control word
WaitID 50 581h
Send 601h  8  23h 02h 16h 02h 20h 01h C1h 60h	//Target Position
WaitID 50 581h
Send 601h  8  2Fh 02h 16h 00h 02h 00h 00h 00h   //two item mapped
WaitID 50 581h
Send 601h  8  2Fh 02h 14h 02h 01h 00h 00h 00h   //Implement on SYNC telegram from master
WaitID 50 581h 
Send 601h  8  23h 02h 14h 01h 01h 04h 00h 00h   //PDOs enabled
WaitID 50 581h 

//**** setup RX PDO4 (disabled)
Send 601h  8  23h 03h 14h 01h 01h 05h 00h 80h   //PDOs disabled
WaitID 500 581h

//**** setup TX PDO1 (disabled)
Send 601h  8  23h 00h 18h 01h 81h 01h 00h 80h	//PDOs disabled
WaitID 500 581h

//**** setup TX PDO2 (disabled)
Send 601h  8  23h 01h 18h 01h 81h 02h 00h 80h	//PDOs disabled
WaitID 500 581h

//**** setup TX PDO3 (status and position)
Send 601h  8  23h 02h 18h 01h 81h 03h 00h 80h	//PDOs disabled
WaitID 50 581h
Send 601h  8  2Fh 02h 1Ah 00h 00h 00h 00h 00h   //clear mapping
WaitID 50 581h
Send 601h  8  23h 02h 1Ah 01h 10h 00h 41h 60h	//STATUS
WaitID 50 581h
Send 601h  8  23h 02h 1Ah 02h 20h 00h 63h 60h	//actual position
WaitID 50 581h
Send 601h  8  2Fh 02h 1Ah 00h 02h 00h 00h 00h	//two items mapped
WaitID 50 581h
Send 601h  8  2Fh 02h 18h 02h 01h 00h 00h 00h	//Send on SYNC telegram from master
WaitID 50 581h
Send 601h  8  23h 02h 18h 01h 81h 03h 00h 00h	//enable PDO
WaitID 50 581h

//**** setup TX PDO4 (PL.ERR)
Send 601h  8  23h 03h 18h 01h 81h 04h 00h 80h	//PDOs disabled
WaitID 500 581h
Send 601h  8  2Fh 03h 1Ah 00h 00h 00h 00h 00h   //clear mapping
WaitID 50 581h
Send 601h  8  23h 03h 1Ah 01h 10h 00h F4h 60h	//PL.ERR
WaitID 50 581h
Send 601h  8  2Fh 03h 1Ah 00h 01h 00h 00h 00h	//One item mapped
WaitID 50 581h
Send 601h  8  2Fh 03h 18h 02h 01h 00h 00h 00h	//Send on SYNC telegram from master
WaitID 50 581h
Send 601h  8  23h 03h 18h 01h 81h 04h 00h 00h	//enable PDO
WaitID 50 581h

Send 601h  8  23h 8Fh 60h 01h 00h 00h 01h 00h	//Set FB1.PSCALE to 16 bit
WaitID 500 581h

Send 601h  8  23h 92h 60h 01h 00h 00h 01h 00h	//Set DS402.POSFCFEED to 16 bit
WaitID 500 581h

Send 601h  8  23h 92h 60h 02h 01h 00h 00h 00h	//Set DS402.POSFCSHAFTREV to 1
WaitID 500 581h

Send 601h  8  23h 91h 60h 01h 01h 00h 00h 00h	//Set DS402.POSGEARMOTORREV to 1
WaitID 500 581h

Send 601h  8  23h 91h 60h 02h 01h 00h 00h 00h	//Set DS402.POSGEARSHAFTREV to 1
WaitID 500 581h

Send 601h  8  2Fh C2h 60h 01h 08h 00h 00h 00h	//Set update time to 8mS
WaitID 500 581h

Send 601h  8  2Fh C2h 60h 02h FDh 00h 00h 00h	//Set update time to 10mS
WaitID 500 581h

Send 601h  8  23h E6h 36h 00h 01h 00h 00h 00h	//Turn on the PLL
WaitID 500 581h

Send 601h  8  2Fh 60h 60h 00h 07h 00h 00h 00h	//opmode to Interpolated position
WaitID 500 581h

Send 601h  8  23h F0h 35h 00h 00h 00h 00h 00h	//HOME.SET
WaitID 500 581h

wait 500

//**** Start send n receive ****
Send 000h 2 01h 00h	//enable NMT

//Ready to switch oFF
Send 401h  6  00h 00h 00h 00h 00h 00h	//send control word and target pos
Send 80h  0								//Sync message
WAIT 500

//Reset fault
Send 401h  6  80h 00h 00h 00h 00h 00h	//send control word and target pos
Send 80h  0								//Sync message
WAIT 2000

//Ready to switch oFF
Send 401h  6  00h 00h 00h 00h 00h 00h	//send control word and target pos
Send 80h  0								//Sync message
WAIT 500

//Ready to switch on
Send 401h  6  06h 00h 00h 00h 00h 00h	//send control word and target pos
Send 80h  0	
Wait 500

//Switch on
Send 401h  6  07h 00h 00h 00h 00h 00h	//send control word and target pos
Send 80h  0	
Wait 500

//op enable
Send 401h  6  0Fh 00h 00h 00h 00h 00h	//send control word and target pos
Send 80h  0	
wait 500

//enable interpolation
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
wait 100

//pos 0
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 0 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 1
Send 401h  6  1Fh 00h 33h 03h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 2
Send 401h  6  1Fh 00h 66h 06h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h

//pos 3
Send 401h  6  1Fh 00h 99h 09h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 4
Send 401h  6  1Fh 00h CCh 0Ch 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 5
Send 401h  6  1Fh 00h 00h 10h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 381h 

//pos 6
Send 401h  6  1Fh 00h 33h 13h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 7
Send 401h  6  1Fh 00h 66h 16h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 8
Send 401h  6  1Fh 00h 99h 19h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 9
Send 401h  6  1Fh 00h CCh 1Ch 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 10 (repeat)
Send 401h  6  1Fh 00h 00h 20h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 11
Send 401h  6  1Fh 00h CCh 1Ch 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 12
Send 401h  6  1Fh 00h 99h 19h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 13
Send 401h  6  1Fh 00h 66h 16h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 14
Send 401h  6  1Fh 00h 33h 13h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 15
Send 401h  6  1Fh 00h 00h 10h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 16
Send 401h  6  1Fh 00h CCh 0Ch 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 17
Send 401h  6  1Fh 00h 99h 09h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 18
Send 401h  6  1Fh 00h 66h 06h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 19
Send 401h  6  1Fh 00h 33h 03h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 20
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 20 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 20 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 20 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 20 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 20 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h
 
//pos 20 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//pos 20 (repeat)
Send 401h  6  1Fh 00h 10h 00h 00h 00h	//send control word and target pos
Send 80h  0	
WaitId 5 1 481h 

//DISABLE
Send 401h  6  00h 00h 00h 00h 14h 00h	//send control word and target pos
Send 80h  0	
wait 500

Send 000h 2 02h 00h	//disable NMT
Send 000h 2 81h 00h	//Reset network