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;

}