Subversion Repositories seema-scanner

#include "RotationStage.h"

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

static int wormGearRatio = 72;
static float degreesPerStep = 1.8;

RotationStage::RotationStage(): microSteps(0){

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

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

    // acquire information
    AR_DWORD num;
    if(!fnPerformaxComGetNumDevices(&num)){
        std::cerr << "Error in fnPerformaxComGetNumDevices!" << std::endl;
        return;
    }
    if(num<1){
        std::cerr << "RotationStage error: no motor controller found!" << std::endl;
        return;
    }

//    // show product string
//    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);

    // open connection for deviceId 0
    if(!fnPerformaxComOpen(0,&Handle)){
        throw "Error opening device!";
    }

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

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

//    // stop motor (in case it is running an old command)
//    strcpy(out, "STOP");
//    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
//        throw "Could not send!";
//    }
//    wait();

    strcpy(out, "ID");
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        std::cerr << "RotationStage error: could not retrieve ID!" << std::endl;
    }
    printf("Arcus Product: %s ",in);

    strcpy(out, "DN");
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        printf("Could not send!");
    }
    printf("  %s  ",in);

    strcpy(out, "VER");
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        printf("Could not send!");
    }
    printf("Firmware: %s\n",in);

    // setup "driver parameters"
    strcpy(out, "DRVMS=500"); // microstep setting
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "DRVIT=1"); // idling time
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "DRVIC=100"); // idle current
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "DRVRC=100"); // running current
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "RW"); // write preceding microstep and current settings
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }
    sleep(3);

    microSteps = 0;
    strcpy(out, "RR"); // update settings read
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }

    strcpy(out, "DRVMS"); // read microstep settings
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }
    printf("Microstep setting: %s [2--500]\n",in);
    microSteps = atoi(in);

    strcpy(out, "DRVIT"); // read idling time setting
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }
    printf("Idling time: %s centi-sec\n",in);

    strcpy(out, "DRVIC"); // read idle current settings
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }
    printf("Idle current: %s mA [100--2800]\n",in);

    strcpy(out, "DRVRC"); // read run current settings
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not send!";
    }
    printf("Run current: %s mA [100--3000]\n",in);

    // Setup "movement parameters"
    strcpy(out, "SL=0"); //disable step-n-loop
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not set low speed!";
    }

    strcpy(out, "LSPD=0"); //set low speed
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not set low speed!";
    }

    strcpy(out, "HSPD=200000"); //set high speed
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not set high speed!";
    }

    strcpy(out, "SCV=1"); //enable s-curve acceleration
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        throw "Could not set acceleration type!";
    }

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

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

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

//    strcpy(out, "STORE"); //store settings to flash
//    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
//        throw "Could not send!";
//    }

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

//    // go to home
//    strcpy(out, "H+");
//    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
//        throw "Could not send!";
//    }
//    wait();

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

    std::cout << std::flush;
}

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

    std::string dummy;
    return executeStringCommand(cmd, dummy);
}

bool RotationStage::executeStringCommand(std::string cmd, std::string& ret){
    bool res=false;
    char out[64];
    char in[64];

    mutex.lock();
    strcpy(out, cmd.c_str());
    if(!fnPerformaxComSendRecv(Handle, out, 64,64, in)){
        std::cerr << "Could not execute command " << cmd << "!" << std::endl;
    } else {
        ret = std::string(in);
        res = true;
    }
    mutex.unlock();
    return res;
}

bool RotationStage::isMoving(){

    std::string ret;
    if(!executeStringCommand("MST", ret)){
        throw "Could not get motor status!";
    }
    int status = atoi(ret.c_str());
    // if any of the first three bits is set, the motor is moving
    return status & 0b111;

}

void RotationStage::wait(){

    // poll motor status until not moving
    bool moving = true;
    while(moving){
        std::string ret;
        if(!executeStringCommand("MST", ret)){
            throw "Could not get motor status!";
        }
        int status = atoi(ret.c_str());
        // if any of the first three bits is set, the motor is moving
        moving = status & 0b111;
        usleep(100000);
    }

}

// Return the current angle in the range [0--360[ (deg)
float RotationStage::getAngle(){

    std::string ret;

    if(!executeStringCommand("PX", ret)){
        throw "Could not get position!";
    } else {
        //std::cout << ret << std::endl;
        int posX = atoi(ret.c_str());
        int fullRevX = (360*microSteps*wormGearRatio)/degreesPerStep;
        posX = posX % fullRevX;
        if(posX < 0)
            posX += fullRevX;
        return posX*degreesPerStep/(microSteps*wormGearRatio);
    }

}

void RotationStage::moveRelative(float angle){

    //set incremental mode
    if(!executeStringCommand("INC"))
        throw "Could not set mode!";

    // X position (in microsteps)
    int posX = (angle*microSteps*wormGearRatio)/degreesPerStep;

    std::cout << "Moving relative " << angle << " deg, " << posX << " microsteps" << std::endl;

    std::ostringstream cmdStream;
    cmdStream << "X" << posX;
    //std::cout << cmdStream.str() << std::endl << std::flush;

    if(!executeStringCommand(cmdStream.str()))
        throw "Could not execute move command!";

}

void RotationStage::moveAbsolute(float angle){

    //set absolute mode
    if(!executeStringCommand("ABS"))
        throw "Could not set mode!";

    // get current position
    int currPosX;
    std::string ret;
    if(!executeStringCommand("PX", ret)){
        throw "Could not get position!";
    } else {
        currPosX = atoi(ret.c_str());
    }

    // X position (in microsteps)
    int posX = (angle*microSteps*wormGearRatio)/degreesPerStep;

    int fullRevX = (360*microSteps*wormGearRatio)/degreesPerStep;
    int currPosXNorm = currPosX % fullRevX;
    if(currPosXNorm < 0)
        currPosXNorm += fullRevX;

    // decide to go cw or ccw
    if(posX > currPosXNorm){
        if((posX - currPosXNorm) < fullRevX/2){
            posX = currPosX + (posX - currPosXNorm);
        } else {
            posX = currPosX - fullRevX + (posX - currPosXNorm);
        }
    } else {
        if((currPosXNorm - posX) < fullRevX/2){
            posX = currPosX - (currPosXNorm - posX);
        } else {
            posX = currPosX + fullRevX - (currPosXNorm - posX);
        }

    }

    std::cout << "Moving absolute " << angle << " deg, pos X" << posX << std::endl;

    std::ostringstream cmdStream;
    cmdStream << "X" << posX;
    //std::cout << cmdStream.str() << std::endl << std::flush;

    if(!executeStringCommand(cmdStream.str()))
        throw "Could not execute move command!";
}

RotationStage::~RotationStage(){
    if(Handle)
        fnPerformaxComClose(Handle);

    std::cout << "Rotation stage closed." << std::endl << std::flush;
}