sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

friction compensating user servo

Browse Source
This commit is contained in:
zamofing_t
2017-02-01 15:21:56 +01:00
parent c2910090f3
commit 3fafe53c39
14 changed files with 841 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
src/usrServo/usrcode.c
View File

@@ -47,10 +47,81 @@ 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)
{
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)
{
//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;
if(Mptr->DesVel>0)
c+=28.2405057332f;
//c+=45.f;
else if (Mptr->DesVel<0)
c-=61.2462872333f;
//c+=45.f;
else
c+=(28.2f-61.2f)/2.f;
// 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;
@@ -70,7 +141,7 @@ double user_pid_ctrl( struct MotorData *Mptr)
else {
Mptr->Servo.Integrator=0.0;
return 0.0;
}
}
}
void CaptCompISR(void)