Subversion Repositories f9daq

/*
8mru
  sudo ./usmc_ctrl -n 0 -p 1 -s 1000 -h 0 -p 0
  sudo ./usmc_ctrl -n 0 -p 1 -s 1000 -m +1210 -p 0
  sudo ./usmc_ctrl -n 0 -p 1 -s 1000 -m -1210 -p 0
   

usmc_ctrl -a
usmc_ctrl -n 0 -i
usmc_ctrl -n 0 -p 1 -s 5000 -m 10000 - p 0
usmc_ctrl -n 0 -p 1 -h -p 0


*/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#ifndef _WINDOWS
#include "libusmc.h"      
#include <getopt.h>
#include <time.h>

void sleep_us(unsigned long microseconds)
{
    struct timespec ts;
    ts.tv_sec = microseconds / 1000000;             // whole seconds
    ts.tv_nsec = (microseconds % 1000000) * 1000;    // remainder, in nanoseconds
    nanosleep(&ts, NULL);
}

#define Delay(x) sleep_us((int)(x*1000000))
#else
#include "USMCDLL.H"
#include <utility.h>
#include <ansi_c.h>  



#define ERR(s, c)       if(opterr) fprintf(stderr,"%s %c\n",s,c);  


int     opterr = 1;
int     optind = 1;
int     optopt;
char    *optarg;

int     getopt(int argc, char **argv, char *opts){
        static int sp = 1;
        int c;
        char *cp;

        if(sp == 1)
                if(optind >= argc ||
                   argv[optind][0] != '-' || argv[optind][1] == '\0')
                        return(EOF);
                else if(strcmp(argv[optind], "--") == NULL) {
                        optind++;
                        return(EOF);
                }
        optopt = c = argv[optind][sp];
        if(c == ':' || (cp=strchr(opts, c)) == NULL) {
                ERR(": illegal option -- ", c);
                if(argv[optind][++sp] == '\0') {
                        optind++;
                        sp = 1;
                }
                return('?');
        }
        if(*++cp == ':') {
                if(argv[optind][sp+1] != '\0')
                        optarg = &argv[optind++][sp+1];
                else if(++optind >= argc) {
                        ERR(": option requires an argument -- ", c);
                        sp = 1;
                        return('?');
                } else
                        optarg = argv[optind++];
                sp = 1;
        } else {
                if(argv[optind][++sp] == '\0') {
                        sp = 1;
                        optind++;
                }
                optarg = NULL;
        }
        return(c);
}

#endif

#define MAXNODES 4
USMC_Devices devices;
USMC_StartParameters parstart[MAXNODES];
USMC_Parameters      parameters[MAXNODES];
USMC_State parstate[MAXNODES];
USMC_EncoderState parencstate[MAXNODES];
#define ERRLEN 128
char errstr[ERRLEN];

