//--------------------------------------------------------------------------- // 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 obsvr_servo_ctrl_1(MotorData *Mptr) { //x[n+1]=A*x[n]+B*u //y=C*x[n]+D*x[n] //u=[DesPos, IqMeas, ActVel, ActPos].T //y=[obsvOut].T //double x[4]; //new state //static double _x[4]={0,0,0,0}; //old state //input values double DesPos=Mptr->DesPos; double IqMeas=Mptr->IqMeas; double ActVel=Mptr->ActVel; double ActPos=Mptr->ActPos; //double obsvOut; // output values double kpPosErr,iqCmd,iqCmdOrig; // output values //double maxDac=Mptr->MaxDac; //other stuff double desAcc,Kfff,posErr; static double itg=0; //integrator static double prevDesVel=0; //integrator if (Mptr->ClosedLoop) { //Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000; posErr=DesPos-ActPos; //should be equal to Mptr->PosError kpPosErr=posErr*Mptr->Servo.Kp; itg+=kpPosErr*Mptr->Servo.Ki; if(itg>Mptr->Servo.MaxInt) itg=Mptr->Servo.MaxInt; else if(itg<-Mptr->Servo.MaxInt) itg=-Mptr->Servo.MaxInt; desAcc=Mptr->DesVel-prevDesVel; if(Mptr->DesVel>0) Kfff=Mptr->Servo.Kfff; else Kfff=-Mptr->Servo.Kfff; iqCmd= +kpPosErr -Mptr->Servo.Kvfb*Mptr->ActVel +Mptr->Servo.Kvff*Mptr->DesVel +itg +Mptr->Servo.Kaff*desAcc +Kfff; prevDesVel=Mptr->DesVel; iqCmdOrig=pshm->ServoCtrl(Mptr); //return iqCmd; pshm->P[2010]=DesPos; pshm->P[2011]=IqMeas; pshm->P[2012]=ActVel; pshm->P[2013]=ActPos; //pshm->P[2014]=_x[0]; //pshm->P[2015]=_x[1]; pshm->P[2016]+=iqCmdOrig-iqCmd; pshm->P[2017]=iqCmdOrig; pshm->P[2018]=iqCmd; return iqCmd; } else { Mptr->Servo.Integrator=0.0; return 0.0; } } double obsvr_servo_ctrl_2(MotorData *Mptr) { //x[n+1]=A*x[n]+B*u //y=C*x[n]+D*x[n] //u=[DesPos, IqMeas, ActVel, ActPos].T //y=[obsvOut].T //double x[4]; //new state //static double _x[4]={0,0,0,0}; //old state //input values double DesPos=Mptr->DesPos; double IqMeas=Mptr->IqMeas; double ActVel=Mptr->ActVel; double ActPos=Mptr->ActPos; //double obsvOut; // output values double kpPosErr,iqCmd,iqCmdOrig; // output values //double maxDac=Mptr->MaxDac; //other stuff double desAcc,Kfff,posErr; static double itg=0; //integrator static double prevDesVel=0; //integrator if (Mptr->ClosedLoop) { //Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000; posErr=DesPos-ActPos; //should be equal to Mptr->PosError kpPosErr=posErr*Mptr->Servo.Kp; itg+=kpPosErr*Mptr->Servo.Ki; if(itg>Mptr->Servo.MaxInt) itg=Mptr->Servo.MaxInt; else if(itg<-Mptr->Servo.MaxInt) itg=-Mptr->Servo.MaxInt; desAcc=Mptr->DesVel-prevDesVel; if(Mptr->DesVel>0) Kfff=Mptr->Servo.Kfff; else Kfff=-Mptr->Servo.Kfff; iqCmd= +kpPosErr -Mptr->Servo.Kvfb*Mptr->ActVel +Mptr->Servo.Kvff*Mptr->DesVel +itg +Mptr->Servo.Kaff*desAcc +Kfff; prevDesVel=Mptr->DesVel; iqCmdOrig=pshm->ServoCtrl(Mptr); //return iqCmd; pshm->P[2020]=DesPos; pshm->P[2021]=IqMeas; pshm->P[2022]=ActVel; pshm->P[2023]=ActPos; //pshm->P[2024]=_x[0]; //pshm->P[2025]=_x[1]; pshm->P[2026]+=iqCmdOrig-iqCmd; pshm->P[2027]=iqCmdOrig; pshm->P[2028]=iqCmd; return iqCmd; } else { Mptr->Servo.Integrator=0.0; return 0.0; } } void user_phase_sample( struct MotorData *Mptr) { } double usr_servo_sample(MotorData *Mptr) { pshm->P[2000]=pshm->P[2000]*.9999+abs(Mptr->PosError)*0.0001; //lowpass of Position error return pshm->ServoCtrl(Mptr); } //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; }