28 กุมภาพันธ์ 2553

Nemin Pan-Tilt Library for Biclops Mo...

Nemin Pan-Tilt Library for Biclops Model PTV
ถ้ายังไม่ได้อ่านเรื่องการ Config Project อ่านได้จาก ใช้งาน Biclops Pan-Tilt-Verge Camera Head

C++ Source Code
Pantilt.h
#pragma once

#include <cstdio>          // for FILE defn

#include <iostream>         // for cout, etc
using namespace std;

#include "PMDUtils.h"       // for DoSleep
#include "Biclops.h"

class CPantilt
{
protected:
    // THE interface to Biclops.
Biclops biclops;

// Defines which axes we want to use.
int axisMask ;//+ Biclops::VergeMask; // add this if you want verge.

// Pointers to each axis (populated once controller is initialized).
PMDAxisControl *panAxis;
PMDAxisControl *tiltAxis;
PMDAxisControl *vergeAxis;
public:
    CPantilt(char *conf);
    ~CPantilt(void);
    void setup(void);
    int homing(void);
    void move(float panD, float tiltD, float vergeD);
    void motorOff(void);
    void moveRat(float panRetD, float tiltRatD, float vergeRatD);
protected:
    char conf[255];
};


Pantilt.cpp
#include "StdAfx.h"
#include "Pantilt.h"

CPantilt::CPantilt(char *conf)
:
panAxis(NULL),
tiltAxis(NULL),
vergeAxis(NULL),
axisMask(Biclops::PanMask + Biclops::TiltMask)
{
    strcpy(this->conf, conf);
    homing();
}

CPantilt::~CPantilt()
{

    motorOff();
}

int CPantilt::homing(void)
{
    // Set Debug level depending on how much info you want to see about
    // the inner workings of the API. Level 2 is highest with 0 being
    // the default (i.e., no messages).
    biclops.SetDebugLevel(2);

    // Initialize the Biclops unit.
    TRACE("\nInitializing Biclops");
    if (!biclops.Initialize(conf))
    {
        TRACE("\nCan't open configuration file");
        return 0;
    }
    // Initialization completed successfully.
    TRACE("\nBiclops initialized");
    biclops.SetDebugLevel(0);

    // Get shortcut references to each axis.
    panAxis =    biclops.GetAxis(Biclops::Pan);
    tiltAxis =    biclops.GetAxis(Biclops::Tilt);
    vergeAxis = biclops.GetAxis(Biclops::Verge);

    //--------- Homing ------------
    TRACE("\nBegin homing sequence." );
    if (!biclops.HomeAxes(axisMask,true))
    {
        TRACE("Can't homing");
        return 0;
    }

    return 1;
}

void CPantilt::move(float panD, float tiltD, float vergeD)
{    
    // Get the currently defined (default) motion profiles.
    PMDAxisControl::Profile panProfile,tiltProfile,vergeProfile;
    panAxis->GetProfile(panProfile);
    tiltAxis->GetProfile(tiltProfile);
    vergeAxis->GetProfile(vergeProfile);

    // Set a position to move to by modifying the respective profiles.
    // NOTE: profile values are in revolutions, so here we convert
    // from degrees (divide by 360) for readability.
    panProfile.pos = PMDUtils::DegsToRevs(panD);
    tiltProfile.pos = PMDUtils::DegsToRevs(tiltD);
    vergeProfile.pos = PMDUtils::DegsToRevs(vergeD);

    // Inform the controller of the new desired position.
    panAxis->SetProfile(panProfile);
    tiltAxis->SetProfile(tiltProfile);
    vergeAxis->SetProfile(vergeProfile);
    
    TRACE("\nMoving Pan Tilt Verge:(%f, %f, %f)",panD,tiltD,vergeD);
    biclops.Move(axisMask);
}

void CPantilt::motorOff(void)
{
    // Turn off motor amps to conserve power.
    move(0,0,0);
    panAxis->DisableAmp();
    tiltAxis->DisableAmp();
    vergeAxis->DisableAmp();
}

void CPantilt::moveRat(float panRetD, float tiltRatD, float vergeRatD)
{
    // Get the currently defined (default) motion profiles.
    PMDAxisControl::Profile panProfile,tiltProfile,vergeProfile;
    panAxis->GetProfile(panProfile);
    tiltAxis->GetProfile(tiltProfile);
    vergeAxis->GetProfile(vergeProfile);

    // Set a position to move to by modifying the respective profiles.
    // NOTE: profile values are in revolutions, so here we convert
    // from degrees (divide by 360) for readability.
    float pan = PMDUtils::RevsToDegs(panProfile.pos)+panRetD,
        tilt = PMDUtils::RevsToDegs(tiltProfile.pos)+tiltRatD,
        verge = PMDUtils::RevsToDegs(vergeProfile.pos)+vergeRatD;
    panProfile.pos = PMDUtils::DegsToRevs(pan);
    tiltProfile.pos = PMDUtils::DegsToRevs(tilt);
    vergeProfile.pos = PMDUtils::DegsToRevs(verge);

    // Inform the controller of the new desired position.
    panAxis->SetProfile(panProfile);
    tiltAxis->SetProfile(tiltProfile);
    vergeAxis->SetProfile(vergeProfile);
    
    TRACE("\nMoving Pan Tilt Verge:(%f, %f, %f)",pan,tilt,verge);
    biclops.Move(axisMask);
}



วิธีการใช้งาน

#include "Pantilt.h"

// สร้าง Object จะทำการต่อกับ Pantilt ให้โดยอัตโนมัติ
char pantiltConf[] = "BiclopsDefaultKMUTTv3.cfg";
CPantilt pantilt(pantiltConf);

//สั่ง pan tilt verge ไปยังตำแหน่งที่ต้องการ ในหน่วยองศา
//อารกิวเมนต์ตัวแรกคือตำแหน่ง Pan ตัวที่สองคือ Tilt และ ตัวสุดท้ายคือ Verge
pantilt.move(0,-40,0);

//สั่ง pan tilt Verge ไปยังตำแหน่งใหม่แบบ Relative คือบวก/ลบ จากตำแหน่งเดิม ในหน่วยองศา
//อารกิวเมนต์ตัวแรกคือตำแหน่ง Pan ตัวที่สองคือ Tilt และ ตัวสุดท้ายคือ Verge
pantilt.moveRat(10,10,0)






เขียนโดย ที่

ไม่มีความคิดเห็น:

แสดงความคิดเห็น

สมัครสมาชิก: ส่งความคิดเห็น (Atom)