// Function that prints information about device parameters to console
void print_parameters(USMC_Parameters *Parameters)
{
        printf( "The parameters are:\n" );
        printf( "Full acceleration time - %.0f ms\n", (double) Parameters->AccelT );
        printf( "Full deceleration time - %.0f ms\n", (double) Parameters->DecelT );
        printf( "Power reduction timeout - %.0f ms\n", (double) Parameters->PTimeout );
        printf( "Button speedup timeout 1 - %.0f ms\n", (double) Parameters->BTimeout1 );
        printf( "Button speed after timeout 1 - %.2f steps/s\n", (double) Parameters->BTO1P );
        printf( "Button speedup timeout 2 - %.0f ms\n", (double) Parameters->BTimeout2 );
        printf( "Button speed after timeout 2 - %.2f steps/s\n", (double) Parameters->BTO2P );
        printf( "Button speedup timeout 3 - %.0f ms\n", (double) Parameters->BTimeout3 );
        printf( "Button speed after timeout 3 - %.2f steps/s\n", (double) Parameters->BTO3P );
        printf( "Button speedup timeout 4 - %.0f ms\n", (double) Parameters->BTimeout4 );
        printf( "Button speed after timeout 4 - %.2f steps/s\n", (double) Parameters->BTO4P );
        printf( "Button reset timeout - %.0f ms\n", (double) Parameters->BTimeoutR );
        printf( "Button reset operation speed - %.2f steps/s\n", (double) Parameters->MinP );
        printf( "Backlash operation distance - %d steps\n", (int)Parameters->MaxLoft );
        printf( "Revolution distance - %d steps\n", (int)Parameters->RTDelta );
        printf( "Minimal revolution distance error - %d steps\n", (int)Parameters->RTMinError );
        printf( "Power off temperature - %.2f\xf8\x43\n", (double)Parameters->MaxTemp );
        printf( "Duration of the output synchronization pulse - ");
        if(Parameters->SynOUTP == 0)
                printf( "minimal\n");
        else
                printf( "%.1f * [Tact Period]\n", Parameters->SynOUTP - 0.5);
        printf( "Speed of the last phase of the backlash operation - ");
        if(Parameters->LoftPeriod == 0.0f)
                printf( "normal\n" );
        else
                printf( "%.2f steps/s\n", (double)Parameters->LoftPeriod );
        printf( "<Angular Encoder Step> Equals <Angular Step Motor Step>/<%.2f>\n", Parameters->EncMult);

}

// Function that prints information about device start parameters to console
void print_start_params ( const USMC_StartParameters sp ){
        printf ( "* Steps Divisor - %d\n", sp.SDivisor );

        if ( sp.SDivisor == 1 )
                printf ( "* Slow start/stop mode - %s\n", sp.SlStart ? "Enabled" : "Disabled" );
        else if ( sp.LoftEn ) {
                printf ( "* Automatic backlash operation - Enabled\n" );
                printf ( "* Automatic backlash operation direction - %s\n", sp.DefDir ? "CCW" : "CW" );
                printf ( "* Force automatic backlash operation - %s\n", sp.ForceLoft ? "TRUE" : "FALSE" );
        } else {
                printf ( "* Automatic backlash operation - Disabled\n" );
        }
        if ( sp.WSyncIN )
                printf ( "* Controller will wait for input synchronization signal to start\n" );
        else
                printf ( "* Input synchronization signal ignored \n" );

        printf ( "* Output synchronization counter will %sbe reset\n", sp.SyncOUTR ? "" : "not " );
}

// Function that prints information about device state to console
void print_state(USMC_State *State)
{
        printf( " The state is:\n" );
        printf( "- Current Position in microsteps - %d\n", State->CurPos );
        printf( "- Temperature - %.2f\xf8\x43\n", State->Temp );
        printf( "- Step Divisor - %d\n", State->SDivisor);
        printf( "- Loft State - %s\n", State->Loft?"Indefinite":"Fixed" );
        printf( "- Power - %s\n", State->Power?(State->FullPower?"Full":"Half"):"Off" );
        if(State->RUN)
                printf( "- Step Motor is Running in %s Direction %s\n",
                                State->CW_CCW?"CCW":"CW", ((State->SDivisor==1) && State->FullSpeed)?"at Full Speed":"" );
        else
                printf( "- Step Motor is Not Running\n" );
        printf( "- Device %s\n", State->AReset?"is After Reset":"Position Already Set" );
        printf( "- Input Synchronization Logical Pin State - %s\n", State->SyncIN?"TRUE":"FALSE" );
        printf( "- Output Synchronization Logical Pin State - %s\n", State->SyncOUT?"TRUE":"FALSE" );
        printf( "- Rotary Transducer Logical Pin State - %s\n", State->RotTr?"TRUE":"FALSE" );
        printf( "- Rotary Transducer Error Flag - %s\n", State->RotTrErr?"Error":"Clear" );
        printf( "- Emergency Disable Button - %s\n", State->EmReset?"Pushed":"Unpushed" );
        printf( "- Trailer 1 Press State - %s\n", State->Trailer1?"Pushed":"Unpushed" );
        printf( "- Trailer 2 Press State - %s\n", State->Trailer2?"Pushed":"Unpushed" );
        if( State->Voltage == 0.0f )
                printf( "- Input Voltage - Low\n");
        else
                printf( "- Input Voltage - %.1fV\n", State->Voltage);

}

