//---------------------------------------------------------------------------
//  Project PowerPMAC Firmware
//  Delta Tau Data Systems, Inc.
//  Copyright  2007. All Rights Reserved.
//
//  SUBSYSTEM:			User Servo Driver
//  FILE:         		usrcode.c
//  TEMPLATE AUTHOR:		Henry Bausley
//
//  OVERVIEW
//  ~~~~~~~~
//  This file is where exportable user code can be placed.
//  To make a function callable as a user servo do three steps
//
//  1.)  Prototye the function user_func(void ,void );
//  2.)  Export the function  EXPORT_SYMBOL(user_func);
//  3.)  Make sure useralgo.ko has been loaded with projpp.ini
//
//--------------------------------------------------------------------------------
#include "usrcode.h"
//----------------------------------------------------------------------------------
// pp_proj.h is the C header for accessing PMAC Global, CSGlobal, Ptr vars
// _PPScriptMode_ for Pmac Script like access global & csglobal
// global Mypvar - access with "Mypvar"
// global Myparray(32) - access with "Myparray(i)"
// csglobal Myqvar - access with "Myqvar(i)" where "i" is Coord #
// csglobal Myqarray(16) - access with "Myqvar(i,j)" where "j" is index
// _EnumMode_ for Pmac enum data type checking on Set & Get global functions
// Example
// global Mypvar
// csglobal Myqvar
// "SetGlobalVar(Myqvar, data)" will give a compile error because its a csglobal var.
// "SetCSGlobalVar(Mypvar, data)" will give a compile error because its a global var.
//------------------------------------------------------------------------------------
#define _PPScriptMode_	// uncomment for Pmac Script type access
// #define _EnumMode_			// uncomment for Pmac enum data type checking on Set & Get global functions		

#include "pp_proj.h"
#ifdef __KERNEL__
// Kernal mode can't have paths with spaces and long names
//#include "../../PMACSC~1/GLOBAL~1/asharedwithcapp.pmh"
#else
//#include "../../PMAC Script Language/Global Includes/asharedwithcapp.pmh"
#endif

extern struct SHM        *pshm;  // Pointer to shared memory
extern volatile unsigned *piom;  // Pointer to I/O memory
extern void              *pushm; // Pointer to user memory

double compensate_friction_servo_ctrl(MotorData *Mptr, int motID);

void user_phase( struct MotorData *Mptr)
{
}

double usr_servo_ctrl_2(MotorData *Mptr)
{
  pshm->P[2000]=pshm->P[2000]*.9999+abs(Mptr->PosError)*0.0001;  //lowpass of Position error
  return pshm->ServoCtrl(Mptr);
}

//friction MX-stage motor 3
//Avg current forward: 28.2405057332 Avg current backward: -61.2462872333
//positions 500,600,700 ...27700,27800,27900
float lutFric[275]={
-7.1551, -12.6559, -21.4081, -30.2418, -35.1982, -36.4386, -37.2831, -38.4421, -39.7832, -39.7141, -37.7057, -33.7864, -32.3466, -34.7608, -37.4658, -36.992, -35.7382, -37.1257, -39.5308, -41.5, -43.0684, -44.4203, -45.678, -47.696, -49.1327, -48.3342, -47.32, -47.6388, -47.6858, -43.7855, -37.843, -32.5668, -28.0956, -24.7008, -21.593, -17.8392, -13.6763, -9.10309, -5.04866, -1.3417, 1.9566, 4.49383, 6.35785, 9.39698, 15.521, 23.7353, 29.7179, 32.9107, 37.6373, 43.2183, 46.9492, 48.7774, 52.098, 57.1212, 60.8518, 61.575, 60.2766, 56.885, 51.2957, 43.5279, 33.8489, 22.2905, 7.2842, -9.01961, -23.3673, -33.5445, -40.3342, -46.3352, -51.7987, -55.6054, -58.5937, -62.3993, -66.5449, -67.4299, -65.8841, -64.9919, -66.0021, -65.5962, -62.5602, -58.2791, -54.7651, -53.3157, -52.0205, -48.1133, -42.9703, -37.9826, -33.8855, -29.9032, -24.1226, -16.7909, -9.89502, -2.63174, 5.23116, 11.8609, 15.8984, 20.6177, 27.263, 32.7512, 36.4521, 40.1571, 44.8302, 48.2186, 49.4304, 47.9396, 45.5347, 43.824, 43.1741, 41.7656, 39.9741, 38.9437, 38.9356, 39.0903, 39.6, 41.1204, 40.9819, 39.7462, 39.0561, 39.0774, 37.3204, 33.627, 31.4442, 30.3062, 26.8336, 19.1632, 9.99558, 2.36922, -4.35166, -12.2266, -21.8815, -30.2799, -35.5315, -38.337, -41.0337, -42.5536, -41.8356, -38.9385, -36.9616, -36.4229, -35.8889, -34.1475, -31.3523, -28.1945, -23.8853, -20.2734, -20.0726, -22.4272, -22.0745, -18.0298, -14.7222, -15.2457, -17.8645, -19.2889, -21.4733, -23.6443, -24.7379, -23.4422, -21.5436, -20.6753, -19.3729, -16.2246, -11.419, -7.87291, -5.41278, -1.40435, 3.3671, 4.66581, 2.70959, 2.15455, 3.87838, 4.97318, 4.11034, 4.4132, 6.59851, 10.736, 15.2254, 18.5329, 19.8407, 20.1992, 21.777, 24.5145, 26.9981, 28.7539, 30.8099, 32.6786, 34.7331, 36.3433, 35.6326, 31.7121, 26.2285, 21.5035, 17.4354, 9.14015, -2.01504, -12.9646, -21.3099, -28.7052, -35.4772, -41.5508, -46.6105, -48.9102, -50.1711, -51.4111, -52.8876, -51.1254, -47.2276, -42.8323, -37.7037, -32.1169, -27.9526, -24.7178, -20.2562, -15.264, -11.7584, -9.64182, -6.72713, -2.17278, 1.20068, 4.02159, 8.10266, 14.9257, 21.8962, 27.2483, 30.9286, 37.2023, 46.0579, 55.1473, 61.4506, 64.6756, 67.6285, 71.5098, 75.4504, 77.3359, 76.9234, 75.733, 73.8723, 71.3198, 70.6438, 71.6276, 71.2124, 68.2844, 65.7515, 65.9122, 65.9395, 66.131, 67.2553, 69.1219, 69.3801, 66.8238, 63.5434, 60.4531, 58.2197, 55.5132, 51.7531, 47.1773, 43.2781, 37.2329, 29.4104, 22.4017, 18.0676, 13.1263, 7.18284, 2.79397, 0.246639, -3.82759, -9.64515, -13.2226, -13.5821, -11.6253, -9.90048, -9.94788, -12.5784, -14.697, -13.6629, -10.2785, -8.82145, };

