mirror.c

Go to the documentation of this file.

/********************************************************************\

  Name:         mirror.c
  Created by:   Andreas Suter  2003/02/19

  Contents:     device driver for the bloody mirror controller

  RS232:        9600 baud, 8 data bits, 1 stop bit, no parity bit

  Comments:     angle () = (adc_value - 2048) * 5 / 1024
                1 step of the stepper motor = 0.01

                02-Sep-2005: 45 degree position corresponds to -2.0 degree at controller
                             this corresponds to 294.8 degree at nonius (this info is
                             from the old VMS DCL script nemu_mirror.com)

        The mirror controller software is written in basic (stamp2 from parallax,
        see http://www.parallax.com/). The actual basic controller version
        is 'mor5.bs2' and can be found in the CVS as well. Since the basic
        controller software is strictly sequential AND slow, some ugly routines
        are included in this in this C driver.

\********************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>

#include "midas.h"

#include "mirror.h"

#include "ets_logout.h"

#define MIRROR_ETS_LOGOUT_SLEEP 10000

/*---- globals -----------------------------------------------------*/

typedef struct {
  INT  ets_in_use;        
  int  time_out;          
  int  adc_45;            
  char names[2][32];      
} MIRROR_DD_SETTINGS;

#define MIRROR_SETTINGS_DD_STR "\
ETS_IN_USE   = INT : 1\n\
TCPIPTimeOut = INT : 500\n\
MirrorADC45  = INT : 1976\n\
Names        = STRING[2] :\n\
[32] Reset\n\
[32] Angle\n\
"

typedef struct {
  MIRROR_DD_SETTINGS mirror_dd_settings; 
  INT (*bd)(INT cmd, ...);               
  void *bd_info;                         
  HNDLE hDB;                             
  HNDLE hkey;                            
  HNDLE hKeyDemand;                      
  HNDLE hKeyInfo;                        
  INT   errorcount;                      
  INT   startup_error;                   
  DWORD lasterrtime;                     
  int   bd_connected;                    
  DWORD last_reconnect;                  
  float last_value;                      
  int   reconnection_failures;           
} MIRROR_INFO;

// FOR DEBUG ONLY !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
int histo[MIRROR_MAX_TRY]; // histo in order to see how many attempts to read were necessary

/*---- device driver routines --------------------------------------*/
/*----------------------------------------------------------------------------------*/
INT mirror_init(HNDLE hKey, void **pinfo, INT channels, INT (*bd)(INT cmd, ...))
{

  INT          status;
  HNDLE        hDB, hKeyDD;
  MIRROR_INFO *info;

  if (MIRROR_DEBUG) { // init read histo
    int i;
    for (i=0; i<MIRROR_MAX_TRY; i++) histo[i]=0;
  }

  cm_get_experiment_database(&hDB, NULL);

  // allocate info structure
  info = calloc(1, sizeof(MIRROR_INFO));
  *pinfo = info;

  // create MIRROR settings record
  status = db_create_record(hDB, hKey, "DD", MIRROR_SETTINGS_DD_STR);
  if (status != DB_SUCCESS)
    return FE_ERR_ODB;

  // get key to DD structure
  status = db_find_key(hDB, hKey, "DD", &hKeyDD);
  if (status != DB_SUCCESS)
    return FE_ERR_ODB;

  // create hot-link onto 'DD' structure
  status = db_open_record(hDB, hKeyDD, &info->mirror_dd_settings, sizeof(info->mirror_dd_settings),
                          MODE_READ, NULL, NULL);
  if (status != DB_SUCCESS)
    return FE_ERR_ODB;

  // initialize driver
  info->bd = bd;
  info->hDB = hDB;
  info->hkey = hKey;
  info->errorcount = 0;
  info->startup_error = 0;
  info->lasterrtime = ss_time();
  info->bd_connected = 0;
  info->last_reconnect = ss_time();
  info->reconnection_failures = 0;

  // get and store some handles needed
  if (db_find_key(info->hDB, 0, "/Equipment/Mirror/Variables/Demand", &info->hKeyDemand) != DB_SUCCESS) {
    cm_msg(MINFO, "mirror_init", "WARNING: Couldn't get Key '/Equipment/Mirror/Variables/Demand' wasn't found in OBD.");
    cm_yield(0);
    return FE_SUCCESS;
  }
  if (db_find_key(info->hDB, 0, "/Info/Mirror/Angle horizontal", &info->hKeyInfo) != DB_SUCCESS) {
    cm_msg(MINFO, "mirror_init", "WARNING: Couldn't get Key '/Info/Mirror/Angle horizontal' wasn't found in OBD.");
    cm_yield(0);
    return FE_SUCCESS;
  }

  // pointer to bus driver available?
  if (!bd)
    return FE_ERR_ODB;

  // initialize bus driver
  status = info->bd(CMD_INIT, hKey, &info->bd_info);
  if (status != SUCCESS) {
    info->startup_error = 1;
    return status;
  }
  cm_msg(MINFO,"mirror_init","MIRROR driver initialized.");
  cm_yield(0);
  info->bd_connected = 1;

  return FE_SUCCESS;
}