void print_enc_state ( USMC_EncoderState enc_state )
{
        printf ( "# The encoder state is:\n" );
        printf ( "#    Current Position in \"Half of Encoder Step\"s - %d\n", enc_state.ECurPos );
        printf ( "#    Encoder Position in \"Half of Encoder Step\"s - %d\n", enc_state.EncoderPos );
 
}


// Function that prints information about connected devices to console
void printdevices(USMC_Devices DVS)
{
    DWORD i;
        for( i = 0; i < DVS.NOD; i++)
        {
                printf("Device - %d,\tSerial Number - %.16s,\tVersion - %.4s\n",i,DVS.Serial[i],DVS.Version[i]);
        }
}



void usmc_SwapSwitches ( int cur_dev, int swap ){
       
        USMC_Mode mode;
        if ( USMC_GetMode ( cur_dev, &mode ) ) return;
        mode.TrSwap = swap;
        if ( USMC_SetMode ( cur_dev, &mode ) ) return;
        printf ( "Now, Switches of %d are %s swapped.\n", cur_dev, mode.TrSwap ? "" : "not" );
}

void usmc_EnableSwitches ( int cur_dev, int enable ){
       
        USMC_Mode mode;
        if ( USMC_GetMode ( cur_dev, &mode ) ) return;
        mode.Tr1En = enable;
        mode.Tr2En = enable;
        if ( USMC_SetMode ( cur_dev, &mode ) ) return;
        printf ( "Now, Switches of %d are %s.\n", cur_dev, mode.Tr1En ? "enabled" : "disabled" );
}


int usmc_Set_Parameters(int Dev, int mode)
{
   
        if( USMC_GetParameters(Dev, &parameters[Dev]) )
                return TRUE;

switch (mode) {
case 0:
// Rotation stage
       
        usmc_SwapSwitches(Dev,1);
        parameters[Dev].MaxTemp = 70.0f;
        parameters[Dev].AccelT = 200.0f;
        parameters[Dev].DecelT = 200.0f;
        parameters[Dev].BTimeout1 = 500.0f;
        parameters[Dev].BTimeout2 = 500.0f;
        parameters[Dev].BTimeout3 = 500.0f;
        parameters[Dev].BTimeout4 = 500.0f;
        parameters[Dev].BTO1P = 10.0f;
        parameters[Dev].BTO2P = 20.0f;
        parameters[Dev].BTO3P = 30.0f;
        parameters[Dev].BTO4P = 60.0f;
        parameters[Dev].MinP = 60.0f;
        parameters[Dev].BTimeoutR = 500.0f;
        parameters[Dev].LoftPeriod = 50.0f;
        parameters[Dev].RTDelta = 20;
        parameters[Dev].RTMinError = 15;
        parameters[Dev].EncMult = 2.5f;
        parameters[Dev].MaxLoft = 32;
        parameters[Dev].PTimeout = 100.0f;
        parameters[Dev].SynOUTP = 1;
        break;
case 1:
// Linear stage
        usmc_SwapSwitches(Dev,0);
        parameters[Dev].MaxTemp = 70.0f;
        parameters[Dev].AccelT = 200.0f;
        parameters[Dev].DecelT = 200.0f;
        parameters[Dev].BTimeout1 = 500.0f;
        parameters[Dev].BTimeout2 = 500.0f;
        parameters[Dev].BTimeout3 = 500.0f;
        parameters[Dev].BTimeout4 = 500.0f;
        parameters[Dev].BTO1P = 100.0f;
        parameters[Dev].BTO2P = 200.0f;
        parameters[Dev].BTO3P = 300.0f;
        parameters[Dev].BTO4P = 600.0f;
        parameters[Dev].MinP = 500.0f;
        parameters[Dev].BTimeoutR = 500.0f;
        parameters[Dev].LoftPeriod = 500.0f;
        parameters[Dev].RTDelta = 200;
        parameters[Dev].RTMinError = 15;
        parameters[Dev].EncMult = 2.5f;
        parameters[Dev].MaxLoft = 32;
        parameters[Dev].PTimeout = 100.0f;
        parameters[Dev].SynOUTP = 1;
        break;
        }

        //

        if( USMC_SetParameters( Dev, &parameters[Dev] ) )
                return TRUE;

        if( USMC_SaveParametersToFlash( Dev ) )
                return TRUE;

        //system("cls");
        print_parameters( &parameters[Dev] );
        printf("\nThese Parameters are Saved to Flash");

        return FALSE;
}

