/***************************************************************************** Copyright 1998 MBARI ****************************************************************************** Summary : iView pan-tilt axis class Filename : Axis.h Author : Michael B. Matthews Project : iView Version : 3.8 Created : 11.2.98 Modified : 23.3.98 Notes : Used to be part of device.h in versions before V3.8 ****************************************************************************/ #ifndef axis_h #define axis_h #define IO_BUFFERING // turns on buffering of control system varibles for // logging; see Axis::Log() #define IVIEW_POSITIVE_LIMIT_DM "POSITIVE_LIMIT" #define IVIEW_NEGATIVE_LIMIT_DM "NEGATIVE_LIMIT" #define IVIEW_OFFSET_DM "OFFSET" #define IVIEW_PANORAMIC_OFFSET_DM "PANORAMIC_OFFSET" #define IVIEW_MOTOR_STALL_DM "MOTOR_STALL" #define IVIEW_PGAIN_DM "PGAIN" #define IVIEW_IGAIN_DM "IGAIN" #define IVIEW_DGAIN_DM "DGAIN" #define IVIEW_FGAIN_DM "FGAIN" #define AXIS_CAL_STEP_SIZE 5.0 // calibration step size #define AXIS_CAL_RANGE 100 // calibration range #define AXIS_CAL_NUM_STEPS (Int16) (AXIS_CAL_RANGE/AXIS_CAL_STEP_SIZE) #define ERROR_STOP 1.0 // error scaling constants for optimal #define DERROR_STOP 10.0 // stopping criterion #define ERROR_STOP_THRESHOLD DEGS_TO_RADS(0.0*0.0) // (MBM) #define ERROR_STOP_HYSTERESIS DEGS_TO_RADS(0.25*0.25) #define INNOVATIONS_THRESHOLD DEGS_TO_RADS(5.0) // for motor stall detection #define STALL_DELAY_COUNT 30 // sample periods before clearing stall #define STALL_DETECTION_COUNT 60 // sample periods before detecting stall #define STALL_INTEGRATOR 0.99 #define STALL_VELOCITY 10.0 // velocity clip during stall #define STALL_DETECTION_THRESHOLD 3.5 // rads #define STALL_DETECTION_HYSTERESIS 1.0 // rads #define INTEGRATOR_DUMP_THRESHOLD 0.1 // rads #define FGAIN 0.50 // feed-forward gain enum { STANDBY, CONTROL }; // DM item struct for class Axis struct AxisDMItems { DmFlt32Object* positiveLimit; // iView positive axis limit DmFlt32Object* negativeLimit; // iView negative axis limit DmFlt32Object* offset; // axis offset DmFlt32Object* panoramicOffset; // axis panoramic offset DmFlt32Object* pGain; // proportional gain DmFlt32Object* iGain; // integral gain DmFlt32Object* dGain; // differential gain DmFlt32Object* fGain; // feedforward gain DmFlt32Object* analogDegs; // Analog position feedback DmFlt32Object* stepperDegs; // Stepper Controller position feedback DmFlt32Object* position; // position estimate in correct coords DmFlt32Object* positionCmd; // Commanded pan/tilt position DmFlt32Object* cwLimitPos; // controller cw limit DmFlt32Object* ccwLimitPos; // controller ccw limit DmBooleanObject* cwLimit; // Clockwise limit switch closed DmBooleanObject* ccwLimit; // Counter Clockwise limit switch closed DmBooleanObject* motorStall; // indicates a motor stall DmEmptyObject* analogRequest; // analog position request command DmEmptyObject* stepperRequest; // stepper position request command DmInt16Object* velocityCmd; // Commanded pan/tilt velocity DmStringObject* ctlrString; // command string to motor controller }; // configuration struct for class Axis struct AxisConfig { FltN direction; // direction of rotation FltN posLimit; // position limits FltN negLimit; // FltN maxVelocity; // velocity limits FltN minVelocity; // FltN velocityConversion; // FltN pGain; // PID loop gains FltN dGain; // FltN iGain; // FltN fGain; // FltN offset; // analog reading offset to center FltN bracketOffset; // offset of device from center of rotation FltN panoramicOffset; // panoramic mode offset Int16 bufferLength; // length of axis logging buffer }; class CalibrationTable; /**************************************************************************** Class : A X I S Purpose : Pan-tilt axis object class Notes : Axis object belongs to a device object. ******************************************************************************/ class Axis { private: Int index; String name; FILE* fp; // for logging data Word status; // status of axis object MBool control; // closed control loop MBool standby; // standby mode indicator MBool stall; // motor stall indicator MBool integratorDump; // FltN stallIntegrator; // Int stallDetectionCount; // motor stall detection counter Int stallDelayCount; // motor stall delay counter MBool directWrite; // write directly to pan/tilt or write in batch FltN t,s; // time axis FltN dp,u; // FltN error; // servo error FltN ierror; // servo integral error FltN derror; // servo differential error FltN analogDegs; // analog position feedback FltN analogDegsLast; // previous analog position feedback FltN analogPosition; // offset and direction adjusted analog position FltN stepperDegs; // stepper controller position feedback FltN stepperDegsLast; // stepper controller position feedback FltN stepperPosition; // direction corrected stepper controller position FltN stepperOffset; // stepper controller position offset FltN positionDelta; // analogPos - stepperPos = steps dropped FltN positionCmd; // commanded pan/tilt position FltN cwLimitPos; // clockwise limit position FltN ccwLimitPos; // counter clockwise limit position FltN position; // axis position FltN dposition; // rate of change of axis position Int16 velocityCmd; // commanded pan/tilt velocity Int16 stepperCmd; // commands to stepper controllers MBool deltaPosAlarm; // Delta Position Alarm MBool cwLimitSwitch; // ctlr cw limit switch closed MBool ccwLimitSwitch; // ctlr ccw limit switch closed MBool posLimitState; // iView positive limit state MBool negLimitState; // iView negative limit state FltN direction; // direction of rotation FltN posLimit; // positive software limit FltN negLimit; // negative software limit FltN maxVelocity; // maximum velocity limit FltN minVelocity; // minimum velocity limit FltN velocityConversion; // pan-tilt conversion (units/deg/sec) FltN pGain; // proportional loop gain FltN dGain; // differential loop gain FltN iGain; // integral loop gain FltN fGain; // feed-forward gain FltN offset; // position feedback offset values FltN panoramicOffset; // panoramicmode offset values FltN panoramicOffsetDefault; // default panoramicmode offset values Int16 bufferLength; // axis logging buffer length CalibrationTable* calibrationTable; public: DmGroup* group; AxisDMItems Dmi; // data manager items IO_Buffer* positionBuffer; // buffer for analog input data #ifdef IO_BUFFERING IO_Buffer* controlBuffer; // buffers for control outputs IO_Buffer* errorBuffer; // buffers for servo error IO_Buffer* ierrorBuffer; // buffers integral servo error IO_Buffer* derrorBuffer; // buffers differential servo error IO_Buffer* setPointBuffer; IO_Buffer* dsetPointBuffer; IO_Buffer* pKalmanBuffer; IO_Buffer* vKalmanBuffer; IO_Buffer* velocityBuffer; IO_Buffer* innovationsBuffer; #endif Kalman_Filter* pFilter; // IIR filter for position feedback Linear_System* uFilter; // linear filter for position feedback IIR_Filter* integrator; // integral servo error estimate Axis(AxisConfig* config, String& s); ~Axis(); String nameConversion(String& s); String Name() { return name; } STATUS Update(FltN p, FltN dp, MBool link = TRUE); STATUS Velocity(FltN v); STATUS urgentVelocity(FltN v); STATUS Position(FltN a); STATUS requestStepperPosition(); STATUS requestAnalogPosition(); STATUS readStepperPosition(); STATUS readAnalogPosition(); STATUS ReadDMItems(); STATUS Log(); STATUS StartConsumers(); FltN linearCorrection(FltN y); MBool positiveLimitSwitch(); MBool negativeLimitSwitch(); Void setPanoramic() { panoramicOffset = panoramicOffsetDefault; } Void resetPanoramic() { panoramicOffset = 0; } inline STATUS Stop() { Velocity(0); } inline STATUS writeCtlrString(String& s) { /* MBM Dmi.ctlrString->urgentWrite(s.str); */ } inline STATUS FGain(FltN f) { fGain = f; } inline Int16 VelocityCmd() { return velocityCmd; } inline FltN getPosition() { return pFilter->State(1); } inline FltN getAnalogPosition() { return analogPosition; } inline FltN getAnalogDegs() { return analogDegs; } inline FltN getOffset() { return offset; } inline FltN velocityMax() { return maxVelocity; } inline FltN velocityMin() { return minVelocity; } inline FltN getPanoramic() { return panoramicOffset; } inline Void enableControl() { control = TRUE; } inline Void disableControl() { control = FALSE; Velocity(0); } inline Void DirectWrite(MBool d) { directWrite = d; } }; #endif