mainly document changes
This commit is contained in:
255
python/usr_code/usrcode.c
Normal file
255
python/usr_code/usrcode.c
Normal file
@@ -0,0 +1,255 @@
|
||||
//---------------------------------------------------------------------------
|
||||
// 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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user