int usmc_Init(int node, int mode){
    printf("\nNode %d\n",node);
        USMC_GetStartParameters(node,&parstart[node]);
    USMC_GetLastErr(errstr,ERRLEN);  if (strlen(errstr)) printf("i1 %s\n",errstr);
   
        USMC_GetState(node,&parstate[node]);
    USMC_GetLastErr(errstr,ERRLEN);  if (strlen(errstr)) printf("i2 %s\n",errstr);

        USMC_GetEncoderState(node,&parencstate[node]);
    USMC_GetLastErr(errstr,ERRLEN);  if (strlen(errstr)) printf("i3 %s\n",errstr);

        print_state(&parstate[node]);
    print_enc_state ( parencstate[node]);
        print_start_params (parstart[node] );
        usmc_Set_Parameters(node, mode);
        return 0;
}

float usmc_speed=10000;
int usmc_Move(int node, int pos,  BOOL absrel){
  time_t tcur=0, tprev=-1;
  USMC_GetStartParameters(node,&parstart[node]);
  USMC_GetLastErr(errstr,ERRLEN);  if (strlen(errstr)) printf("x1 %s\n",errstr);
 
  parstart[node].WSyncIN=absrel;
  parstart[node].SlStart=1;
  parstart[node].LoftEn=0;  
  print_start_params ( parstart[node] );
  if (absrel) printf("usmc_Move relative for %d\n",pos);
  else printf("usmc_Move absolute %d\n",pos);
  USMC_Start(node,pos,&usmc_speed, &parstart[node]);
  Delay(0.1);
  USMC_GetLastErr(errstr,ERRLEN);  if (strlen(errstr)) printf("x2 %s speed %f\n",errstr,usmc_speed);

  do {
    USMC_GetState(node,&parstate[node]);
    USMC_GetLastErr(errstr,ERRLEN); // if (strlen(errstr)) printf("x3 %s\n",errstr);
        tcur= time(NULL);
        if (tcur != tprev){
    printf("%d CurPos %d L%d R%d Rot%d %s %s\n",node,
                        parstate[node].CurPos,parstate[node].Trailer1,parstate[node].Trailer2, parstate[node].RotTr,
                        parstate[node].RUN?"Run":"Stopped",parstate[node].Power?"On":"Off" );
    tprev= tcur;
    }
   
  } while ( parstate[node].RUN == 1 );
  USMC_GetState(node,&parstate[node]);
  printf("***** %d CurPos %d L%d R%d %s %s\n",node,
                        parstate[node].CurPos,parstate[node].Trailer1,parstate[node].Trailer2,
                        parstate[node].RUN?"Run":"Stopped",parstate[node].Power?"On":"Off" );
  USMC_GetParameters(node,&parameters[node]);  
                     USMC_GetEncoderState(node,&parencstate[node]);
                     printf("node %d ECurPos 0x%0x %f EncoderPos =0x%04x %f\n",node,
                     parencstate[node].ECurPos, (((parencstate[node].ECurPos)>>5)&0x8FFFFF)/parameters[node].EncMult,
                     parencstate[node].EncoderPos,((parencstate[node].EncoderPos >> 5)&0x8FFFFF)/parameters[node].EncMult);                      
 
 
        return 0;
}



