194 lines
8.2 KiB
C
194 lines
8.2 KiB
C
//---------------------------------------------------------------------------
|
|
// 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;
|
|
}
|
|
|