/*============================================================================ ============================================================================== vor_pd_task.c ============================================================================== Remarks: Author: Gorang Gandhi, Stefan Schaal Implements a OKR-VOR Feedback Error Learning scheme TO DO: Test out OKR by tracking a sinusoidal target (this may require smooth pursuit) Do sinusoidal motion in multiple directions (finished) Incorporate gyroscopes (problem w/ sensors) FEL needs time allignment b/t inputs and ufb (use eligibility traces) Eligibility trace can be 2nd order linear filter with unimodal peak impulse response Questions about LWPR: why are in inputs the weights also? output is an error? Sensors: ORTH_X,Y,Z measure linear acceleration RIGHT_EAR/LEFT_EAR measure auditory signals VEST_A,B,G are gyroscopes. A,B,G are alpha, beta, gamma (x,y,z maybe) Y-axis is forward, Z-axis is upward, and X-axis is to the right ============================================================================*/ /* vxWorks includes */ #include "stdio.h" #include "math.h" #include "string.h" #ifndef VX #include "strings.h" #endif #include "math.h" #include "vx_headers.h" /* private includes */ #include "SL.h" #include "SL_user.h" #include "tasks.h" #include "task_servo.h" #include "kinematics.h" #include "dynamics.h" #include "lwpr.h" /* defines */ #define R_VOR 11 #define L_VOR 12 #define R_VORt 13 #define L_VORt 14 #define RIGHT 1 // also defined is SL.h #define LEFT 2 /* local variables */ static SL_DJstate target[N_DOFS+1]; // this is a structure in SL.h // desired values for the controller static double amp = 0.2; static double freq = 1.0; static int firsttime; /* // Robotics class constants (same as the publications): static double gain_x = 0.4875; static double gain_xd = 0.0341; static double gain_xdd = 0.00036889; */ // Schaal's constants: static double gain_x = -2.1415e-05; static double gain_xd = 5.9215e-05; static double gain_xdd = 1.7086e-06; // I came up with these from experimentation: static double gain_xt = 0.15; static double gain_xdt = 0.01; static double P = 10; static double D = 0.1; static double I = 0; static double Pt = 6; static double Dt = 0.01; static double It = 0; static int use_pd = 0; /* global functions */ void add_vor_pd_task(void); /* local functions */ // Static makes these funcs invisible outside of this file static int init_vor_pd_task(void); static int run_vor_pd_task(void); static int run_vor_pd_task2(void); static int change_vor_pd_task(void); /***************************************************************************** ****************************************************************************** Function Name : add_vor_pd_task Date : Feb 1999 Remarks: adds the task to the task menu ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ void add_vor_pd_task( void ) { int i, j; addTask("Vor PD Task", init_vor_pd_task, run_vor_pd_task, change_vor_pd_task); } /***************************************************************************** ****************************************************************************** Function Name : init_vor_task Date : Dec. 1997 Remarks: initialization for task sets firsttime = TRUE ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int init_vor_pd_task(void) { int j, i; char string[100]; // not used double max_range=0; // not used int ans; // for lwpr double aux; // not used int flag = FALSE; // not used static int models_read; // for lwpr /* check whether any other task is running */ if (strcmp(current_task_name,NO_TASK) != 0) { //e("Goto task can only be run if no other task is running!\n"); return FALSE; } /* get the amplitude and frequency of the head motion */ get_double("Head amplitude",amp,&); get_double("Head frequency",freq,&freq); // initialize the learning system ans = 0; get_int("Create new LWPR models?",ans,&ans); if (!models_read || ans==1) { if (models_read && ans) { deleteLWPR(R_VOR); deleteLWPR(L_VOR); deleteLWPR(R_VORt); deleteLWPR(L_VORt); } if (!readLWPRScript("learn/l_vor_lwpr_g.script", ans, L_VOR)) { return FALSE; } if (!readLWPRScript("learn/r_vor_lwpr_g.script", ans, R_VOR)) return FALSE; if (!readLWPRScript("learn/l_vor_lwpr_g.script", ans, L_VORt)) { return FALSE; } if (!readLWPRScript("learn/r_vor_lwpr_g.script", ans, R_VORt)) return FALSE; models_read = TRUE; } else { writeLWPR(R_VOR); writeLWPR(L_VOR); writeLWPR(R_VORt); writeLWPR(L_VORt); } /* go to a save posture */ bzero((char *)&(target[1]),N_DOFS*sizeof(target[1])); for (i=1; i<=N_DOFS; ++i) { target[i] = joint_default_state[i]; } // Goes to the desired target position, which is the joint default state if (!go_target_wait_ID(target)) return FALSE; /* ready to go */ ans = 999; while (ans == 999) { if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans)) return FALSE; } if (ans != 1) return FALSE; firsttime = TRUE; return TRUE; } /***************************************************************************** ****************************************************************************** Function Name : run_vor_pd_task Date : Dec. 1997 Remarks: run the task from the task servo: REAL TIME requirements! This is my version So far does not work well at all with Robotics class constants Works perfectly w/ just forward model and Schaals constants Works pefectly using fcp hypothesis w/ or w/o learning The PID feedback controller does not work when simply added to the forward model. It should decrease the amount of oscillation in the retinal slip. According to the Biom_Gaze_Stabilizing and the final common pathway hypothesis The PID/PD controller should be added (sub?) to the input of the forward model Doesnt seem to be working for the tilting motion The weights for the uff command (forward model) do make a difference but dont need to be the exact values. ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int run_vor_pd_task(void) { int j, i; static double offset=0; // used for task servo offsett double rsp[2+1],rsdp[2+1]; // retinal slip and vel of rs (pan) double ufbp[2+1]; // feedback motor command double ufmp[2+1]; // forward model motor command double integralp[2+1] = {0,0,0}; // Integral term for PID controller double fcp_velp[2+1]; // forward command pathway velocity static double in_rp[2+1]; // used for LWPR static double in_lp[2+1]; // used for LWPR double out_rp[1+1]; // used for LWPR double out_lp[1+1]; // used for LWPR static double fcp_posp[2+1]; // forward command pathway position static int wait_ticks; // wait for learning static int rfIDp; // used for LWPR double rst[2+1],rsdt[2+1]; // retinal slip and vel of rs (tilt) double ufbt[2+1]; // feedback motor command double ufmt[2+1]; // forward model motor command double integralt[2+1] = {0,0,0}; // Integral term for PID controller double fcp_velt[2+1]; // forward command pathway velocity static double in_rt[2+1]; // used for LWPR static double in_lt[2+1]; // used for LWPR double out_rt[1+1]; // used for LWPR double out_lt[1+1]; // used for LWPR static double fcp_post[2+1]; // forward command pathway position static int rfIDt; // used for LWPR // compute the retinal slip // retinal slip = theta_target - (theta_head + theta_eye), in this case the target = 0. // Just retinal slip does not work well at all.... rsp[RIGHT] = joint_state[B_HR].th + joint_state[R_EP].th; rsdp[RIGHT] = joint_state[B_HR].thd + joint_state[R_EP].thd; rsp[LEFT] = joint_state[B_HR].th + joint_state[L_EP].th; rsdp[LEFT] = joint_state[B_HR].thd + joint_state[L_EP].thd; rst[RIGHT] = joint_state[B_HN].th + joint_state[R_ET].th; rsdt[RIGHT] = joint_state[B_HN].thd + joint_state[R_ET].thd; rst[LEFT] = joint_state[B_HN].th + joint_state[L_ET].th; rsdt[LEFT] = joint_state[B_HN].thd + joint_state[L_ET].thd; // the feedback controller (OKR) // May not need I term integralp[RIGHT] = (integralp[RIGHT] + rsp[RIGHT])/(double)task_servo_rate; // Integral term = T*sum(retinal slip) // task_servo_rate is around 400 - 500 Hz integralp[LEFT] = (integralp[LEFT] + rsp[LEFT])/(double)task_servo_rate; ufbp[RIGHT] = P*rsp[RIGHT] + D*rsdp[RIGHT] + I*integralp[RIGHT]; ufbp[LEFT] = P*rsp[LEFT] + D*rsdp[LEFT] + I*integralp[LEFT]; integralt[RIGHT] = (integralt[RIGHT] + rst[RIGHT])/(double)task_servo_rate; // Integral term = T*sum(retinal slip) // task_servo_rate is around 400 - 500 Hz integralt[LEFT] = (integralt[LEFT] + rst[LEFT])/(double)task_servo_rate; ufbt[RIGHT] = Pt*rst[RIGHT] + Dt*rsdt[RIGHT] + It*integralt[RIGHT]; ufbt[LEFT] = Pt*rst[LEFT] + Dt*rsdt[LEFT] + It*integralt[LEFT]; if(firsttime) { firsttime = FALSE; offset = task_servo_time; // offset from entering the initialzing file maybe fcp_posp[RIGHT] = joint_state[R_EP].th; // this may be the initial state for the integration? fcp_posp[LEFT] = joint_state[L_EP].th; // used to be R_EP fcp_post[RIGHT] = joint_state[R_ET].th; // this may be the initial state for the integration? fcp_post[LEFT] = joint_state[L_ET].th; // used to be R_EP // this does not help remove the drift: /* fcp_pos[RIGHT] = 0; // this may be the initial state for the integration? fcp_pos[LEFT] = 0; // used to be R_EP */ wait_ticks = 480; } else if (--wait_ticks < 0) { out_rp[1]= -ufbp[RIGHT] * 0.001; // is this using feedback error learning, 0.001 is a forgetting factor (maybe) b/c ufb is approx out_lp[1]= -ufbp[LEFT] * 0.001; addDataToLWPR(R_VOR,in_rp,in_rp,out_rp,TRUE,&rfIDp); // addDataToLWPR(ID,weights,input,output,output is an error,&rfID) addDataToLWPR(L_VOR,in_lp,in_lp,out_lp,TRUE,&rfIDp); out_rt[1]= -ufbt[RIGHT] * 0.001; // is this using feedback error learning, 0.001 is a forgetting factor (maybe) b/c ufb is approx out_lt[1]= -ufbt[LEFT] * 0.001; addDataToLWPR(R_VORt,in_rt,in_rt,out_rt,TRUE,&rfIDt); // addDataToLWPR(ID,weights,input,output,output is an error,&rfID) addDataToLWPR(L_VORt,in_lt,in_lt,out_lt,TRUE,&rfIDt); } // the desired head movement // this will be sent to the motor servo automatically joint_des_state[B_HR].th = amp * sin(2.*PI*freq*(task_servo_time-offset)); joint_des_state[B_HR].thd = 2.*PI*freq*amp * cos(2.*PI*freq*(task_servo_time-offset)); // derivative joint_des_state[B_HN].th = amp * sin(2.*PI*freq*(task_servo_time-offset)); joint_des_state[B_HN].thd = 2.*PI*freq*amp * cos(2.*PI*freq*(task_servo_time-offset)); // derivative joint_des_state[B_HT].th = amp * sin(2.*PI*freq*(task_servo_time-offset)); // tilt is left to right joint_des_state[B_HT].thd = 2.*PI*freq*amp * cos(2.*PI*freq*(task_servo_time-offset)); // derivative // compute the learning output // these inputs are very similar to that of figure 5 but takes out the PD controller of VOR in_rp[1] = fcp_posp[RIGHT]; in_rp[2] = -joint_state[B_HR].thd; // only use head velocity for learning -- OKR // is otherwise interfering with VOR learning //in_rp[2] = -joint_state[B_HR].thd - ufbp[RIGHT]; // this is the FEL framework. Does not work better in_lp[1] = fcp_posp[LEFT]; in_lp[2] = -joint_state[B_HR].thd; //in_lp[2] = -joint_state[B_HR].thd - ufbp[LEFT]; // this is the FEL framework. // predictLWPROutput(ID, weights, input, cutoff, blend predictions, output, rfID), returns max activation if (predictLWPROutput(R_VOR, in_rp, in_rp, 0.001, TRUE, out_rp, &i) <= 0) { out_rp[1] = 0.0; } if (predictLWPROutput(L_VOR, in_lp, in_lp, 0.001, TRUE, out_lp, &i) <= 0) { out_lp[1] = 0.0; } // compute the learning output // these inputs are very similar to that of figure 5 but takes out the PD controller of VOR in_rt[1] = fcp_post[RIGHT]; in_rt[2] = -joint_state[B_HT].thd; // only use head velocity for learning -- OKR // is otherwise interfering with VOR learning //in_rt[2] = -joint_state[B_HT].thd - ufbt[RIGHT]; // this is the FEL framework. in_lt[1] = fcp_post[LEFT]; in_lt[2] = -joint_state[B_HT].thd; //in_lt[2] = -joint_state[B_HT].thd - ufbt[LEFT]; // this is the FEL framework. // predictLWPROutput(ID, weights, input, cutoff, blend predictions, output, rfID), returns max activation if (predictLWPROutput(R_VORt, in_rt, in_rt, 0.001, TRUE, out_rt, &j) <= 0) { out_rt[1] = 0.0; } if (predictLWPROutput(L_VORt, in_lt, in_lt, 0.001, TRUE, out_lt, &j) <= 0) { out_lt[1] = 0.0; } // the motor command /* // Forward model based on head motion (VOR) // This does not work well, just adding the feedback and feedforward models: ufm[RIGHT] = gain_x*joint_state[B_HR].th + gain_xd*joint_state[B_HR].thd + gain_xdd*joint_state[B_HR].thdd; ufm[LEFT] = gain_x*joint_state[B_HR].th + gain_xd*joint_state[B_HR].thd + gain_xdd*joint_state[B_HR].thdd; joint_des_state[R_EP].uff = -ufb[RIGHT] -ufm[RIGHT]; joint_des_state[L_EP].uff = -ufb[LEFT] -ufm[LEFT] ; joint_des_state[R_EP].uff = -ufm[RIGHT]; joint_des_state[L_EP].uff = -ufm[LEFT] ; // This will use the internal PD controller based on the head motion (same as the publications) joint_des_state[R_EP].th = -joint_state[B_HR].th; joint_des_state[R_EP].thd = -joint_state[B_HR].thd; joint_des_state[L_EP].th = -joint_state[B_HR].th; joint_des_state[L_EP].thd = -joint_state[B_HR].thd; */ // the final common pathway command fcp_velp[RIGHT] = -ufbp[RIGHT] - joint_state[B_HR].thd; // this is vel. pathway of the forward model fcp_velp[LEFT] = -ufbp[LEFT] - joint_state[B_HR].thd; // the integrator update fcp_posp[RIGHT] += fcp_velp[RIGHT] /(double)task_servo_rate; // task_servo_rate is around 400 - 500 Hz fcp_posp[LEFT] += fcp_velp[LEFT] /(double)task_servo_rate; // this is the integral pathway // the final common pathway command fcp_velt[RIGHT] = -ufbt[RIGHT] - joint_state[B_HT].thd; // this is vel. pathway of the forward model fcp_velt[LEFT] = -ufbt[LEFT] - joint_state[B_HT].thd; // the integrator update fcp_post[RIGHT] += fcp_velt[RIGHT] /(double)task_servo_rate; // task_servo_rate is around 400 - 500 Hz fcp_post[LEFT] += fcp_velt[LEFT] /(double)task_servo_rate; // this is the integral pathway // the motor command // this is similar to feedback error learning scheme in figure 5 // this works well /* joint_des_state[R_EP].uff = fcp_vel[RIGHT]*gain_xd + fcp_pos[RIGHT]*gain_x; // using inverse dynamics regressed from real data joint_des_state[L_EP].uff = fcp_vel[LEFT]*gain_xd + fcp_pos[LEFT]*gain_x; */ // This works well also /* joint_des_state[R_EP].uff = fcp_vel[RIGHT] * 0.00001 + fcp_pos[RIGHT] * 0.0005; // B = 0.00001, K = 0.0005 joint_des_state[L_EP].uff = fcp_vel[LEFT] * 0.00001 + fcp_pos[LEFT] * 0.0005; */ // This works well also joint_des_state[R_EP].uff = fcp_velp[RIGHT] * gain_xd + fcp_posp[RIGHT] * gain_x + out_rp[1];; // B = gain_xd, K = gain_x joint_des_state[L_EP].uff = fcp_velp[LEFT] * gain_xd + fcp_posp[LEFT] * gain_x + out_lp[1]; // This works well also joint_des_state[R_ET].uff = fcp_velt[RIGHT] * gain_xdt + fcp_post[RIGHT] * gain_xt + out_rt[1]; // B = gain_xdt, K = gain_xt joint_des_state[L_ET].uff = fcp_velt[LEFT] * gain_xdt + fcp_post[LEFT] * gain_xdt + out_lt[1]; // Gorang added this to cancel out the internal PD controller: // this does not help! // this adds a slow drift /* joint_des_state[R_EP].th = joint_state[R_EP].th; joint_des_state[R_EP].thd = joint_state[R_EP].thd; joint_des_state[L_EP].th = joint_state[L_EP].th; joint_des_state[L_EP].thd = joint_state[L_EP].thd; */ // Printing sensor states: // the order: // VEST_A=1, VEST_B, VEST_G, ORTH_X, ORTH_Y, ORTH_Z, RIGHT_EAR, LEFT_EAR // This gives me all zeros /* int k; for(k = 1; k <= N_MISC_SENSORS; k++) { printf("%f ",misc_sensor[k]); printf("%f ",misc_sim_sensor[k]); } */ return TRUE; } /***************************************************************************** ****************************************************************************** Function Name : run_vor_pd_task2 Date : Dec. 1997 Remarks: run the task from the task servo: REAL TIME requirements! This Schaal's code with the learning This final common pathway is from Biom_Gaze_Stable fig 4 ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int run_vor_pd_task2(void) { int j, i; static double offset=0; // used for task servo offsett double rs[2+1],rsd[2+1]; // retinal slip and vel of rs double ufb[2+1]; // feedback motor command double fcp_vel[2+1]; // forward command pathway velocity static double in_r[2+1]; // used for LWPR static double in_l[2+1]; // used for LWPR double out_r[1+1]; // used for LWPR double out_l[1+1]; // used for LWPR static double fcp_pos[2+1]; // forward command pathway position static int wait_ticks; // wait for learning static int rfID; // used for LWPR // compute the retinal slip // retinal slip = theta_target - (theta_head + theta_eye), in this case the target = 0. rs[RIGHT] = joint_state[B_HR].th + joint_state[R_EP].th; rsd[RIGHT] = joint_state[B_HR].thd + joint_state[R_EP].thd; rs[LEFT] = joint_state[B_HR].th + joint_state[L_EP].th; rsd[LEFT] = joint_state[B_HR].thd + joint_state[L_EP].thd; // the feedback controller ufb[RIGHT] = P*rs[RIGHT] + D*rsd[RIGHT]; ufb[LEFT] = P*rs[LEFT] + D*rsd[LEFT]; // leaning box if (firsttime) { firsttime = FALSE; offset = task_servo_time; fcp_pos[RIGHT] = joint_state[R_EP].th; // this may be the initial state for the integration? fcp_pos[LEFT] = joint_state[L_EP].th; // used to be R_EP wait_ticks = 480; } else if (--wait_ticks < 0){ out_r[1]= -ufb[RIGHT] * 0.001; // is this using feedback error learning, 0.001 is a forgetting factor (maybe) b/c ufb is approx out_l[1]= -ufb[LEFT] * 0.001; addDataToLWPR(R_VOR,in_r,in_r,out_r,TRUE,&rfID); // addDataToLWPR(ID,weights,input,output,output is an error,&rfID) addDataToLWPR(L_VOR,in_l,in_l,out_l,TRUE,&rfID); } // the desired head movement // this will be sent to the motor servo automatically joint_des_state[B_HR].th = amp * sin(2.*PI*freq*(task_servo_time-offset)); joint_des_state[B_HR].thd = 2.*PI*freq*amp * cos(2.*PI*freq*(task_servo_time-offset)); // derivative //#if 1 // this will always execute // the final common pathway command fcp_vel[RIGHT] = -ufb[RIGHT] - joint_state[B_HR].thd; // this is vel. pathway of the forward model fcp_vel[LEFT] = -ufb[LEFT] - joint_state[B_HR].thd; // the integrator update fcp_pos[RIGHT] += fcp_vel[RIGHT] /(double)task_servo_rate; // task_servo_rate is around 400 - 500 Hz fcp_pos[LEFT] += fcp_vel[LEFT] /(double)task_servo_rate; // this is the integral pathway /* #else // the final common pathway command fcp_vel[RIGHT] = -rsd[RIGHT] - joint_state[B_HR].thd; fcp_vel[LEFT] = -rsd[LEFT] - joint_state[B_HR].thd; // the integrator update fcp_pos[RIGHT] += (fcp_vel[RIGHT]-rs[RIGHT]*0.01) /(double)task_servo_rate; fcp_pos[LEFT] += (fcp_vel[LEFT]-rs[LEFT]*0.01) /(double)task_servo_rate; #endif */ // compute the learning output // these inputs are very similar to that of figure 5 but takes out the PD controller of VOR in_r[1] = fcp_pos[RIGHT]; in_r[2] = -joint_state[B_HR].thd; // only use head velocity for learning -- OKR // is otherwise interfering with VOR learning in_l[1] = fcp_pos[LEFT]; in_l[2] = -joint_state[B_HR].thd; out_r[1] = 0.0; out_l[1] = 0.0; // predictLWPROutput(ID, weights, input, cutoff, blend predictions, output, rfID), returns max activation if (predictLWPROutput(R_VOR, in_r, in_r, 0.001, TRUE, out_r, &i) <= 0) { out_r[1] = 0.0; } if (predictLWPROutput(L_VOR, in_l, in_l, 0.001, TRUE, out_l, &i) <= 0) { out_l[1] = 0.0; } // the motor command // this is similar to feedback error learning scheme in figure 5 /* joint_des_state[R_EP].uff = fcp_vel[RIGHT] * 0.00001 + fcp_pos[RIGHT] * 0.0005 + out_r[1]; // B = 0.00001, K = 0.0005 joint_des_state[L_EP].uff = fcp_vel[LEFT] * 0.00001 + fcp_pos[LEFT] * 0.0005 + out_l[1]; */ joint_des_state[R_EP].uff = fcp_vel[RIGHT] * gain_xd + fcp_pos[RIGHT] * gain_x + out_r[1]; // B = 0.00001, K = 0.0005 joint_des_state[L_EP].uff = fcp_vel[LEFT] * gain_xd + fcp_pos[LEFT] * gain_x + out_l[1]; // this also works very well /* joint_des_state[R_EP].uff = fcp_vel[RIGHT] * 0.00001 + fcp_pos[RIGHT] * 0.0005; // B = 0.00001, K = 0.0005 joint_des_state[L_EP].uff = fcp_vel[LEFT] * 0.00001 + fcp_pos[LEFT] * 0.0005; */ // Gorang added this to cancel out the internal PD controller: // this does not help! /* joint_des_state[R_EP].th = joint_state[R_EP].th; joint_des_state[R_EP].thd = joint_state[R_EP].thd; joint_des_state[L_EP].th = joint_state[L_EP].th; joint_des_state[L_EP].thd = joint_state[L_EP].thd; */ // quick hack: inverse dynamics regressed from real data, using acceleration and PD // servo // this works pretty well /* joint_des_state[L_EP].uff = -(-2.1415e-05*joint_state[B_HR].th + 5.9215e-05*joint_state[B_HR].thd + 1.7086e-06*joint_state[B_HR].thdd); joint_des_state[L_EP].th = -joint_state[B_HR].th; joint_des_state[L_EP].thd = -joint_state[B_HR].thd; joint_des_state[R_EP].uff = -(-2.1415e-05*joint_state[B_HR].th + 5.9215e-05*joint_state[B_HR].thd + 1.7086e-06*joint_state[B_HR].thdd); joint_des_state[R_EP].th = -joint_state[B_HR].th; joint_des_state[R_EP].thd = -joint_state[B_HR].thd; //joint_des_state[R_EP].uff=0; // huh? why is this zero? (just to serve as an comparison to the other eye) */ // optional: we can also send the position & velocity to the PD servo // this seems to help a bit if (use_pd) { joint_des_state[R_EP].th = fcp_pos[RIGHT]; joint_des_state[R_EP].thd = fcp_vel[RIGHT]; joint_des_state[L_EP].th = fcp_pos[LEFT]; joint_des_state[L_EP].thd = fcp_vel[LEFT]; } return TRUE; } /***************************************************************************** ****************************************************************************** Function Name : change_vor_task Date : Dec. 1997 Remarks: changes the task parameters ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int change_vor_pd_task(void) { int j, i; get_double("Gain xt",gain_xt,&gain_xt); get_double("Gain xdt",gain_xdt,&gain_xdt); get_double("P",P,&P); get_double("I",I,&I); get_double("D",D,&D); get_double("Pt",Pt,&Pt); get_double("It",It,&It); get_double("Dt",Dt,&Dt); // get_int("Use PD?",use_pd,&use_pd); return TRUE; }