/*!****************************************************************************
******************************************************************************
\note initOsc
\date Aug. 2010
\remarks
initializes the oscilloscope processing
*****************************************************************************
Function Parameters: [in]=input,[out]=output
none
*****************************************************************************/
void
initOsc(void)
{
int rc;
char string[40];
char fname[100];
FILE *fp;
if (no_graphics_flag)
return;
if (read_parameter_pool_int(config_files[PARAMETERPOOL],"osc_enabled", &rc))
osc_enabled = macro_sign(abs(rc));
if (osc_enabled) {
addToMan("oscMenu","interactively change oscilloscope settings",oscMenu);
sprintf(osc_vars_name,"%s_default.osc",servo_name);
// make sure that the default exists
sprintf(fname,"%s%s",PREFS,osc_vars_name);
fp = fopen(fname,"a");
fclose(fp);
sprintf(string,"osc_vars_%s_file",servo_name);
if (read_parameter_pool_string(config_files[PARAMETERPOOL],string,fname))
strcpy(osc_vars_name,fname);
readOscVarsScript(osc_vars_name,FALSE);
}
}
/*!*****************************************************************************
*******************************************************************************
\note receive_des_commands
\date Nov. 2007
\remarks
reads the commands from the joint_sim_state shared memory
structure
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
receive_des_commands(void)
{
int i;
extern double *controller_gain_th;
extern double *controller_gain_thd;
if (semTake(sm_des_commands_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
for (i=1; i<=n_dofs; ++i) {
joint_sim_state[i].u = (double) sm_des_commands->des_command[i].u -
(double) sm_des_commands->des_command[i].upd;
joint_des_state[i].th = (double) sm_des_commands->des_command[i].th;
joint_des_state[i].thd = (double) sm_des_commands->des_command[i].thd;
}
// re-generate and add the PD command from the motor servo, as the
// delay between motor servo and simulation servo can cause instabilities
// in case of stiff contacts -- the reason is that the state info on the
// motor servo is one tick delayed; the old PD command has been subtracted
// from the total command above
for (i=1; i<=n_dofs; ++i) {
if (sm_des_commands->des_command[i].upd != 0.0) {
joint_sim_state[i].u += (joint_des_state[i].th - joint_sim_state[i].th) *
controller_gain_th[i];
joint_sim_state[i].u += (joint_des_state[i].thd - joint_sim_state[i].thd) *
controller_gain_thd[i];
}
if (fabs(joint_sim_state[i].u) > u_max[i]) {
joint_sim_state[i].u = macro_sign(joint_sim_state[i].u) * u_max[i];
}
}
semGive(sm_des_commands_sem);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : run_elbow_perturbation
Date : January 2015
Remarks:
run the task from the task servo: REAL TIME requirements!
******************************************************************************
Parameters: (i/o = input/output)
none
*****************************************************************************/
static int
run_elbow_perturbation(void)
{
int i;
double tau;
double task_time;
task_time = task_servo_time - start_time;
// some extra filtering for better inertial compensation (inherited from Michael Mistry's program)
for (i=1; i<=N_DOFS; ++i)
{
joint_filt_state[i] = extra_joint_state_filter(i);
}
if (joint_state[R_TL].th < R_TL_threshold) // if joint R_TL is below threshold (thumb-in switch is activated)
{
for (i=1; i<=N_DOFS; ++i)
{
// The one that is very noisy is the joint acceleration sensing, that's why we are using the filtered version of it
// (joint_filt_state[i].thdd), instead of the unfiltered one (joint_state[i].thdd). Joint position (joint_state[i].th) and joint velocity (joint_state[i].thd)
// on the other hand are not so noisy, so we are using the original (unfiltered) one.
if (i == R_EB)
{
joint_des_state[i].th = joint_state[i].th;
joint_des_state[i].thd = joint_state[i].thd;
joint_des_state[i].thdd = joint_filt_state[i].thdd * inertia_gain; // multiplication by inertia gain here is maybe for reducing the risk of the system becoming unstable???
joint_des_state[i].uff = 0.0;
}
else
{
joint_des_state[i].th = joint_des_state[i].th; // don't change the joint_des_state;
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
joint_des_state[i].uff = 0.0;
}
}
SL_InvDynNE(joint_state,joint_des_state,endeff,&base_state,&base_orient);
for (i=1; i<=N_DOFS; ++i)
{
if (i == R_EB)
{
// Command joint PD servo to track the current state, in other words, cancel out the joint feedback control:
joint_des_state[i].th = joint_state[i].th;
joint_des_state[i].thd = joint_state[i].thd;
joint_des_state[i].thdd = joint_state[i].thdd;
if (i < R_TL)
{
tau = kp_drag_gain * (joint_des_drag_state[i].th-joint_state[i].th);
//tau = kp_drag_gain * (joint_des_drag_state[i].th-joint_state[i].th) +
// exp(-kd_damping_factor*fabs(joint_state[i].thd)) * kd_drag_gain * (-joint_state[i].thd);
if (fabs(tau) > f_saturation)
{
tau = macro_sign(tau) * f_saturation;
}
joint_des_state[i].uff += tau;
if (fabs(tau) > drag_threshold)
{
joint_des_drag_state[i].th = joint_state[i].th;
}
}
}
}
}
else // if (joint_state[R_TL].th >= R_TL_threshold), or in other words if (thumb-in switch is NOT activated)
{
// inverse dynamics for dynamic compensation
for (i=1; i<=N_DOFS; ++i)
{
joint_des_state[i].th = joint_des_state[i].th; // don't change the joint_des_state;
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
joint_des_state[i].uff = 0.0;
}
SL_InvDynNE(joint_state,joint_des_state,endeff,&base_state,&base_orient);
}
}
/*!*****************************************************************************
*******************************************************************************
\note initOscWindow
\date May 2010
\remarks
initializes the oscilloscope window
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
initOscWindow(void)
{
int i,j;
int x = 600;
int y = 20;
int width = 600;
int height = 400;
char string[100];
char xstring[100];
Display *disp;
int screen_num;
int display_width;
int display_height;
double aux;
// is the oscilloscope enabled?
if (read_parameter_pool_int(config_files[PARAMETERPOOL],"osc_enabled", &i))
osc_enabled = macro_sign(abs(i));
if (!osc_enabled)
return TRUE;
// how many oscilloscope graphs?
if (read_parameter_pool_int(config_files[PARAMETERPOOL],"n_osc_plots", &i)) {
if (i > 0)
n_oscilloscope_plots = i;
}
// #periods of oscilloscope A/D plot
if (read_parameter_pool_int(config_files[PARAMETERPOOL],"osc_periods_ad", &i)) {
if (i > 0)
periods_window_AD = i;
}
// window size of variable display
if (read_parameter_pool_double(config_files[PARAMETERPOOL],"osc_time_window_vars", &aux)) {
if (aux > 0)
time_window_vars = aux;
}
// allocate memory for the plots and initialize
osc_data = (OscData *) my_calloc(n_oscilloscope_plots+1,sizeof(OscData),MY_STOP);
for (i=0; i<=n_oscilloscope_plots; ++i) {
for (j=1; j<=MAX_VARS_PER_PLOT; ++j) {
osc_data[i].current_index[j] = 1;
}
osc_data[i].max = -1.e10;
osc_data[i].min = 1.e10;
}
// connect to X server using the DISPLAY environment variable
if ( (disp=XOpenDisplay(NULL)) == NULL ) {
printf("Cannot connect to X servo %s\n",XDisplayName(NULL));
exit(-1);
}
// get screen size from display structure macro
screen_num = DefaultScreen(disp);
display_width = DisplayWidth(disp, screen_num);
display_height = DisplayHeight(disp, screen_num);
// overwrite the default window options from ParameterPool.cf
if (read_parameter_pool_string(config_files[PARAMETERPOOL],
"osc_window_geometry", string))
parseWindowSpecs(string, display_width,display_height,xstring,
&x,
&y,
&width,
&height);
// create the window
glutInitWindowPosition(x,y);
glutInitWindowSize(width,height);
openGLId_osc = glutCreateWindow("Oscilloscope"); // makes window current, too
// attach appropriate OpenGL functions to the current window
glutDisplayFunc(osc_display);
glutReshapeFunc(osc_reshape);
/*
glutIdleFunc(idle);
glutKeyboardFunc(keyboard);
glutMouseFunc(mouse);
glutMotionFunc(motion);
glutSpecialFunc(special);
glutMenu(wptr);
*/
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note generate_total_commands
\date April 1999
\remarks
this is the feedback/feedforward controller
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
generate_total_commands( void)
{
int j,i;
#ifdef VX
power_on = user_power_status();
#else
power_on = TRUE;
#endif
if (power_on == -1)
return FALSE;
switch (controller_kind) {
case PD:
for (i=1; i<=n_dofs; ++i) {
ufb[i] = 0.0;
if (zero_ufb_P_flag[i] > 0)
--zero_ufb_P_flag[i];
else
ufb[i] += (joint_des_state[i].th - joint_state[i].th) *
controller_gain_th[i];
if (zero_ufb_D_flag[i] > 0)
--zero_ufb_D_flag[i];
else
ufb[i] += (joint_des_state[i].thd - joint_state[i].thd) *
controller_gain_thd[i];
u[i] = ufb[i];
upd[i] = ufb[i];
}
break;
case PID:
for (i=1; i<=n_dofs; ++i) {
ufb[i] = 0.0;
if (zero_ufb_P_flag[i] > 0)
--zero_ufb_P_flag[i];
else {
integrator_state[i] += joint_des_state[i].th - joint_state[i].th;
ufb[i] += (joint_des_state[i].th - joint_state[i].th) *
controller_gain_th[i];
}
if (zero_ufb_D_flag[i] > 0)
--zero_ufb_D_flag[i];
else
ufb[i] += (joint_des_state[i].thd - joint_state[i].thd) *
controller_gain_thd[i];
if (!power_on)
integrator_state[i] = 0;
upd[i] = ufb[i];
ufb[i] += integrator_state[i] * controller_gain_int[i];
u[i] = ufb[i];
}
break;
case PDFF:
for (i=1; i<=n_dofs; ++i) {
ufb[i] = 0.0;
if (zero_ufb_P_flag[i] > 0)
--zero_ufb_P_flag[i];
else
ufb[i] += (joint_des_state[i].th - joint_state[i].th) *
controller_gain_th[i];
if (zero_ufb_D_flag[i] > 0)
--zero_ufb_D_flag[i];
else
ufb[i] += (joint_des_state[i].thd - joint_state[i].thd) *
controller_gain_thd[i];
upd[i] = ufb[i];
u[i] = ufb[i] + joint_des_state[i].uff;
}
break;
case PIDFF:
for (i=1; i<=n_dofs; ++i) {
ufb[i] = 0.0;
if (zero_ufb_P_flag[i] > 0)
--zero_ufb_P_flag[i];
else {
integrator_state[i] += joint_des_state[i].th - joint_state[i].th;
ufb[i] += (joint_des_state[i].th - joint_state[i].th) *
controller_gain_th[i];
}
if (zero_ufb_D_flag[i] > 0)
--zero_ufb_D_flag[i];
else
ufb[i] += (joint_des_state[i].thd - joint_state[i].thd) *
controller_gain_thd[i];
if (!power_on)
integrator_state[i] = 0;
upd[i] = ufb[i];
ufb[i] += integrator_state[i] * controller_gain_int[i];
u[i] = ufb[i] + joint_des_state[i].uff;
}
break;
case FF:
for (i=1; i<=n_dofs; ++i) {
u[i] = joint_des_state[i].uff;
}
break;
default:
stop("Invalid Controller");
}
// allow the user to modify the controller
user_controller(u,ufb);
// apply the new control
for (i=1; i<=n_dofs; ++i) {
if (my_isnan(u[i])) {
char string[100];
bzero((char *)&(ufb[1]),sizeof(double)*n_dofs);
bzero((char *)&(u[1]),sizeof(double)*n_dofs);
sprintf(string,"NaN in motor command of joint %s",joint_names[i]);
stop(string);
}
if (fabs(u[i]) > u_max[i]) {
u[i] = macro_sign(u[i]) * u_max[i];
}
joint_state[i].u = u[i];
joint_state[i].ufb = ufb[i];
}
return TRUE;
}