/*----------------------------------------------------------------------------------*/
INT mirror_exit(MIRROR_INFO *info)
{

  // call EXIT function of bus driver, usually closes device
  info->bd(CMD_EXIT, info->bd_info);

  free(info);

  return FE_SUCCESS;
}

/*----------------------------------------------------------------------------------*/
INT mirror_get_current_position(MIRROR_INFO *info, char *str)
{
  char cmd[MIRROR_MAX_STR];
  int  i, status;

  // read back reached value command
  strcpy(cmd, "M+0\r");

  if (MIRROR_DEBUG)
    printf("\n>>gcp: cmd = %s", cmd);

  i=0;
  do { // loop
    BD_PUTS(cmd);
    status = BD_GETS(str, MIRROR_MAX_STR*sizeof(char), "\r", info->mirror_dd_settings.time_out);
    i++;
    if (strstr(str, "HS") || strstr(str, "ES")) { // mirror controller glitch
      i--;
      status = 0;
    }
    ss_sleep(MIRROR_WAIT);
  } while (i<MIRROR_MAX_TRY && !status);

  if (MIRROR_DEBUG)
    printf("\n>>gcp: str = %s", str);

  return i;
}

/*----------------------------------------------------------------------------------*/
INT mirror_move_to_position(MIRROR_INFO *info, char *cmd, char *str, int max_try)
{
  int i, status;

  i=0;
  do { // till new position reached
    BD_PUTS(cmd);
    status = BD_GETS(str, MIRROR_MAX_STR*sizeof(char), "\r", info->mirror_dd_settings.time_out);
    i++;
    ss_sleep(MIRROR_WAIT);
  } while (i<max_try && !strstr(str, "HS") && !strstr(str, "ES"));

  return i;
}

