//the motor has 512*200*100 usteps per revolution (512 uystep/step, 200 steps/rev 1:100 gear)
//the ssi-encoder has 2**18=262144 steps per revolution
//calibration values such that 0 mdeg is the highest point (+5mm) and 180000mdeg = lowest point (-5mm)
//#1: homeoffset: -20907.5928142
//#2: homeoffset: 102320.329068
//#3: homeoffset: 168946.772022
//#4: homeoffset: -87830.7554146
//#5: homeoffset: -47956.2759182

!encoder_ssi(enc=1,numBits=18,posSf=-5625./4096)
//!motor(mot=1,dirCur=1000,JogSpeed=40,servoSf=256./9.,Pos2Sf=0,InPosBand=0,FatalFeLimit=1000,HomeOffset=0.)
!motor(mot=1,dirCur=1000,JogSpeed=40,servoSf=256./9.,InPosBand=0,FatalFeLimit=1000,HomeOffset=65477.)

!encoder_ssi(enc=2,numBits=18,posSf=-5625./4096)
//!motor(mot=2,dirCur=1000,JogSpeed=40,servoSf=256./9.,Pos2Sf=0,InPosBand=0,FatalFeLimit=1000,HomeOffset=0.)
!motor(mot=2,dirCur=1000,JogSpeed=40,servoSf=256./9.,InPosBand=0,FatalFeLimit=1000,HomeOffset=-154370.)

!encoder_ssi(enc=3,numBits=18,posSf=-5625./4096)
//!motor(mot=3,dirCur=1000,JogSpeed=40,servoSf=256./9.,Pos2Sf=0,InPosBand=0,FatalFeLimit=1000,HomeOffset=0.)
!motor(mot=3,dirCur=1000,JogSpeed=40,servoSf=256./9.,InPosBand=0,FatalFeLimit=1000,HomeOffset=-5248.)

!encoder_ssi(enc=4,numBits=18,posSf=-5625./4096)
//!motor(mot=4,dirCur=1000,JogSpeed=40,servoSf=256./9.,Pos2Sf=0,InPosBand=0,FatalFeLimit=1000,HomeOffset=0.)
!motor(mot=4,dirCur=1000,JogSpeed=40,servoSf=256./9.,InPosBand=0,FatalFeLimit=1000,HomeOffset=-46620.)

!encoder_ssi(enc=5,numBits=18,posSf=-5625./4096)
//!motor(mot=5,dirCur=1000,JogSpeed=40,servoSf=256./9.,Pos2Sf=0,InPosBand=0,FatalFeLimit=1000,HomeOffset=0.)
!motor(mot=5,dirCur=1000,JogSpeed=40,servoSf=256./9.,InPosBand=0,FatalFeLimit=1000,HomeOffset=93168.)

// PKP266D14BA2 1.4A 1.8deb/step
//!encoder_sim(enc=6)
//!motor(mot=6,dirCur=1400,JogSpeed=40,InPosBand=0,FatalFeLimit=1000,HomeOffset=0.)
// #6j=102400 -> 1 rev
//#6j=102400 == 1 rev == 102400 enc_cnt == 2000*1um
//servoSf  : motorusteps/user_units    = 102400./2000
//posSf    : user_units/encoder_steps  = 2000./102400
//!encoder_sim(enc=6,posSf=2000./102400,pos2Sf=0)
!encoder_sim(enc=6,posSf=2000./102400)
!motor(mot=6,dirCur=1400,servoSf=102400./2000,invDir=False,JogSpeed=2,InPosBand=1,FatalFeLimit=1000,HomeOffset=0.)

define(SP_RelBrk='15',SP_LockBrk='16')

// ---------- Custom Motion Programs ----------

!cm_prem_post(ax=1,prem=15,post=16) //generates subprog 10,11,12,13
!cm_prem_post(ax=2,prem=15,post=16) //generates subprog 20,21,22,23
!cm_prem_post(ax=3,prem=15,post=16) //generates subprog 30,31,32,33
!cm_prem_post(ax=4,prem=15,post=16) //generates subprog 40,41,42,43
!cm_prem_post(ax=5,prem=15,post=16) //generates subprog 40,41,42,43

!cm_prem_post(ax='X',axid=9,prem=15,post=16) //generates subprog 90,91,92,93
!cm_prem_post(ax='Y',axid=10,prem=15,post=16) //generates subprog 100,101,102,103
!cm_prem_post(ax='U',axid=11,prem=15,post=16) //generates subprog 110,111,112,113
!cm_prem_post(ax='V',axid=12,prem=15,post=16) //generates subprog 120,121,122,123
!cm_prem_post(ax='W',axid=13,prem=15,post=16) //generates subprog 120,121,122,123

// ---------- PREM POST Programs ----------

//prem
open subprog SP_RelBrk
  define(EndTime='L1'); // Local variable
  Motor[1].IdCmd=1000;Motor[1].InPosBand=2
  Motor[2].IdCmd=1000;Motor[2].InPosBand=2
  Motor[3].IdCmd=1000;Motor[3].InPosBand=2
  Motor[4].IdCmd=1000;Motor[4].InPosBand=2
  Motor[5].IdCmd=1000;Motor[5].InPosBand=2
  PowerBrick[0].GpioData[0].16.5=$1f
  //send 1"wait\n"
  //EndTime = Sys.Time + .1; // time + 10 sec.
  //while (EndTime > Sys.Time){}
  //send 1"wait done\n"
close

//post
open subprog SP_LockBrk
  define(EndTime='L1'); // Local variable
  //wait until all motors are in pos
  EndTime = Sys.Time + 10; // time + 10 sec.
  //send 1"wait."
  while (EndTime > Sys.Time)
  {
    if(Coord[1].InPos)
    {
      //send 1"inPos\n"
      break
    }
    //if(Motor[1].InPos && Motor[2].InPos&& Motor[3].InPos&& Motor[4].InPos&& Motor[5].InPos)
    //  break
    //send 1"."
  }
  //send 1"done\n"

  Motor[1].IdCmd=0;Motor[1].InPosBand=50
  Motor[2].IdCmd=0;Motor[2].InPosBand=50
  Motor[3].IdCmd=0;Motor[3].InPosBand=50
  Motor[4].IdCmd=0;Motor[4].InPosBand=50
  Motor[5].IdCmd=0;Motor[5].InPosBand=50
  PowerBrick[0].GpioData[0].16.5=$0
close