void usmc_PowerOnOff ( int cur_dev, int smpower ){
       
        USMC_Mode mode;
        if ( USMC_GetMode ( cur_dev, &mode ) ) return;
        mode.ResetD = !smpower;
        if ( USMC_SetMode ( cur_dev, &mode ) ) return;
        printf ( "Now, Power of the node %d is %s\n", cur_dev, mode.ResetD ? "Off" : "On" );
}

int usmc_MoveTo(int node, int pos){
  return usmc_Move(node,pos, FALSE);
}


int usmc_RelMove(int node, int pos){
  //return usmc_Move(node,pos, TRUE);// does not work - don't know why

  USMC_GetState(node,&parstate[node]);
  return usmc_MoveTo( node, pos + parstate[node].CurPos );
}

int Revert_Start_Position_to_0(int node)
{
    USMC_Parameters Prms;
        // Initialize structures (in case this function runs first)
        if( USMC_GetParameters(node, &Prms) )
                return TRUE;
        Prms.StartPos = 0;
        if( USMC_SetParameters( node, &Prms ) )
                return TRUE;
        // Then of Course You Need to SaveToFlash
        if( USMC_SaveParametersToFlash( node ) )
                return TRUE;
        //system("cls");
        printf("\nStart Position is Reset to 0\n");
        printf("\nPress any key to exit");
        return FALSE;
}

int usmc_Reset(int node, int mode){

usmc_Init(node,mode);
USMC_SetCurrentPosition(node, 0);
Revert_Start_Position_to_0(node);
return 0;
}

int usmc_ReferenceMove(int node, int mode){

int pos = 1000000;
int dir=1;
if (!mode) usmc_SwapSwitches(node,1);
else usmc_SwapSwitches(node,0);

usmc_EnableSwitches(node,1);

USMC_GetState(node,&parstate[node]);
usmc_MoveTo(node, parstate[node].CurPos -2 *pos * dir);
USMC_GetState(node,&parstate[node]);
pos = 5000;
if (!mode) pos=500;
usmc_MoveTo(node, parstate[node].CurPos + pos * dir);
USMC_GetState(node,&parstate[node]);
usmc_speed /=5;
usmc_MoveTo(node, parstate[node].CurPos -2 *pos * dir);
USMC_GetState(node,&parstate[node]);
pos = 2000;
if (!mode) pos=200;
usmc_MoveTo(node, parstate[node].CurPos + pos *dir);
USMC_GetState(node,&parstate[node]);
usmc_speed /= 10;
usmc_MoveTo(node, parstate[node].CurPos -2 *pos * dir);
USMC_GetState(node,&parstate[node]);
USMC_SetCurrentPosition(node, 0);
Revert_Start_Position_to_0(node);

usmc_EnableSwitches(node,0);
return 0;
}

int help(){
fprintf(stderr, "Usage: mikro [-i node][-n node] [-u up] [-d down] [-r node] [-h node] [-a] [-g] [-m pos]\n");
                   fprintf(stderr,"      Options:\n");
                   fprintf(stderr," -n node -i mode  .. initialize node\n");
                   fprintf(stderr," -n node -h <endswitch_direction>  .. homing procedure for node\n");
                   fprintf(stderr," -n node -p 1   .. power on\n");
                   fprintf(stderr," -n node -p 0   .. power off\n");
                   fprintf(stderr," -n node -r mode  .. reset node\n");
                   fprintf(stderr," -n node -u <steps>  .. move node for <steps>\n");
                   fprintf(stderr," -a   .. current status of the nodes\n");
                   fprintf(stderr," -n node -v value -f cmd   .. set value of the cmd on the node\n");
                   fprintf(stderr," -n node -g cmd   .. get value of the cmd on the node\n");          
                   fprintf(stderr," -n node -m position  -s speed .. move node to position at speed (default 10000)\n");
                   fprintf(stderr," -l delaysec  .. loop test with the delay delaysec\n");          
return 0;
}