/*----------------------------------------------------------------------------------*/
BOOL reset_mirror_position(MIRROR_INFO *info)
{
  char  cmd[MIRROR_MAX_STR], str[MIRROR_MAX_STR];
  BOOL  done;
  int   i, j, pos, step;

  cm_msg(MINFO,"mirror_set","INFO: reset process start. This is going to take some time!");

  // drive anticlock wise (negative angles) up to the limit switch
  done = FALSE;
  strcpy(cmd, "M-999\r");
  j = 0;
  do { // till end switch reached
    if (MIRROR_DEBUG)
      printf("\n>>mirror reset: cmd = %s", cmd);

    i=mirror_move_to_position(info, cmd, str, 5*MIRROR_MAX_TRY);

    if (MIRROR_DEBUG)
      printf("\n>>mirror reset: i=%d, str=%s", i, str);

    if (i==5*MIRROR_MAX_TRY) // didn't get anything at all!!!
      return FALSE;

    if (strstr(str, "ES")) // end switch reached
      done = TRUE;

    j++;
  } while(!done && j<5);
  cm_msg(MINFO,"mirror_set","INFO: reset process: End switch reached. Start to move back.");

  // read the current position
  mirror_get_current_position(info, str);
  // decode str
  pos = atoi(str);
  // calculate angle to move (in 0.01 units)
  step = abs(100*(info->mirror_dd_settings.adc_45-pos)*5/512);

  // first in steps of 999
  for (j=0; j<step/999; j++) {
    strcpy(cmd, "M+999\r");
    if (MIRROR_DEBUG)
      printf("\n>>mirror reset: cmd = %s", cmd);
    i=mirror_move_to_position(info, cmd, str, 5*MIRROR_MAX_TRY);

    if (MIRROR_DEBUG)
      printf("\n>>mirror reset: i=%d, str=%s", i, str);

    if (i==5*MIRROR_MAX_TRY) // didn't get anything at all!!!
      return FALSE;
  }

  // final step
  step = step % 999; // remaining angle to move
  sprintf(cmd, "M+%d\r", step);
  if (MIRROR_DEBUG)
    printf("\n>>mirror reset: cmd = %s", cmd);

  i=mirror_move_to_position(info, cmd, str, 5*MIRROR_MAX_TRY);

  if (MIRROR_DEBUG)
    printf("\n>>mirror reset: i=%d, str=%s", i, str);

  if (i==5*MIRROR_MAX_TRY) // didn't get anything at all!!!
    return FALSE;

  // check position
  if (mirror_get_current_position(info, str) == MIRROR_MAX_TRY) {
    return FALSE;
  }

  cm_msg(MINFO,"mirror_set","INFO: reset process completed.");

  if (MIRROR_DEBUG)
    printf("\n>>mirror reset: str=%d, adc_45=%d", atoi(str), info->mirror_dd_settings.adc_45);

  if (abs(atoi(str) - info->mirror_dd_settings.adc_45) > 10)
    return FALSE;

  return TRUE;
}

/*----------------------------------------------------------------------------------*/

BOOL mirror_position_reached(MIRROR_INFO *info, char *cmd, float value)
{
  int  i=0;
  char str[MIRROR_MAX_STR];

  if (MIRROR_DEBUG)
    printf("\n\n\n\n\n>>mpr: cmd = %s", cmd);

  i=mirror_move_to_position(info, cmd, str, 5*MIRROR_MAX_TRY);

  if (MIRROR_DEBUG)
    printf("\n>>mpr: i=%d, str=%s", i, str);

  if (i==5*MIRROR_MAX_TRY)
    return FALSE;

  // wait a bit so that the controller is back
  ss_sleep(7000);

  // read back reached value
  if (mirror_get_current_position(info, str) == MIRROR_MAX_TRY)
    return FALSE;

  // check the value
  if (fabs(value-(atof(str)-2048.f)*5.f/512.f)>0.1)
    return FALSE;

  return TRUE;
}

