// Here we use 'real encoder with direct PWM'. Further the axis are scaled
// in this configuration, the PID gives 'torque' to iqCmd.
// the phasePos is received from tne encoder on the motor shaft.
// The idCmd is set to 0
// the PID regulates the position by setting torque, if the motor is not at the desired position
// Compared to 'real encoder with direct microstepping', following main elements have to be reconfigured:
// SlipGain=0 (instead 0.25) ,PhasePosSf= calculated value (instead of 0)
// look also at PwmSf,PhaseMode,PhaseCtrl,

// -> PhasePosSf is calculated as follows:  (2048*pole_cycle)/(256*enc_step) = 8*pole_cycle/enc_step

// e.g.   Motor[x].pPhaseEnc -> PowerBrick[.].Chan[.].PhaseCapt.a
//        1 rev = 8192 phase_step = 4 pole_cycle = 512000 PhaseCapt =256*2000 (256=scaling of encTable, 2000=enc_step/rev)
//        PhasePosSf 8*4/2000=0.016


// x einraster == -> x-N and x-S poles =2*x poles -> 1 rev = x*2048 ustep=phase_step
// changing the polarity from S-N-S (one pole cycle) are 2048 phase_step. phase_step is also called ustep

//Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H    32 poles = 1 rev = 16*2048=32768 phase_step
//Enc 1: Rotation stage LS Mecapion                  1 rev = 1048576 enc_steps

//Mot 2: Stage X Parker MX80L D11 25mm               one pole cycle = 13mm = 2048 phase_step
//Enc 2: Stage X Parker MX80L D11 inc_enc 20nm       one pole cycle = 13mm = 650000 enc_step (20nm/enc_step)

//Mot 3: Stage Y Parker MX80L D11 25mm               one pole cycle = 13mm = 2048 phase_step
//Enc 3: Stage Y Parker MX80L D11 inc_enc 20nm       one pole cycle = 13mm = 650000 enc_step (20nm/enc_step)

//Mot 4: Test Servo: Trinamic QBL 4208 motor                   8 poles 1 rev = 4*2048=8192 phase_step
//Enc 4: Test Servo: Incremental encoder mounted with motor 1  1 rev = 2000 enc count (500 inc_ quadrature encoder)
//Enc 5: Test Servo: Trinamic QBL 4208 hall sensor             1 rev = 24 enc count (hall sensor encoder)

//Mot 7: Stada Stepper:                           670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
//Enc 7: Renishaw absolute BiSS

//Mot 8: Stada Stepper:                           670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
//Enc 8: Renishaw absolute BiSS

//$$$***
//!common()
//!common(PhaseFreq=20000,PhasePerServo=4)
//!common(PhaseFreq=20000,PhasePerServo=1)
//!common(PhaseFreq=40000)

!torqueCtrl()
!init()


&1
//#1-> 0.00001X+ 0.00001Y +   A
//#2->   +1. X    + .5Y + 0.01A
//#3->   + .5X    +1. Y + 0.01A

#1->   A
#2->   X
#3->   Y

Coord[1].AltFeedRate=0
Coord[1].Tm=1  //1ms time
Coord[1].Ta=1
Coord[1].Td=1
Coord[1].Ts=0

//Parker: Continous Force 4N -> assume 1kg load -> acceleration=a=F/m=4m/s^2
//do not use time to accelerate but acceleration
//4m/s^2=4um/(ms)^2 Motor units=um -> JogTa= -1ms^2/4mu -2.5-> 4m/s^2 -25->0.4m/s^2
Motor[2].JogTs=0
Motor[2].JogTa=-2.5
Motor[3].JogTs=0
Motor[3].JogTa=-2.5


Motor[1].MaxSpeed=360
Motor[2].MaxSpeed=50
Motor[3].MaxSpeed=50

open prog 1
  //this uses jogspeed
  rapid abs
  X(10000)  Y(0)  A(0)
  X(0)  Y(10000)  A(0)
  X(0)  Y(0)  A(36000)
  X(0)  Y(0)  A(0)
close

open prog 2
  //this uses Coord[1].Tm and limits with MaxSpeed
  linear abs
  X(10000)  Y(0)  A(0)
  X(0)  Y(10000)  A(0)
  X(0)  Y(0)  A(0)
  X(0)  Y(0)  A(36000)
  X(0)  Y(0)  A(0)
close