ServoComms.h

Go to the documentation of this file.

#ifndef SERVOCOMMS_H
#define SERVOCOMMS_H

// Required C header files from the array control code

#include "gcp/control/code/unix/libunix_src/common/regmap.h" // RegMapBlock

#include "gcp/antenna/control/specific/AxisPositions.h"
#include "gcp/antenna/control/specific/Board.h"
#include "gcp/antenna/control/specific/Model.h"
#include "gcp/antenna/control/specific/ServoCommand.h"
#include "gcp/antenna/control/specific/PmacTarget.h"

#include "gcp/util/common/Angle.h"
#include "gcp/util/common/FdSet.h"

#include <termios.h>

#include <string>

#define SERVO_TIMEOUT_USEC 150000 

namespace gcp {
  namespace antenna {
    namespace control {

      class SpecificShare;
      
      class ServoComms : public Board {
    
      public:
    
    ServoComms(SpecificShare* share, std::string name, bool sim);
    
    ServoComms();
    
    ~ServoComms();

    void setAzEl(gcp::util::Angle& az, gcp::util::Angle& el);
    void hardStopAntenna(); //  not nice on drives.  don't use.
    void haltAntenna();
    void calibrateEncoders();
    void initializeAntenna(int antNum=1);
    void queryStatus();
    void queryStatus(gcp::util::TimeVal& currTime);
    void queryAntPositions();
    void queryAntPositions(gcp::util::TimeVal& currTime);

    bool readPosition(AxisPositions* axes, Model* model);
    void commandNewPosition(PmacTarget* pmac);

    void setMaxAzVel(int antNum, float vMax);

    void finishInitialization();
    

    int inactiveCount_;


    void fillUtc(gcp::util::TimeVal& currTime);


    bool connect();
    
    void disconnect();

    bool servoIsConnected();
    
    void sendCommand(ServoCommand& command);
    
    int readResponse(ServoCommand& command);

    // Return true if the servo is not ready for another command

    bool isBusy();

    // Return true if we have a 1PPS
    bool isPpsPresent();

    // Return true if the encoders are calibrated
    bool isCalibrated();

    // Returns the value of the tracking loop (1 -TON, 3 - Cal encoders, 5 - Time loop)
    int checkTrackLoop();

    unsigned readPositionFault();

    //------------------------------------------------------------
    // Methods which will be used to read and write register values.
    // These methods will send a the appropriate command to the servo,
    // and time out waiting for a response.
    //------------------------------------------------------------
    
    int writeString(std::string message);

      private:

    bool sim_;

    int fd_;
    
    struct termios termioSave_;
    
    RegMapBlock* utc_;        
    RegMapBlock* azPositions_;        
    RegMapBlock* azErrors_;        
    RegMapBlock* elPositions_;        
    RegMapBlock* elErrors_;        
    RegMapBlock* errorCount_;
    RegMapBlock* slowAzPos_; 
    RegMapBlock* slowElPos_; 
    RegMapBlock* az1CommI_;  
    RegMapBlock* az2CommI_;  
    RegMapBlock* el1CommI_;  
    RegMapBlock* az1ActI_;   
    RegMapBlock* az2ActI_;   
    RegMapBlock* el1ActI_;   
    RegMapBlock* az1Enable_; 
    RegMapBlock* az2Enable_; 
    RegMapBlock* el1Enable_; 
    RegMapBlock* taskLoop_; 
    RegMapBlock* encStatus_;   
    RegMapBlock* lowSoftAz_;
    RegMapBlock* lowHardAz_;
    RegMapBlock* hiSoftAz_; 
    RegMapBlock* hiHardAz_; 
    RegMapBlock* lowSoftEl_;
    RegMapBlock* lowHardEl_;
    RegMapBlock* hiSoftEl_; 
    RegMapBlock* hiHardEl_; 
    RegMapBlock* azWrap_; 
    RegMapBlock* azBrake_; 
    RegMapBlock* elBrake_; 
    RegMapBlock* eStop_; 
    RegMapBlock* simulator_; 
    RegMapBlock* ppsPresent_; 

    gcp::util::FdSet fdSet_;
    
    bool connected_;
    
      public:
    ServoCommand command_;

      private:
    ServoCommand issueCommand(ServoCommand::Request req, std::vector<float>& values);
    ServoCommand issueCommand(ServoCommand::Request req);
    
    bool responsePending_;

    int calibrationStatus_;

    bool initializationComplete_;

    void serviceMsgQ();
    
    void zeroReadFds();
    
    void registerReadFd(int fd);
    
    int waitForNextMessage();

    int readSerialPort(ServoCommand& command);

    void waitForResponse();

    void recordStatusResponse(ServoCommand& command);

    void wait(long nsec=100000000);

    
      }; // End class ServoComms
      
    }; // End namespace control
  }; // End namespace antenna
}; // End namespace gcp




#endif // End #ifndef