/*
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>
# define Delay(x) usleep((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)
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)
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( "- 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
]);
}
}
int usmc_Set_Parameters(int Dev, int mode)
{
USMC_Mode umode;
if( USMC_GetParameters(Dev, ¶meters[Dev]) )
return TRUE;
switch (mode) {
case 1:
// Linear stage
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 0:
// Rotation stage
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, ¶meters[Dev] ) )
return TRUE;
if( USMC_SaveParametersToFlash( Dev ) )
return TRUE;
if (mode){
if ( USMC_GetMode ( Dev, &umode ) ) return FALSE;
umode.TrSwap = 0;
if ( USMC_SetMode ( Dev, &umode ) ) return FALSE;
printf ( "Now, TrSwap is %d \n", umode.
TrSwap );
}
//system("cls");
print_parameters( ¶meters[Dev] );
printf("\nThese Parameters are Saved to Flash");
return FALSE;
}
int usmc_Init(int node, int mode){
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] );
USMC_Start(node,pos,&usmc_speed, &parstart[node]);
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);
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,¶meters[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_RelMove(int node, int pos){
return usmc_Move(node,pos, TRUE);
}
int usmc_MoveTo(int node, int pos){
return usmc_Move(node,pos, FALSE);
}
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) dir = -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);
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
," -n node -i mode .. initialize node\n");
fprintf(stderr
," -n node -h .. 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 .. move node for +1000\n");
fprintf(stderr
," -n node -d .. move node for -1000\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;
}
#ifndef _WINDOWS
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:udn:m:s:v:g:h: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,¶meters[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':
for (i=0;i<5;i++){
int xpos=i*1000+10000;
usmc_MoveTo (1, xpos);
for (j=0;j<5;j++){
int ypos=j*1000+10000;
usmc_MoveTo (2, ypos);
for (k=0;k<50;k++){
int zpos=k*1000+10000;
usmc_MoveTo (3, zpos);
printf("x=%d y=%d z=%d\n",xpos
,ypos
,zpos
);
}
}
}
break;
case 'n':
break;
case 's':
usmc_speed
= atoi(optarg
);
break;
case 'p':
usmc_PowerOnOff
(node
,atoi(optarg
));
break;
case 'm':
usmc_MoveTo
(node
, atoi(optarg
));
printf("usmc_MoveTo node=%d pos=%d \n",node
,atoi(optarg
));
break;
case 'v':
break;
/*
case 'f':
MIKRO_Set (node,optarg,value);
printf("MIKRO_Set node %d cmd=%s val=%d\n",node,optarg, value);
break;
case 'g':
MIKRO_Get (node,optarg,&i);
printf("MIKRO_Get node %d cmd=%s val=%d\n",node,optarg, i);
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 mode=%d\n",node
, atoi(optarg
));
usmc_ReferenceMove
(node
, atoi(optarg
));
break;
case 'u':
usmc_RelMove(node, 1000);
break;
case 'd':
usmc_RelMove(node, -1000);
break;
default: /* '?' */
help();
break;
}
}
if (argc==1) help();
USMC_Close ();
#ifdef _WINDOWS
#endif
return 0;
}
#endif