Index: trunk/MagicSoft/Cosy/main/MCosy.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.cc (revision 926) +++ trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1111) @@ -22,4 +22,6 @@ //#include // PRIO_PROCESS +ClassImp(MCosy); + typedef struct tm tm_t; @@ -32,15 +34,15 @@ double Rad2SE(double rad) { - return 16384.0/D2PI*rad; + return 16384.0/k2Pi*rad; } double Rad2ZdRE(double rad) { - return 16384.0/D2PI*rad*kGearRatio.X(); + return 16384.0/k2Pi*rad*kGearRatio.X(); } double Rad2AzRE(double rad) { - return 16384.0/D2PI*rad*kGearRatio.Y(); + return 16384.0/k2Pi*rad*kGearRatio.Y(); } @@ -68,8 +70,8 @@ ZdAz source = src * 360.0/16384.0; - ZdAz dest = dst * 360.0/D2PI; + ZdAz dest = dst * kRad2Deg; if (dest.Zd()>-1e-6 && dest.Zd()<1e-6) - return dst*(16384.0/D2PI); + return dst*(16384.0/k2Pi); const float fZdMin = -67; @@ -122,7 +124,18 @@ } - +// -------------------------------------------------------------------------- +// +// GetSePos, reads the Shaftencoder positions from the Can-drivers +// for the shaftencoders. The two shaft encoders at the elevation axis +// are avaraged. The values are returned as a ZdAz object. +// +// The positions are alway up-to-date because the shaftencoders are +// sending all changes immediatly. +// ZdAz MCosy::GetSePos() { + // + // Get the values + // const int p0 = fZd1->GetPos(); const int p1 = fZd2->GetPos(); @@ -144,4 +157,46 @@ } +// -------------------------------------------------------------------------- +// +// request the current positions from the rotary encoders. +// use GetRePos to get the psotions. If the request fails the function +// returns kFALSE, otherwise kTRUE +// +Bool_t MCosy::RequestRePos() +{ + // + // Send request + // + fMac2->RequestSDO(0x6004); + fMac1->RequestSDO(0x6004); + + // + // Wait until the objects are received. + // + WaitForSdos(); + + // + // If waitng was not interrupted everything is ok. return. + // + if (!StopWaitingForSDO()) + return kTRUE; + + // + // If the waiting was interrupted due to a network error, + // print some logging message. + // + if (HasError()) + lout << "Error #6004 (requesting re pos from Macs) happened." << endl; + + return kFALSE; +} + +// -------------------------------------------------------------------------- +// +// reads the Rotary encoder positions from the last request of the Macs. +// +// The positions are returned as a ZdAz object. Use RequestRePos to request +// the current positions first. +// ZdAz MCosy::GetRePos() { @@ -149,4 +204,13 @@ } +// -------------------------------------------------------------------------- +// +// reads the Rotary encoder positions from the Macs. +// +// The positions are returned as a ZdAz object. The positions are the ones +// which are send as PDOs to the computer. This is done at a given +// frequency. Which means, that this positions are not ought to be +// up-to-date. +// ZdAz MCosy::GetRePosPdo() { @@ -154,4 +218,14 @@ } +// -------------------------------------------------------------------------- +// +// set the velocity and accelerations for position maneuvers. +// +// The acceleratin is set as given (in percent of maximum). +// The velocity is given in percent, depending on the ratio (<1 or >1) +// one of the axis becomes a slower velocity. This is used for maneuvers +// in which both axis are moved synchromously and should reach their +// target position at the same time. +// void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc) { @@ -186,7 +260,16 @@ } +// -------------------------------------------------------------------------- +// +// Does a relative positioning. +// +// The steps to move are given in a ZdAz object relative to the current +// position. The coordinates are given in Roteryencoder steps. +// Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only +// if Axis 2==kTRUE. The function waits for the movement to be finished. +// void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) { - SetStatus(kMoving); + SetStatus(MCosy::kMoving); if (axe1) fMac2->StartRelPos(rd.Zd()); @@ -200,21 +283,80 @@ } +// -------------------------------------------------------------------------- +// +// check for a break-signal (from the msgqueue) and errors. +// +int MCosy::StopWaitingForSDO() const +{ + return Break() || HasError(); +} + +// -------------------------------------------------------------------------- +// +// Waits for a movement to become finished. +// +// First waits for all peding Sdos, then waits until both motors are stopped +// or waiting for SDOs was stopped (either by an error or by Break) +// +void MCosy::WaitForEndMovement() +{ + WaitForSdos(); + + while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO()) + usleep(1); +} + +// -------------------------------------------------------------------------- +// +// Check for an error... +// +// This is ment for usage after the Action: All Motors Stop. +// void MCosy::CheckForError() { + // + // Check all Can-Nodes for an Error. If there is no error the motor + // status is set to stopped. + // if (!HasError()) { - SetStatus(kStopped); + SetStatus(MCosy::kStopped); return; } - SetStatus(kError); + // + // If there is an error, the error status is set to Error. + // + SetStatus(MCosy::kError); + + // + // No try to handle the error. + // fMac1->HandleError(); fMac2->HandleError(); + + // + // If the error couldn't get solved return + // if (HasError()) return; - SetStatus(kStopped); -} - + // + // Set motor status to stopped + // + SetStatus(MCosy::kStopped); +} + +// -------------------------------------------------------------------------- +// +// Move the telescope to the given position. The position must be given in +// a ZdAz object in rad. +// +// The first positioning is done absolutely. If we didn't reach the +// correct psotion we try to correct for this by 10 relative position +// maneuvers. If this doesn't help positioning failed. +// +// As a reference the shaftencoder values are used. +// int MCosy::SetPosition(const ZdAz &dst) // [rad] { @@ -296,27 +438,33 @@ } -Bool_t MCosy::RequestRePos() -{ - - fMac2->RequestSDO(0x6004); - fMac1->RequestSDO(0x6004); +// -------------------------------------------------------------------------- +// +// Sets the tracking velocity +// +// The velocities are given in a ZdAz object in re/min. Return kTRUE +// in case of success, kFALSE in case of failure. +// +Bool_t MCosy::SetVelocity(ZdAz v) +{ + // + // Send the new velocities for both axes. + // + fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min] + fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min] + + // + // Wait for the objects to be OKed. + // WaitForSdos(); + + // + // If the waiting for the objects wasn't interrupted return kTRUE + // if (!StopWaitingForSDO()) return kTRUE; - if (HasError()) - lout << "Error #6004 (requesting re pos from Macs) happened." << endl; - - return kFALSE; -} - -Bool_t MCosy::SetVelocity(ZdAz v) -{ - fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min] - fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min] - WaitForSdos(); - if (!StopWaitingForSDO()) - return kTRUE; - + // + // print a message if the interruption was due to a Can-node Error + // if (HasError()) lout << "Error #3006 (setting velocity of Macs) happened." << endl; @@ -325,4 +473,12 @@ } +// -------------------------------------------------------------------------- +// +// Initializes Tracking mode +// +// Initializes the accelerations of both axes with 90% of the maximum +// acceleration. Set the status for moving and tracking and starts thr +// revolution mode. +// void MCosy::InitTracking() { @@ -336,5 +492,5 @@ fMac1->SetDeceleration(0.90*fMac1->GetVelRes()); - SetStatus(kMoving | kTracking); + SetStatus(MCosy::kMoving | MCosy::kTracking); fMac2->SetRpmMode(TRUE); @@ -342,4 +498,12 @@ } +// -------------------------------------------------------------------------- +// +// Limits the speed. +// +// This function should work as a limiter. If a tracking error is too large +// to be corrected fast enough we would get enormous velocities. These +// velocities are limited to the maximum velocity. +// void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const { @@ -527,34 +691,42 @@ } -int MCosy::IsPositioning() const -{ - return (fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO(); -} - -void MCosy::WaitForEndMovement() -{ - WaitForSdos(); - - while (IsPositioning()) - usleep(1); -} - +// -------------------------------------------------------------------------- +// +// Stops the movement of both motors. +// +// Sets the status to stopping. Sets the deceleration to 50% of the maximum. +// stops. Quits the revolution mode and wait for the end of the movement. +// void MCosy::StopMovement() { - SetStatus(kStopping); - + // + // Set status to stopped + // + SetStatus(MCosy::kStopping); + + // + // set deceleration to 50% + // cout << "Stopping positioning..." << endl; fMac1->SetDeceleration(0.5*fMac1->GetVelRes()); fMac2->SetDeceleration(0.5*fMac2->GetVelRes()); - cout << "Stoping possible RPM mode..." << endl; + // + // Stop revolution mode (movement) + // + cout << "Stoping possibleRPM mode..." << endl; fMac1->SetRpmMode(FALSE); fMac2->SetRpmMode(FALSE); + // + // Wait for the movement to really be finished. + // cout << "Waiting for silence..." << endl; WaitForEndMovement(); + // + // Check whether everything works fine. + // CheckForError(); - cout << "Movement stopped." << endl; } @@ -587,5 +759,5 @@ RaDec rd(37.94, 89.2644); - ZdAz za=sla.CalcZdAz(rd*D2PI/360.0)*16384.0/D2PI; + ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi; cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl; @@ -607,5 +779,5 @@ ZdAz dest = *((ZdAz*)mp); - SetPosition(dest*D2PI/360.0); + SetPosition(dest*kDeg2Rad); } cout << "WM_Position: done. (return 0x7777)" << endl; @@ -616,5 +788,5 @@ { RaDec dest = *((RaDec*)mp); - TrackPosition(dest*D2PI/360.0); + TrackPosition(dest*kDeg2Rad); } cout << "WM_Track: done. (return 0x8888)" << endl; @@ -843,9 +1015,12 @@ -int MCosy::StopWaitingForSDO() const -{ - return Break() || HasError(); -} - +// -------------------------------------------------------------------------- +// +// Start the work of the application: +// +// Start the Can-Network. +// Start the MCosy::TalkThread thread. +// turn on the gui update +// void MCosy::Start() { @@ -861,4 +1036,12 @@ } +// -------------------------------------------------------------------------- +// +// Start the work of the application: +// +// Turn of the gui update +// stop the MCosy::TalkThread thread. +// Stop the network +// void MCosy::Stop() { Index: trunk/MagicSoft/Cosy/main/MCosy.h =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.h (revision 926) +++ trunk/MagicSoft/Cosy/main/MCosy.h (revision 1111) @@ -37,9 +37,4 @@ }; -#define kError 0x01 -#define kMoving 0x02 -#define kTracking 0x04 -#define kStopping 0x08 -#define kStopped 0x10 class TTimer; @@ -50,4 +45,13 @@ private: + enum + { + kError = 0x01, + kMoving = 0x02, + kTracking = 0x04, + kStopping = 0x08, + kStopped = 0x10 + }; + ShaftEncoder *fZd1; ShaftEncoder *fZd2; @@ -99,5 +103,4 @@ int StopWaitingForSDO() const; - int IsPositioning() const; void CheckForError(); @@ -117,6 +120,7 @@ static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst); -// static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0)); + // static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0)); + ClassDef(MCosy, 0) }; Index: trunk/MagicSoft/Cosy/main/MainCint.h =================================================================== --- trunk/MagicSoft/Cosy/main/MainCint.h (revision 926) +++ (revision ) @@ -1,35 +1,0 @@ -/******************************************************************** -* MainCint.h -********************************************************************/ -#ifdef __CINT__ -#error MainCint.h/C is only for compilation. Abort cint. -#endif -#include -#include -#include -#include -#include -#define G__ANSIHEADER -#define G__DICTIONARY -#include "G__ci.h" -extern "C" { -extern void G__cpp_setup_tagtableMainCint(); -extern void G__cpp_setup_inheritanceMainCint(); -extern void G__cpp_setup_typetableMainCint(); -extern void G__cpp_setup_memvarMainCint(); -extern void G__cpp_setup_globalMainCint(); -extern void G__cpp_setup_memfuncMainCint(); -extern void G__cpp_setup_funcMainCint(); -extern void G__set_cpp_environmentMainCint(); -} - - -#include "TROOT.h" -#include "TMemberInspector.h" -#include "MainIncl.h" - -#ifndef G__MEMFUNCBODY -#endif - - -/* STUB derived class for protected member access */ Index: trunk/MagicSoft/Cosy/main/MainLinkDef.h =================================================================== --- trunk/MagicSoft/Cosy/main/MainLinkDef.h (revision 926) +++ trunk/MagicSoft/Cosy/main/MainLinkDef.h (revision 1111) @@ -5,3 +5,5 @@ #pragma link off all functions; +#pragma link C++ class MCosy; + #endif Index: trunk/MagicSoft/Cosy/main/Makefile =================================================================== --- trunk/MagicSoft/Cosy/main/Makefile (revision 926) +++ trunk/MagicSoft/Cosy/main/Makefile (revision 1111) @@ -35,7 +35,8 @@ MStarguider.cc -SRCS = $(SRCFILES) -HEADERS = $(SRCFILES:.cc=.h) -OBJS = $(SRCFILES:.cc=.o) +SRCS = $(SRCFILES) +HEADERS = $(SRCFILES:.cc=.h) +OBJS = $(SRCFILES:.cc=.o) +CINTHEADERS = $(HEADERS) ############################################################