Subversion Repositories seema-scanner

#include "RotationStage.h"

#include <string.h>
#include <iostream>
#include <sstream>
#include <unistd.h>

RotationStage::RotationStage(){

    char out[64];
    char in[64];

    memset(out,0,64);
    memset(in,0,64);

    // acquire information
    AR_DWORD num;
    if(!fnPerformaxComGetNumDevices(&num)){
        throw "Error in fnPerformaxComGetNumDevices!";
    }
    if(num<1){
        throw "No motor found!";
    }

    char lpDeviceString[PERFORMAX_MAX_DEVICE_STRLEN];
    if(!fnPerformaxComGetProductString(0, lpDeviceString, PERFORMAX_RETURN_SERIAL_NUMBER) ||
            !fnPerformaxComGetProductString(0, lpDeviceString, PERFORMAX_RETURN_DESCRIPTION) ){
        throw "Error acquiring product string!";
    }

    printf("device description: %s\n", lpDeviceString);

    //setup the connection
    if(!fnPerformaxComOpen(0,&Handle)){
        throw "Error opening device!";
    }

    if(!fnPerformaxComSetTimeouts(5000,5000)){
        throw "Error setting timeouts!";
    }

    if(!fnPerformaxComFlush(Handle)){
        throw "Error flushing the coms!";
    }

    // setup the device
    strcpy(out, "LSPD=3000"); //set low speed
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }
    strcpy(out, "HSPD=100000"); //set high speed
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "ACC=300"); //set acceleration
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "DEC=300"); //set acceleration
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }


    strcpy(out, "EO=1"); //enable device
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "INC"); //set incremental mode
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "ID"); //read ID
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    printf("Arcus Product: %s\n",in);

    strcpy(out, "DN"); //read Device nr
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        printf("Could not send!");
    }

    printf("Device Number: %s\n",in);

    strcpy(out, "EO"); //read current
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }
    printf("Motor Power: %s\n",in);

    strcpy(out, "EX"); //Encoder value
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    printf("Encoder Value: %s\n",in);

    strcpy(out, "PX=0"); //Position value
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "PX"); //Position value
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }
    printf("Position Value init: %s\n",in);

}

int RotationStage::executeStringCommand(std::string cmd){

    char out[64];
    char in[64];

    strcpy(out, cmd.c_str());
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        std::cerr << "Could not execute command " << cmd << "!" << std::endl;
        return 1;
    } else
        return 0;

}

int RotationStage::executeStringCommandBlocking(std::string cmd){

    char out[64];
    char in[64];

    do{
        strcpy(out, cmd.c_str());
        if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
            std::cerr << "Could not execute command " << cmd << "!" << std::endl;
            return 1;
        }
        usleep(1000);
    } while(in[0]!='0');

    return 0;
}


void RotationStage::moveToAngle(float angle){

    std::ostringstream cmdStream;
    cmdStream << "H" << (int)(angle*1000);
    executeStringCommandBlocking(cmdStream.str());

}