/*----------------------------------------------------------------------------------*/
INT mirror_set(MIRROR_INFO *info, INT channel, float value)
{
  int   status, i, j;
  float measured;
  char  cmd[MIRROR_MAX_STR], str[MIRROR_MAX_STR];
  float dummy;

  if (channel == 0) { // reset channel
    if (value != 0) { // reset mirror position
      // set demand value
      dummy = (info->mirror_dd_settings.adc_45-2048.f)*5.f/512.f;
      db_set_data_index(info->hDB, info->hKeyDemand, &dummy, sizeof(float), channel+1, TID_FLOAT);
      if (!reset_mirror_position(info)) {
        cm_msg(MINFO, "mirror_set", "ERROR: Mirror reset couldn't be performed properly.");
      }
      // set reset value to zero again
      dummy = 0.f;
      db_set_data_index(info->hDB, info->hKeyDemand, &dummy, sizeof(float), channel, TID_FLOAT);
    }
    return FE_SUCCESS;
  }

  if (value > 15.f) {   // check if value is =< 15
    value = 0.f;
    status = db_set_data_index(info->hDB, info->hKeyDemand, &value, sizeof(float), channel, TID_FLOAT);
    cm_msg(MINFO,"mirror_set","ERROR: MIRROR out of range +-15. Set it to 0.");
  }
  if (value < -15.f) { // check if value is >= -15
    value = 0.f;
    status = db_set_data_index(info->hDB, info->hKeyDemand, &value, sizeof(float), channel, TID_FLOAT);
    cm_msg(MINFO,"mirror_set","ERROR: MIRROR out of range +-15. Set it to 0.");
  }

  // get current position
  mirror_get_current_position(info, str);

  measured = (atof(str)-2048.f)*5.f/512.f;

  if (fabs(measured-value)<0.1) { // do nothing
    return FE_SUCCESS;
  }

  i = (100*fabs(measured-value) / 999);

  if (measured - value > 0) { // current position > demanded position
    // loop down in steps of 999 if necessary
    strcpy(cmd, "M-999\r"); // move a defined amount backwards
    for (j=0; j<i+1; j++) {
      cm_msg(MINFO,"mirror_set","INFO: moving mirror, this is going to take a while.");
      mirror_position_reached(info, cmd, value);
    }
    // final step
    cm_msg(MINFO,"mirror_set","INFO: moving mirror, this is going to take a while.");
    // get current position
    mirror_get_current_position(info, str);
    measured = (atof(str)-2048.f)*5.f/512.f;
    if (value-measured > 0) { // end switch has never been reached
      sprintf(cmd, "M+%d\r", abs(100*(value-measured))); // move back to the correct position
      if (!mirror_position_reached(info, cmd, value)) {
        cm_msg(MINFO,"mirror_set","ERROR: haven't reached demand value.");
        return FE_ERR_HW;
      }
    } else { // end switch has been reached
      strcpy(cmd, "M-999\r"); // move a defined amount backwards
      mirror_position_reached(info, cmd, value);
      sprintf(cmd, "M+%d\r", abs(100*(value-measured))); // move back to the correct position
      if (!mirror_position_reached(info, cmd, value)) {
        cm_msg(MINFO,"mirror_set","ERROR: haven't reached demand value.");
        return FE_ERR_HW;
      }
    }
  } else { // current position < demanded position
    // loop up in steps of 999 if necessary
    strcpy(cmd, "M+999\r");
    for (j=0; j<i; j++) {
      cm_msg(MINFO,"mirror_set","INFO: moving mirror, this is going to take a while.");
      mirror_position_reached(info, cmd, value);
    }
    // final step
    cm_msg(MINFO,"mirror_set","INFO: moving mirror, this is going to take a while.");
    // read back current value
    mirror_get_current_position(info, str);
    measured = (atof(str)-2048.f)*5.f/512.f;
    sprintf(cmd, "M+%d\r", abs(100*(value-measured)));
    if (!mirror_position_reached(info, cmd, value)) {
      cm_msg(MINFO,"mirror_set","ERROR: haven't reached demand value.");
      return FE_ERR_HW;
    }
  }
  cm_msg(MINFO,"mirror_set","INFO: mirror at new position.");

  return FE_SUCCESS;
}

/*----------------------------------------------------------------------------------*/

