Subversion Repositories f9daq

#include        <stdio.h>
#include        <string.h>
#include        <unistd.h>
#include        "libusmc.h"


#define         MENU_EXIT       0
#define         MENU_ERROR      1
#define         MENU_CONTINUE   2

USMC_Devices devices;
DWORD        cur_dev;

char str [105] = { 100 };

// Function that prints information about connected devices to console
void print_devices ( USMC_Devices devices )
{
        DWORD i;

        for ( i = 0 ; i < devices.NOD ; i++ )
                printf ( "Device# %d,\tSerial Number - %.16s,\tVersion - %.4s\n", i + 1, devices.Serial [i], devices.Version [i] );
}

// 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 );
}

// Function that scans start parameters
void scan_start_params ( int * dest_pos, float * speed, USMC_StartParameters * sp )
{
        // Defaults
        *speed = 2000.0f;
        *dest_pos  = 0;
        sp -> SDivisor = 8;

        printf ( "Destination position:" );
        scanf  ( "%d", dest_pos );
        printf ( "\nSpeed (in tacts):" );
        scanf  ( "%f", speed );
        printf ( "\nSteps Divisor:" );
        scanf  ( "%c", &( sp -> SDivisor ) );
        getchar ();
}

// Function that prints information about device start parameters to console
void print_start_params ( int dest_pos, float speed, const USMC_StartParameters sp )
{
        printf ( "Destination position - %d\n", dest_pos );
        printf ( "speed - %.2ftacts/s\n", speed );
        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 parameters to console
void print_params ( USMC_Parameters params )
{
        printf ( "The parameters are:\n" );
        printf ( "Full acceleration time - %.0f ms\n", ( double ) params.AccelT );
        printf ( "Full deceleration time - %.0f ms\n", ( double ) params.DecelT );
        printf ( "Power reduction timeout - %.0f ms\n", ( double ) params.PTimeout );
        printf ( "Button speedup timeout 1 - %.0f ms\n", ( double ) params.BTimeout1 );
        printf ( "Button speed after timeout 1 - %.2f steps/s\n", ( double ) params.BTO1P );
        printf ( "Button speedup timeout 2 - %.0f ms\n", ( double ) params.BTimeout2 );
        printf ( "Button speed after timeout 2 - %.2f steps/s\n", ( double ) params.BTO2P );
        printf ( "Button speedup timeout 3 - %.0f ms\n", ( double ) params.BTimeout3 );
        printf ( "Button speed after timeout 3 - %.2f steps/s\n", ( double ) params.BTO3P );
        printf ( "Button speedup timeout 4 - %.0f ms\n", ( double ) params.BTimeout4 );
        printf ( "Button speed after timeout 4 - %.2f steps/s\n", ( double ) params.BTO4P );
        printf ( "Button reset timeout - %.0f ms\n", (double) params.BTimeoutR );
        printf ( "Button reset operation speed - %.2f steps/s\n", ( double ) params.MinP );
        printf ( "Backlash operation distance - %d steps\n", ( int ) params.MaxLoft );
        printf ( "Revolution distance - %d steps\n", ( int ) params.RTDelta );
        printf ( "Minimal revolution distance error - %d steps\n", ( int ) params.RTMinError );
        printf ( "Power off temperature - %.2f\xf8\x43\n", ( double ) params.MaxTemp );
        printf ( "Duration of the output synchronization pulse - " );

        if ( params.SynOUTP == 0 )
                printf ( "minimal\n");
        else
                printf ( "%.1f * [Tact Period]\n", params.SynOUTP - 0.5 );

        printf ( "speed of the last phase of the backlash operation - " );

        if ( params.LoftPeriod == 0.0f )
                printf ( "normal\n" );
        else
                printf ( "%.2f steps/s\n", ( double ) params.LoftPeriod );

        printf ( "<Angular Encoder Step> Equals <Angular Step Motor Step>/<%.2f>\n", params.EncMult );
}

// Function that prints information about device "mode" parameters to console
void print_mode ( USMC_Mode mode )
{
        printf ( "mode parameters:\n" );
        printf ( "Buttons - ");

        if ( mode.PMode )
                printf ( "Disabled\n" );
        else {
                printf ( "Enabled\nButton 1 TRUE state - %s\n", mode.Butt1T ? "+3/+5 V" : "0 V(GND)" );
                printf ( "Button 2 TRUE state - %s\n", mode.Butt2T ? "+3/+5 V" : "0 V(GND)" );
        }

        printf ( "Current reduction regime - %s\n", mode.PReg ? "Used" : "Not Used" );

        if ( mode.ResetD )
                printf ( "Power - %s\n", mode.EMReset ? "Emerjency Off" : "Off" );
        else
                printf ( "Power - On\n" );

        if ( mode.Tr1En || mode.Tr2En )
                printf ( "Trailers are - %s\n", mode.TrSwap ? "Swapped" : "Direct" );

        printf ( "Trailer 1 - ");

        if ( mode.Tr1En )
                printf ( "Enabled\nTrailer 1 TRUE state - %s\n", mode.Tr1T ? "+3/+5 V" : "0 V(GND)" );
        else
                printf ( "Disabled\n" );

        printf ( "Trailer 2 - " );

        if ( mode.Tr2En )
                printf ( "Enabled\nTrailer 2 TRUE state - %s\n", mode.Tr2T ? "+3/+5 V" : "0 V(GND)" );
        else
                printf ( "Disabled\n" );

        if ( mode.EncoderEn ) {
                printf ( "Encoder - Enabled\n" );
                printf ( "Encoder Position Counter is %s\n", mode.EncoderInv ? "Inverted" : "Direct" );
                printf ( "Rotary Transducer and Input Syncronisation are\n"
                                " Disabled Because of Encoder\n" );
        } else {
                printf ( "Encoder - Disabled\n" );
                printf ( "Rotary Transducer - " );

                if ( mode.RotTeEn ) {
                        printf ( "Enabled\nRotary Transducer TRUE state - %s\n", mode.RotTrT ? "+3/+5 V" : "0 V(GND)" );
                        printf ( "Rotary Transducer Operation - %s\n", mode.RotTrOp ? "Stop on error" : "Check and ignore error" );
                        printf ( "Reset Rotary Transducer Check Positions - %s\n", mode.ResetRT ? "Initiated" : "No, why?" );
                } else {
                        printf ( "Disabled\n" );
                }

                printf ( "Synchronization input mode:\n" );

                if ( mode.SyncINOp )
                        printf ( "Step motor will move one time to the destination position\n" );
                else
                        printf ( "Step motor will move multiple times by [destination position]\n" );
        }

        printf ( "Output Syncronization - " );

        if ( mode.SyncOUTEn ) {
                printf ( "Enabled\nReset Output Synchronization Counter - %s\n", mode.SyncOUTR ? "Initiated" : "No, why?" );
                printf ( "Number of steps after which synchronization output sygnal occures - %u\n", mode.SyncCount );
        } else {
                printf ( "Disabled\n" );
        }

        printf ( "Synchronization Output is " );

        if ( mode.SyncInvert )
                printf ( "INVERTED\n" );
        else
                printf ( "NORMAL\n" );
}

void print_enc_state ( USMC_EncoderState enc_state, USMC_Parameters up )
{
        printf ( "The encoder state is:\n" );
        printf ( "- Current Position in microsteps - %.2f\n", enc_state.ECurPos/up.EncMult );
        printf ( "- Encoder Position in microsteps - %.2f\n\n", enc_state.EncoderPos/up.EncMult );
        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 last error information
void print_error ( void )
{
        char er [256];

        USMC_GetLastErr ( er, 255 );
        er [255] = '\0';
        printf ( "\n%s", er );
}


USMC_State state;
USMC_StartParameters start_params;
USMC_Parameters params;
USMC_Mode mode;
USMC_EncoderState enc_state;

// Reference to Functions For Every Command (defined at the end of the file)
BOOL f1_get_dev_state ( void );
BOOL f2_start ( void );
BOOL f3_stop ( void );
BOOL f4_set_params ( void );
BOOL f5_set_mode ( void );
BOOL f6_set_cur_pos ( void );
BOOL f7_turn_off_and_save_pos ( void );
BOOL fs7_revert_sp_to_0 ( void );
BOOL f8_get_enc_state ( void );
BOOL fp_change_power ( void );

BOOL smpower = FALSE;

int menu ( void )
{
        BOOL err = FALSE;
        char menu_sel;

        printf ( "menu :\n"
                   "1 - Get Device state\n"
                   "2 - START\n"
                   "3 - STOP\n"
                   "4 - Set Parameters (can be changed only in c++ code)\n"
                   "5 - Set mode (can be changed only in c++ code)\n"
                   "6 - Set Current Position (can be changed only in c++ code)\n"
                   "7 - Turn Off and Save Current Position to Flash\n"
                   "SHIFT + 7 - Revert Start Position to 0\n"
                   "8 - Get Encoder state\n"
                   "\n"
                   "p - Turn %s Power"
                   "\n"
                   "9 - Select other device\n"
                   "0 - Exit program\n"
                   "Choose:", smpower ? "Off" : "On" );

        menu_sel = getchar ();

        switch ( menu_sel )
        {
        case '1':
                err = f1_get_dev_state ();
                break;
        case '2':
                err = f2_start ();
                break;
        case '3':
                err = f3_stop ();
                break;
        case '4':
                err = f4_set_params ();
                break;
        case '5':
                err = f5_set_mode ();
                break;
        case '6':
                err = f6_set_cur_pos ();
                break;
        case '7':
                err = f7_turn_off_and_save_pos ();
                break;
        case '&':       // 'SHIFT 7' on my keyboard
                err = fs7_revert_sp_to_0 ();
                break;
        case '8':
                err = f8_get_enc_state ();
                break;
        case 'p':
        case 'P':
                err = fp_change_power ();
                break;

        case '9':
                return  MENU_ERROR;
        case '0':
                printf ( "\n" );
                return  MENU_EXIT;
        default:
                system ( "clear" );
                break;
        }

        if ( err ) {
                print_error ();
                printf ( "\nPerforming Refressh..." );
                getchar ();

                if ( USMC_Init ( &devices ) ) {
                        print_error ();
                        return  MENU_EXIT;
                }

                return  MENU_ERROR;
        }

        return  MENU_CONTINUE;
}

int select_menu ( void )
{
        int  ret;
        do {
                system ( "clear" );
                cur_dev = -1;
                print_devices ( devices );
                printf ( "\n\t0\tExit\n" );
    for ( ret=0;ret<devices.NOD;ret++)  printf ( "\t%d\tDevice # %d\n",ret+1,ret+1 );
                printf ( "\tx\tReInitialize\n" );
                printf ( "\txx\tClose Driver Window and Exit Program\n" );

                printf ( "Select:" );
                scanf  ( "%s", str );
                getchar ();

                if ( strcmp ( str, "x" ) == 0 )
                {
                        //// Perform "Refresh" of Driver
                        if ( USMC_Init ( &devices ) )
                        {
                                print_error ();
                                return  MENU_EXIT;
                        }

                        continue;
                } else if ( strcmp ( str, "xx" ) == 0 ) {
                        //// Close the MicroSMC.exe process
                        if ( USMC_Close () )
                        {
                                print_error ();
                                return  MENU_EXIT;
                        }

                        return  MENU_EXIT;
                }

                sscanf ( str, "%u", &cur_dev );

                if ( cur_dev == 0 )
                        return  MENU_EXIT;
        } while ( cur_dev > devices.NOD );

        system ( "clear" );

        cur_dev--;      // Decrement cur_dev by 1 ( we have zero-based device list,
                        // but 1-based is user-friendly )

        do {
                ret = menu ();
        } while ( ret == MENU_CONTINUE );

        return  ret;
}

int exit_program ( void )
{
        printf ( "\nPress any key to exit" );
        getchar ();

        return  MENU_EXIT;
}

int main ( int argc, const char ** argv )
{
        int res;

        // change terminal mode to non-buffering
        system ( "stty -icanon min 1" );
        setbuf ( stdin, NULL );
        system ( "clear" );

        if ( USMC_Init ( &devices ) )
        {
                print_error ();

                return  exit_program ();
        }

        while ( select_menu () );

        res = exit_program ();
        //restore terminal mode
        system ( "stty -icanon" );

        return  res;
}



BOOL f1_get_dev_state ( void )
{
        if ( USMC_GetState ( cur_dev, &state ) )
                return  TRUE;

        system ( "clear" );
        print_state ( state );
        printf ( "\nPress any key to continue" );
        getchar ();
        system ( "clear" );

        return  FALSE;
}

BOOL f2_start ( void )
{
        float speed    = 2000.0f;
        int   dest_pos = 0;

        if ( USMC_GetStartParameters ( cur_dev, &start_params ) )
                return  TRUE;

        system ( "clear" );
        scan_start_params ( &dest_pos, &speed, &start_params );
        start_params.SlStart = TRUE;

        if ( USMC_Start ( cur_dev, dest_pos, &speed, &start_params ) )
                return  TRUE;
       
        system ( "clear" );
        print_start_params ( dest_pos, speed, start_params );
        printf ( "\n" );
        return  FALSE;
}

BOOL f3_stop ( void )
{
        if ( USMC_Stop ( cur_dev ) )
                return  TRUE;
       
        system ( "clear" );
        printf ( "Motor stopped\n\n" );

        return  FALSE;
}

BOOL f4_set_params ( void )
{
        if ( USMC_GetParameters ( cur_dev, &params ) )
                return  TRUE;

        // Set anything you want here
        params.MaxTemp = 70.0f;
        params.AccelT  = 200.0f;
        params.DecelT  = 200.0f;
        params.BTimeout1 = 5000.0f;
        params.BTimeout2 = 5000.0f;
        params.BTimeout3 = 5000.0f;
        params.BTimeout4 = 5000.0f;
        params.BTO1P = 625.0f;
        params.BTO2P = 16.0f;
        params.BTO3P = 625.0f;
        params.BTO4P = 16.0f;//500.0f;
        params.MinP  = 500.0f;
        params.BTimeoutR  = 500.0f;
        params.LoftPeriod = 500.0f;
        params.RTDelta    = 200;
        params.RTMinError = 15;
        params.EncMult = 2.5f;
        params.MaxLoft = 32;
        params.PTimeout = 100.0f;
        params.SynOUTP  = 1;
        //

        if ( USMC_SetParameters ( cur_dev, &params ) )
                return  TRUE;

        if ( USMC_SaveParametersToFlash ( cur_dev ) )
                return  TRUE;

        system ( "clear" );
        print_params ( params );
        printf ( "\nThese Parameters are Saved to Flash" );
        printf ( "\nPress any key to continue" );
        getchar ();
        system ( "clear" );
       
        return  FALSE;
}

BOOL f5_set_mode ( void )
{
        if ( USMC_GetMode ( cur_dev, &mode ) )
                return  TRUE;

        // Set anything you want here
        mode.SyncInvert = !mode.SyncInvert;
        mode.EncoderEn  = TRUE;
        mode.RotTrOp = FALSE;
        mode.ResetRT = TRUE;
        mode.ResBEnc = TRUE;
        // Set anything you want here

        if ( USMC_SetMode ( cur_dev, &mode ) )
                return  TRUE;

       
        system ( "clear" );
        print_mode ( mode );
        printf ( "\nPress any key to continue" );
        getchar ();
        system ( "clear" );
       
        return  FALSE;
}

BOOL f6_set_cur_pos ( void )
{
        int newCurPos = 1000;

        if ( USMC_SetCurrentPosition ( cur_dev, newCurPos ) )
                return  TRUE;
       
        system ( "clear" );
        printf ( "\nSetCurrentPosition executed with argument %d", newCurPos );
        printf ( "\nPress any key to continue" );
        getchar ();
        system ( "clear" );
       
        return  FALSE;
}

BOOL f7_turn_off_and_save_pos ( void )
{
        // Initialize structures (in case this function runs first)
        if ( USMC_GetParameters ( cur_dev, &params ) )
                return  TRUE;
        if ( USMC_GetMode ( cur_dev, &mode ) )
                return  TRUE;

        // Go to Full Step (automaticaly), then Turn Off
        mode.ResetD = TRUE;

        if ( USMC_SetMode ( cur_dev, &mode ) )
                return  TRUE;

        // Wait until Previous Comand is Done
        do {
                usleep ( 50 );

                if ( USMC_GetState ( cur_dev, &state ) )
                        return  TRUE;
        } while ( state.Power == TRUE );

        // Remember CurPos in Parameters Only While state.Power - FALSE
        params.StartPos = state.CurPos;

        if ( USMC_SetParameters ( cur_dev, &params ) )
                return  TRUE;

        // Then of Course You Need to SaveToFlash
        if ( USMC_SaveParametersToFlash ( cur_dev ) )
                return  TRUE;

        // Now You Can Unplug ME
        system ( "clear" );
        printf ( "\nThe Position \"%d\" is Saved to Flash\n", params.StartPos );
        printf ( "\nWhen Controller is Powered Up Next Time\n" );
        printf ( "\nIt Will Start From This Position\n" );
        printf ( "\nPress any key to exit" );
        getchar ();
        system ( "clear" );
       
        return  FALSE;
}

BOOL fs7_revert_sp_to_0 ( void )
{
        // Initialize structures (in case this function runs first)
        if ( USMC_GetParameters ( cur_dev, &params ) )
                return  TRUE;

        params.StartPos = 0;

        if ( USMC_SetParameters ( cur_dev, &params ) )
                return  TRUE;

        // Then of Course You Need to SaveToFlash
        if ( USMC_SaveParametersToFlash ( cur_dev ) )
                return  TRUE;
       
        system ( "clear" );
        printf ( "\nStart Position is Reset to 0\n" );
        printf ( "\nPress any key to continue" );
        getchar ();

        return  FALSE;
}

BOOL f8_get_enc_state ( void )
{
        if ( USMC_GetEncoderState ( cur_dev, &enc_state ) )
                return  TRUE;
        if ( USMC_GetParameters ( cur_dev, &params ) )
                return  TRUE;
       
        system ( "clear" );
        print_enc_state ( enc_state, params );
        printf ( "\nPress any key to continue" );
        getchar ();
        system ( "clear" );

        return  FALSE;
}

BOOL fp_change_power ( void )
{
        if ( USMC_GetMode ( cur_dev, &mode ) )
                return  TRUE;

        smpower     = !smpower;
        mode.ResetD = !smpower;

        if ( USMC_SetMode ( cur_dev, &mode ) )
                return  TRUE;
       
        system ( "clear" );
        printf ( "Now, Power is %s\n", mode.ResetD ? "Off" : "On" );

        return  FALSE;
}