int main (int argc, char ** argv){
  int i,j,k;
  int node=0,opt,value=0;
  int EOldPos=-1;      
  if ( USMC_Init ( &devices ) ) abort();


  while ((opt = getopt(argc, argv, "i:av:f:l:u:n:m:s:v:gh:r:p:")) != -1) {
               switch (opt) {
               case 'i':
                   usmc_Init (node, atoi(optarg));
                   
                   break;
               case 'a':
                   printdevices(devices);
                   for (i=0;i<devices.NOD;i++){
                     USMC_GetState(i,&parstate[i]);
                     printf("%d CurPos %d L%d R%d %s %s\t",i,
                        parstate[i].CurPos,parstate[i].Trailer1,parstate[i].Trailer2,
                        parstate[i].RUN?"Run":"Stopped",parstate[i].Power?"On":"Off" );
                     USMC_GetParameters(i,&parameters[i]);  
                     USMC_GetEncoderState(i,&parencstate[i]);
                     printf("node %d ECurPos 0x%0x %f EncoderPos =0x%04x %f\n",i,
                     parencstate[i].ECurPos, (((parencstate[i].ECurPos)>>5)&0x8FFFFF)/parameters[i].EncMult,
                     parencstate[i].EncoderPos,((parencstate[i].EncoderPos >> 5)&0x8FFFFF)/parameters[i].EncMult);
                     
                   }
                   break;

               case 'l':
                   printf("usmc_MoveTo Loop\n");
                   for (i=0;i<5;i++){
                     int xpos=i*1000+10000;                  
                     usmc_MoveTo (0, xpos);
                                           for (j=0;j<5;j++){
                                             int ypos=j*1000+10000;                      
                                             usmc_MoveTo (1, ypos);
                         for (k=0;k<50;k++){
                           int zpos=k*1000+10000;                        
                           usmc_MoveTo (2, zpos);
                           printf("x=%d y=%d z=%d\n",xpos,ypos,zpos);
                           Delay(atof(optarg));
                   
                       }
                     }
                   }
                   break;
               case 'n':
                  node = atoi(optarg);
                  break;
               case 's':
                  usmc_speed = atoi(optarg);
                  break;  
               case 'p':
                  usmc_PowerOnOff(node,atoi(optarg));
                  break;
               case 'm':
                   usmc_EnableSwitches(node,1);
                   usmc_MoveTo (node, atoi(optarg));
                   usmc_EnableSwitches(node,0);
                   printf("usmc_MoveTo node=%d pos=%d \n",node,atoi(optarg));
                   
                   break;
             
               case 'v':
                   value=atoi(optarg);
                   break;
                                                                         /*
               case 'f':
                   MIKRO_Set (node,optarg,value);
                   printf("MIKRO_Set node %d  cmd=%s val=%d\n",node,optarg, value);
                   break;
*/
               case 'g':
                     
                     USMC_GetState(node,&parstate[node]);
                     printf("node %d CurPos %d L%d R%d %s %s\n",node,
                        parstate[node].CurPos,parstate[node].Trailer1,parstate[node].Trailer2,
                        parstate[node].RUN?"Run":"Stopped",parstate[node].Power?"On":"Off" );

     
                   break;
               
               
              case 'r':
                      printf("usmc_Reset node=%d mode=%d\n",node, atoi(optarg));
                      usmc_Reset (node, atoi(optarg));
                      break;
                     
               case 'h':
                      printf("usmc_ReferenceMove node=%d endswitch=%d\n",node, atoi(optarg));
                      usmc_ReferenceMove (node, atoi(optarg));
                      break;
                     
               case 'u':
                  usmc_EnableSwitches(node,1);
                  usmc_RelMove(node, atoi(optarg));
                  usmc_EnableSwitches(node,0);
                  break;

               default: /* '?' */
                   help();
                       
                   break;
               }
           }
         if (argc==1) help();

 
         USMC_Close ();
#ifdef _WINDOWS
         getchar();
#endif   
  return 0;
}