INT mirror_get(MIRROR_INFO *info, INT channel, float *pvalue)
{
  char  str[MIRROR_MAX_STR];
  INT   i, status;
  DWORD nowtime, difftime;

  ss_sleep(10); // sleep 10ms to keep CPU load small during run active

  if (channel == 0) { // reset
    *pvalue = 0;
    return FE_SUCCESS;
  }

  if (!info->bd_connected) { // lost connection
    *pvalue = info->last_value;

    if (info->reconnection_failures > 5) {
      if (info->reconnection_failures == 6) {
        cm_msg(MERROR, "mirror_get", "too many reconnection failures, bailing out :-(");
        info->reconnection_failures++;
      }
      return FE_SUCCESS;
    }

    // try to disconnect and reconnect the bus driver
    if ((ss_time()-info->last_reconnect > 10) && info->bd_connected) { // disconnect bus driver
      info->bd(CMD_EXIT, info->bd_info);
      cm_msg(MINFO, "mirror_get", "mirror_get: try to disconnect and reconnect the bus driver");
      info->last_reconnect = ss_time();
      info->bd_connected = 0;
      if (info->mirror_dd_settings.ets_in_use)
        ets_logout(info->bd_info, MIRROR_ETS_LOGOUT_SLEEP, 0);
    }

    // try to reconnect after a timeout of 10 sec
    if ((ss_time()-info->last_reconnect > 10) && !info->bd_connected) {
      cm_msg(MINFO, "mirror_get", "mirror_get: reconnection trial ...");
      status = info->bd(CMD_INIT, info->hkey, &info->bd_info);
      if (status != FE_SUCCESS) {
        cm_msg(MINFO, "mirror_get", "mirror_get: reconnection attempted failed");
        info->reconnection_failures++;
        return FE_ERR_HW;
      } else {
        info->bd_connected = 1; // bus driver is connected again
        info->reconnection_failures = 0;
        info->last_reconnect = ss_time();
        cm_msg(MINFO, "mirror_get", "mirror_get: successfully reconnected");
      }
    }
  }

  nowtime  = ss_time();
  difftime = nowtime - info->lasterrtime;
  if ( difftime > MIRROR_DELTA_TIME_ERROR ) {
    info->errorcount  = 0;
    info->lasterrtime = nowtime;
  }

  if ( info->startup_error == 1 ) {   // error during CMD_INIT, return -2
    *pvalue = (float) -2;
    ss_sleep(10); // to keep CPU load low when Run active
    return FE_SUCCESS;
  }

  strcpy(str, ""); // initialize str
  i = mirror_get_current_position(info, str);

  if (i == MIRROR_MAX_TRY) { // lost connection
    info->bd_connected = 0;
    *pvalue = -111; // haven't gotten anything
    return FE_SUCCESS;
  }

  if (MIRROR_DEBUG) {
    histo[i-1]++;
    printf("\n>> ");
    for (i=0; i<MIRROR_MAX_TRY; i++)
      printf("%5d", i+1);
    printf("\n>> ");
    for (i=0; i<MIRROR_MAX_TRY; i++)
      printf("%5d", histo[i]);
  }

  // decode return string
  if (!strstr(str,"HS") && !strstr(str,"ES")) { // stepper motor is not moving, i.e. a value is returned
    *pvalue = (atof(str) - 2048)*5.f/512.f; // ADC Range 0..4096, Mirror Angle Range +- 20
    info->last_value = *pvalue; // keep last valid value
    // write value also back into /Info/Mirror/Angle horizontal
    db_set_data_index(info->hDB, info->hKeyInfo, pvalue, sizeof(float), 0, TID_FLOAT);
  } else {
    *pvalue = -222; // stepper motor moving
  }

  ss_sleep(1000); // sleep for a second that the mirror controller is not too stressed!!

  return FE_SUCCESS;
}

/*----------------------------------------------------------------------------------*/
INT mirror_get_label(MIRROR_INFO *info, INT channel, char *name)
{
  strcpy(name, info->mirror_dd_settings.names[channel]);
  return FE_SUCCESS;
}

/*---- device driver entry point ---------------------------------------------------*/
INT mirror(INT cmd, ...)
{
va_list argptr;
HNDLE   hKey;
INT     channel, status;
DWORD   flags;
void    *info, *bd;
float   value, *pvalue;
char    *name;

 va_start(argptr, cmd);
 status = FE_SUCCESS;

 switch (cmd) {
    case CMD_INIT:
      hKey    = va_arg(argptr, HNDLE);
      info    = va_arg(argptr, void *);
      channel = va_arg(argptr, INT);
      flags   = va_arg(argptr, DWORD);
      bd      = va_arg(argptr, void *);
      status  = mirror_init(hKey, info, channel, bd);
      break;

    case CMD_EXIT:
      info    = va_arg(argptr, void *);
      status  = mirror_exit(info);
      break;

    case CMD_GET:
      info    = va_arg(argptr, void *);
      channel = va_arg(argptr, INT);
      pvalue  = va_arg(argptr, float *);
      status  = mirror_get(info, channel, pvalue);
      break;

    case CMD_SET:
      info    = va_arg(argptr, void *);
      channel = va_arg(argptr, INT);
      value   = (float) va_arg(argptr, double);
      status  = mirror_set(info, channel, value);
      break;

    case CMD_GET_LABEL:
      info    = va_arg(argptr, void *);
      channel = va_arg(argptr, INT);
      name    = va_arg(argptr, char *);
      status  = mirror_get_label(info, channel, name);
      break;

    default:
      break;
  }
  va_end(argptr);
  return status;
}
/*------------------------------------------------------------------*/