double usr_servo_ctrl_3(MotorData *Mptr)
{
  //Structure of shared memory file is at: /opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac/RtGpShm.h

  //compensate_friction_servo_ctrl for motor 3
  static int cnt=0;
  float p,c;
  int idx;
  double ctrl_out;

  p=(Mptr->ActPos-Mptr->HomePos-500.f)/100.f;
  //idx=(int)floor(p);
  idx=(int)p;
  if (idx<0)
    c=lutFric[0];
  else if (idx>273)
    c=lutFric[274];
  else
    c=lutFric[idx]+(lutFric[idx+1]-lutFric[idx])*(p-idx);

  c=0.f; //do not use friction lut
  if(Mptr->DesVel>0)
  {
    c+=28.2405057332f;
    //c+=100.f;
  }
  else if (Mptr->DesVel<0)
  {
    c-=61.2462872333f;
    //c-=100.f;
  }
  else
  {
    c+=(28.2f-61.2f)/2.f;
  }
  //c-=550.f;//compensate if stage is vertically
  c/=11.f; //no friction compensation at all
  c=0.f; //no friction compensation at all
  pshm->P[3000]=pshm->P[3000]*.9999+abs(Mptr->PosError)*0.0001;  //lowpass of Position error

  // The default servo algo.
  ctrl_out=pshm->ServoCtrl(Mptr);

  //  Mptr->IdCmd = 34;
  //compensate friction
  ctrl_out+=c;

  //if(ctrl_out>200)ctrl_out=200;
  //if(ctrl_out<-200)ctrl_out=-200;
  //if (cnt%10000 == 0 )
  //{
  //  printk("usr_servo_ctrl_3 (%d) (%d)\n",cnt,idx);
  //}
  //cnt++;
  return ctrl_out;

}

  // Your custom super duper filter probable more than incrementing a P variable
  //if(Mptr->DesVelZero == 1 && Mptr->InPos == 1)  // not cmd to move and no FE when using external position encoder
  //{
  //  pshm->P[1] = 1.0;
  //  Mptr->IdCmd = 34;
  //}
  //else
  //{
  //  pshm->P[1] = 0.0;
  //  Mptr->IdCmd = 3400;
  //}

double user_pid_ctrl( struct MotorData *Mptr)
{
  //double *p;
  //p = pushm;
  //return 0;
  double ctrl_out;
  if (Mptr->ClosedLoop) {
    // Compute PD terms
    ctrl_out=Mptr->Servo.Kp*Mptr->PosError-Mptr->Servo.Kvfb*Mptr->ActVel;
    Mptr->Servo.Integrator+=Mptr->PosError*Mptr->Servo.Ki; // I term
    ctrl_out+=Mptr->Servo.Integrator;
    // Combine
    if (ctrl_out>2000)ctrl_out=2000;
    if (ctrl_out<-2000)ctrl_out=-2000;
    return ctrl_out;
  }
  else {
    Mptr->Servo.Integrator=0.0;
    return 0.0;
  }
}

void CaptCompISR(void)
{
  unsigned *pUnsigned = pushm;
  *pUnsigned = *pUnsigned + 1;
}

double GetLocal(struct LocalData *Ldata,int m)
{
  return *(Ldata->L + Ldata->Lindex + m);
}

void SetLocal(struct LocalData *Ldata,int m,double value)
{
  *(Ldata->L + Ldata->Lindex + m) = value;
}

double *GetLocalPtr(struct LocalData *Ldata,int m)
{
  return (Ldata->L + Ldata->Lindex + m);
}

double CfromScript(double cfrom_type, double arg2, double arg3, double arg4, double arg5, double arg6, double arg7, struct LocalData *Ldata)
{
  int icfrom_type = (int) cfrom_type;
  double *C, *D, *L, *R, rtn; // C, D, R - only needed if doing Kinmatics
  
  C = GetCVarPtr(Ldata);  // Only needed if doing Kinmatics
  D = GetDVarPtr(Ldata);  // Only needed if doing Kinmatics
  L = GetLVarPtr(Ldata);  // Only needed if using Ldata or Kinmatics
  R = GetRVarPtr(Ldata);  // Only needed if doing Kinmatics
  rtn = -1.0;
  return rtn;
}