/***************************************************************************** Copyright 1998 MBARI ****************************************************************************** Summary : Integrated viewing routines Filename : Device.C Author : Michael B. Matthews Project : New ROV Version : 3.8 Created : 11.2.98 Modified : 23.3.98 Notes : ******************************************************************************/ #define __PROTOTYPE_5_0 // logMsg variable arg list #include // VxWorks systems declarations #include // VxWorks standard I/O library #include // VxWorks floating point math library #include // VxWorks semaphore library #include // VxWorks watchdog timer library #include // VxWorks system time declarations #include // VxWorks string library #include #include #include #include // VxWorks logMsg library #include // vxWorks vxTas routine definition extern "C" { #include // MBARI type declarations #include // sensor data concentrator definitns #include // VME IBC definitions #include // #include // data manager declarations #include // data manager error declarations #include #include #include #include #include #include #include #include }; #include "Iview.h" #include "Misc.h" // misc functions #include "String.h" // homebrew string classes #include "DataManager.h" // data manager classes #include "DmGroup.h" // data manager classes #include "uFilter.h" // filter and model parameters #include "Buffer.h" // homebrew buffer class #include "Linear_System.h" // linear system and Kalman filter classes #include "Calibration.h" // calibration table class #include "Axis.h" // device axis class #include "Device.h" // device classes #include "ControlSource.h" // control source classes #include "SetPoint.h" #include "iview.h" Extern IView* iView; Extern Void Get_Time(Flt64*, Flt64*); Extern Void Get_Time(Flt64*); Extern SetPoint* setPointRegister[MAX_NUMBER_SETPOINTS]; /***************************************************************************** Function : Device::Init Purpose : Device class constructor Inputs : dev - device identifier : cfg - device configuration struct : sp - initial device setpoint Outputs : None Notes : The pan axis direction is reversed to be consistent with the : definition of the device coordinate system. ******************************************************************************/ STATUS Device::Init(IviewDevice dev, DeviceConfig* cfg) { comLinkError = FALSE; xOrigin = cfg->xOrigin; yOrigin = cfg->yOrigin; zOrigin = cfg->zOrigin; l = cfg->bracketOffsetTilt; device = dev; // map setpoint, set initial coordinates, and declare reference device setPoint = new SetPoint(cfg->xSetPoint, cfg->ySetPoint, cfg->zSetPoint); setPoint->DeviceMap(device); setPoint->Reference(this); // Create pan/tilt axes AxisConfig config; // initialize pan axis config.direction = -1.0; config.maxVelocity = cfg->maxVelocityPan; config.minVelocity = cfg->minVelocityPan; config.posLimit = cfg->posLimitPan; config.negLimit = cfg->negLimitPan; config.pGain = cfg->pGainPan; config.dGain = cfg->dGainPan; config.iGain = cfg->iGainPan; config.fGain = cfg->fGainPan; config.offset = cfg->offsetPan; config.panoramicOffset= cfg->panoramicOffsetPan; config.velocityConversion = cfg->velocityConversion; config.bufferLength = cfg->bufferLength; String s; s = dmName + ".PAN."; pan = new Axis(&config, s); // initialize tilt axis config.direction = 1.0; config.posLimit = cfg->posLimitTilt; config.negLimit = cfg->negLimitTilt; config.maxVelocity = cfg->maxVelocityTilt; config.minVelocity = cfg->minVelocityTilt; config.pGain = cfg->pGainTilt; config.dGain = cfg->dGainTilt; config.iGain = cfg->iGainTilt; config.fGain = cfg->fGainTilt; config.offset = cfg->offsetTilt; config.panoramicOffset= cfg->panoramicOffsetTilt; s = dmName + ".TILT."; tilt = new Axis(&config, s); // initialize logging file s = nameConversion(dmName); logParameters(s, TRUE); Print(); return OK; } /**************************************************************************** Function : Device::nameConversion Purpose : Converts data manager name to usable filename Inputs : name - string with data manger item name Outputs : returns string of form "portCamera" Notes : This routine is a cludge for the general problem of passing names : to member objects of lower hierarchy. Needs works. ******************************************************************************/ String Device::nameConversion(String& name) { String r; if (name == "PORT.CAMERA") { r = "portCamera"; return r; } if (name == "STBD.CAMERA") { r = "stbdCamera"; return r; } if (name == "PORT.LIGHT") { r = "portLight"; return r; } if (name == "STBD.LIGHT") { r = "stbdLight"; return r; } } /****************************************************************************** Function : Device::Print Purpose : Useful print routine for diagnostics Inputs : None Outputs : None *******************************************************************************/ STATUS Device::Print() { #ifdef NO_PRINT if (iView->print) { dmName.Print(); fprintf(stderr,"Origin (%6.2f,%6.2f,%6.2f)\n", xOrigin, yOrigin, zOrigin); fprintf(stderr,"Setpoint 0x%x\n", setPoint); fprintf(stderr,"Setpoint control sources\n"); fprintf(stderr,"pan/tilt 0x%x\n", setPoint->controlSource); fprintf(stderr,"\n"); } #endif return OK; } /****************************************************************************** Function : Device::logParameters Purpose : Logs all axis and setpoint values Inputs : None Outputs : None *******************************************************************************/ STATUS Device::logParameters(char* name, MBool firstCall) { Flt64 time; char s[200]; String string; string = IVIEW_LOG_DIRECTORY; string += name; string += "Parameters"; if (firstCall) { logMsg("opening file %s\n", string.str); if ((fp = fopen(string.str, "w")) == NULL) { logMsg("error opening file %s\n", string.str); return ERROR; } sprintf(s, "%9.5f %9.5f %9.5f\n", xOrigin, yOrigin, zOrigin); fwrite((char*) s, strlen(s), 1, fp); sprintf(s, "%9.5f %9.5f\n", RADS_TO_DEGS(pan->getOffset()), RADS_TO_DEGS(tilt->getOffset()) ); fwrite((char*) s, strlen(s), 1, fp); sprintf(s, "%9.5f\n", l); fwrite((char*) s, strlen(s), 1, fp); fclose(fp); return OK; } else { logMsg("opening file %s\n", string.str); if ((fp = fopen(string.str, "a")) == NULL) { logMsg("error opening file %s\n", string.str); return ERROR; } Get_Time(&time); sprintf(s, "%f %d %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %d\n", time, setPoint->DeviceMap(), setPoint->positionX(), setPoint->positionY(), setPoint->positionZ(), RADS_TO_DEGS(pan->getPosition()), RADS_TO_DEGS(pan->positionBuffer->Average(100)), RADS_TO_DEGS(tilt->getPosition()), RADS_TO_DEGS(tilt->positionBuffer->Average(100)), 0, 0); fwrite((char*) s, strlen(s), 1, fp); fclose(fp); return OK; } } /****************************************************************************** Function : Device::Update Purpose : Converts setpoint to from fiducial rectangular coordinate frame to : device spherical frame and servos pan and tilt axes. Inputs : None Outputs : Returns ERROR status if there is no comm link to controller or if : the device has no assigned setpoint. *******************************************************************************/ STATUS Device::Update() { // if no assigned set point, then exit if (setPoint == NULL) return ERROR; // convert setpoint from fiducial coordinate frame to device coordinate frame x = setPoint->x - xOrigin; y = setPoint->y - yOrigin; z = setPoint->z - zOrigin; dx = setPoint->dx; dy = setPoint->dy; dz = setPoint->dz; // convert from device rectangular to device spherical coordinate frame f = sqrt(x*x + y*y); fd = sqrt(f*f + z*z - l*l); // focal distance psi = sign(y) * acos(x/f); // pan angle theta = atan((l*f - fd*z) / (fd*f + l*z)); // tilt angle // compute time derivative of device spherical coordinate frame df = (x*dx + y*dy) / f; dpsi = (dy*x - y*dx) / (f*f); dtheta = (z*df - dz*f) / (z*z + f*f); // servo pan/tilt devices to spherical coordinates pan->Update(psi, dpsi, !comLinkError); tilt->Update(theta, dtheta, !comLinkError); return OK; } /****************************************************************************** Function : Device::Move Purpose : Sends direct velocity commands in radians/sec to pan and tilt axes. Inputs : p - pan velocity : t - tilt velocity Outputs : None *******************************************************************************/ STATUS Device::Move(FltN p, FltN t) { pan->Velocity(p); tilt->Velocity(t); return OK; } /****************************************************************************** Function : Device::Position Purpose : Sends direct position commands to pan and tilt axes Inputs : p - pan position : t - tilt position Outputs : None *******************************************************************************/ STATUS Device::Position(FltN p, FltN t) { pan->Position(p); tilt->Position(t); return OK; } /****************************************************************************** Function: Device::Remap Purpose : Remaps this device to new setpoint Inputs : sp - pointer to new setpoint Outputs : None Notes : This device is removed from its previous setpoint and the control : source is informed. This device is then pointed to the new setpoint : and is added to device map of new setpoint. ******************************************************************************/ STATUS Device::Remap(SetPoint* sp) { // remove this device from old setpoint and inform it control source setPoint->Remove(device); // if setpoint has no devices then delete it if (!setPoint->deviceMap) { if (setPoint->controlSource) setPoint->controlSource->setPoint = NULL; delete setPoint; } // add this device to new setpoint setPoint = sp; setPoint->Add(device); return OK; } /****************************************************************************** Function : Device::enableControl Purpose : Enables selected axis for closed loop control Inputs : n - enumerated axes Outputs : None *******************************************************************************/ STATUS Device::enableControl(Int n) { if (n == PAN || n == BOTH) pan->enableControl(); if (n == TILT || n == BOTH) tilt->enableControl(); return OK; } /****************************************************************************** Function : Device::disableControl Purpose : Enables selected axis for closed loop control Inputs : Outputs : *******************************************************************************/ STATUS Device::disableControl(Int n) { if (n == PAN || n == BOTH) pan->disableControl(); if (n == TILT || n == BOTH) tilt->disableControl(); return OK; } /****************************************************************************** Function : Device::Display Purpose : Displays current state of device Inputs : None Outputs : None *******************************************************************************/ void Device::Display() { dmName.Print(); fprintf(stderr,"set point %7.2f %7.2f %7.2f\n", setPoint->x, setPoint->y, setPoint->z); fprintf(stderr,"origin %7.2f %7.2f %7.2f\n", xOrigin, yOrigin, zOrigin); fprintf(stderr,"panoramic offset %7.2f %7.2f\n", pan->getPanoramic(), tilt->getPanoramic()); fprintf(stderr,"pan/tilt %7.2f %7.2f\n", pan->getPosition(), tilt->getPosition()); } /***************************************************************************** Function : Camera::Init Purpose : Common part of Camera constructor Inputs : Outputs : ******************************************************************************/ STATUS Camera::Init(DeviceConfig* cfg) { if (dmName == "PORT.CAMERA") { focus = new Focus(PORT_CAMERA_DM_PREFIX); zoom = new Zoom(PORT_CAMERA_DM_PREFIX); iris = new Iris(PORT_CAMERA_DM_PREFIX); } else { focus = new Focus(STBD_CAMERA_DM_PREFIX); zoom = new Zoom(STBD_CAMERA_DM_PREFIX); iris = new Iris(STBD_CAMERA_DM_PREFIX); } fd = cfg->initialFocalLength; return OK; } /****************************************************************************** Function : Camera::Update Purpose : Update camera settings Inputs : comLinkError - indicates whether there exists communications with : controller Outputs : None Notes : For focus update, if both cameras are mapped together and the focus : has no control source, then use the ******************************************************************************/ STATUS Camera::Update(MBool comLinkError) { Camera::comLinkError = comLinkError; zoom->Update(); iris->Update(); Device::Update(); if ( (setPoint->deviceMap & IviewStbdCamera) && (setPoint->deviceMap & IviewPortCamera) && (!focus->controlSource) ) focus->Update(fd); else focus->Update(); return OK; } /****************************************************************************** Function : Camera::logParameters Purpose : Logs all axis and setpoint values Inputs : None Outputs : None *******************************************************************************/ STATUS Camera::logParameters(char* name, MBool firstCall) { Flt64 time; char s[200]; String string; string = IVIEW_LOG_DIRECTORY; string += name; string += "Parameters"; if (firstCall) { logMsg("opening file %s\n", string.str); if ((fp = fopen(string.str, "w")) == NULL) { logMsg("error opening file %s\n", string.str); return ERROR; } sprintf(s, "%9.5f %9.5f %9.5f\n", xOrigin, yOrigin, zOrigin); fwrite((char*) s, strlen(s), 1, fp); sprintf(s, "%9.5f %9.5f\n", RADS_TO_DEGS(pan->getOffset()), RADS_TO_DEGS(tilt->getOffset()) ); fwrite((char*) s, strlen(s), 1, fp); sprintf(s, "%9.5f\n", l); fwrite((char*) s, strlen(s), 1, fp); fclose(fp); return OK; } else { logMsg("opening file %s\n", string.str); if ((fp = fopen(string.str, "a")) == NULL) { logMsg("error opening file %s\n", string.str); return ERROR; } Get_Time(&time); sprintf(s, "%f %d %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %9.5f %d\n", time, setPoint->DeviceMap(), setPoint->positionX(), setPoint->positionY(), setPoint->positionZ(), RADS_TO_DEGS(pan->getPosition()), RADS_TO_DEGS(pan->positionBuffer->Average(100)), RADS_TO_DEGS(tilt->getPosition()), RADS_TO_DEGS(tilt->positionBuffer->Average(100)), focus->focalDistance(), focus->focusPosition() ); fwrite((char*) s, strlen(s), 1, fp); fclose(fp); return OK; } } /****************************************************************************** Function : Camera::logParameters Purpose : Logs all axis and setpoint values Inputs : None Outputs : None *******************************************************************************/ STATUS Camera::Print() { Device::Print(); fprintf(stderr,"focus 0x%x\n", focus->controlSource); return OK; } /****************************************************************************** Function : Focus::Init Purpose : Common constructor for Focus object Inputs : Outputs : ******************************************************************************/ Void Focus::Init(String& dmName) { focus = DEFAULT_FOCUS_SETPOINT; setPoint = (Int16) (FOCUS_FULL_SCALE * focus); fd = DEFAULT_FOCAL_DISTANCE; // load calibration table String string = IVIEW_DIRECTORY; if (dmName == PORT_CAMERA_DM_PREFIX) string += "portCameraFocusTable"; else string += "stbdCameraFocusTable"; focalLengthTable = new CalibrationTable(string); controlSource = NULL; // create data manager group group = new DmGroup(); // initialize pan Dm items string = dmName + INTEGRATED_FOCUS_SET_POINT_DM; setPointDmi = new DmInt16Object(string.str); setPointDmi->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider); setPointDmi->write(&setPoint); string = dmName + "FOCAL_DISTANCE"; focalDistanceDmi = new DmFlt32Object(string.str); focalDistanceDmi->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider); focalDistanceDmi->write(&fd); string = dmName + FOCUS_POSITION_DM; positionDmi = new DmInt16Object(string.str); positionDmi->startConsume(DM_ASYNC); positionDmi->read(&position); group->addItem(positionDmi); } /****************************************************************************** Function : Focus::Update Purpose : Update the focus DM item Inputs : d - focal distance Outputs : None Notes : Reads in focus position from camera; writes new focus value. *******************************************************************************/ STATUS Focus::Update() { group->getChanges(); // Read in focus position if (group->itemChanged(positionDmi)) positionDmi->read(&position); // Write new setpoint position fd = Focus_to_Focal_Distance(focus); setPoint = (Int16) (FOCUS_FULL_SCALE * focus); // update DM item if new value if (setPoint != setPointLast) { focalDistanceDmi->write(&fd); setPointDmi->write(&setPoint); setPointLast = setPoint; } return OK; } /****************************************************************************** Function : Focus::Focal_Distance_to_Focus Purpose : Computes focus position from focal point Inputs : fd - focal distance Outputs : focus in range [0,1] Notes : ******************************************************************************/ Flt32 Focus::Focal_Distance_to_Focus(Flt32 fd) { return focalLengthTable->LinearPiecewiseInterpolationY(fd); } /****************************************************************************** Function : Focus::Focus_to_Focal_Distance Purpose : Computes focal distant from focus in range [0,1] Inputs : x - focus in [0,1] Outputs : focal distance Notes : ******************************************************************************/ Flt32 Focus::Focus_to_Focal_Distance(Flt32 x) { return focalLengthTable->LinearPiecewiseInterpolationX(x); } /****************************************************************************** Function : Focus::Remap Purpose : Remaps a focus control source to this focus Inputs : controlSource - pointer to new control source Outputs : None ******************************************************************************/ STATUS Focus::Remap(CameraControlSource* controlSrc) { if (controlSource == controlSrc) return ERROR; // remove previous control source from focus if (controlSource != NULL) { controlSource->camera = NULL; controlSource->refDevice = IviewNoDevice; controlSource->deviceMap = 0; controlSource->focusEnable = FALSE; controlSource->Update(); } // add new control source to camera controlSource = controlSrc; return OK; } /****************************************************************************** Function : Focus::Log Purpose : Logs focus positions to file Inputs : Outputs : ******************************************************************************/ STATUS Focus::Log(FILE* fp, String& string) { char s[20]; String str = string; str += FOCUS_POSITION_DM; sprintf(s, " %f\n", position); str += s; str.Write(fp); return OK; } /****************************************************************************** Function : Zoom::Init Purpose : Common constructor for Zoom object Inputs : Outputs : ******************************************************************************/ Void Zoom::Init(String& dmName) { position = 0; rate = rateLast = 0; setPoint = setPointLast = ZOOM_DEFAULT_SETPOINT; mode = POSITION_MODE; // create data manager group group = new DmGroup(); // initialize pan Dm items String string = dmName + ZOOM_CONTROL_MODE_DM; modeDmi = new DmNat16Object(string.str); modeDmi->startProvide(DM_ASYNC, ExclusiveProvider); modeDmi->write(&mode); string = dmName + ZOOM_SET_POINT_DM; setPointDmi = new DmInt16Object(string.str); setPointDmi->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider); setPointDmi->write(&setPoint); string = dmName + ZOOM_POSITION_DM; positionDmi = new DmInt16Object(string.str); positionDmi->startConsume(DM_ASYNC); group->addItem(positionDmi); positionDmi->read(&position); } /****************************************************************************** Function : Zoom::Rate Purpose : Send a zoom rate command to camera Inputs : None Outputs : None *******************************************************************************/ STATUS Zoom::Rate(Flt32 z) { rate = (Int16) (ZOOM_RATE_FULL_SCALE * z); if (rate > ZOOM_MAX_RATE) rate = ZOOM_MAX_RATE; if (rate < ZOOM_MIN_RATE) rate = ZOOM_MIN_RATE; mode = ZOOM_RATE_MODE; return OK; } /****************************************************************************** Function : Zoom::Position Purpose : Send a zoom posiiton command to camera Inputs : None Outputs : None *******************************************************************************/ STATUS Zoom::Position(Flt32 z) { setPoint = (Int16) (ZOOM_FULL_SCALE * z); if (setPoint > ZOOM_MAX_SETPOINT) setPoint = ZOOM_MAX_SETPOINT; if (setPoint < ZOOM_MIN_SETPOINT) setPoint = ZOOM_MIN_SETPOINT; mode = ZOOM_POSITION_MODE; return OK; } /****************************************************************************** Function : Zoom::Update Purpose : Update the zoom DM item Inputs : None Outputs : None *******************************************************************************/ STATUS Zoom::Update() { group->getChanges(); // Read in zoom position if (group->itemChanged(positionDmi)) positionDmi->read(&position); // if rate mode, set camera mode, send rate and reset back to position mode if (mode == ZOOM_RATE_MODE) { if (rate != rateLast) { modeDmi->write(&mode); setPointDmi->write(&rate); rateLast = rate; } } else { if (setPoint != setPointLast) { modeDmi->write(&mode); setPointDmi->write(&setPoint); setPointLast = setPoint; } } return OK; } /****************************************************************************** Function : Iris::Init Purpose : Common constructor for Iris object Inputs : Outputs : ******************************************************************************/ Void Iris::Init(String& dmName) { setPoint = IRIS_DEFAULT_SETPOINT; rate = 0; // initialize pan Dm items String string = dmName + IRIS_VALUE_DM; setPointDmi = new DmInt16Object(string.str); setPointDmi->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider); // create data manager group group = new DmGroup(); string = dmName + IRIS_MODE_DM; modeDmi = new DmBooleanObject(string.str); modeDmi->startConsume(DM_STATIC); group->addItem(modeDmi); modeDmi->read(&mode); Update(); } /****************************************************************************** Function : Iris::Update Purpose : Update the iris DM items Inputs : None Outputs : None *******************************************************************************/ STATUS Iris::Update() { // update iris mode group->getChanges(); if (group->itemChanged(modeDmi)) modeDmi->read(&mode); // integrate rate input setPoint += IRIS_RATE_CONSTANT * rate; // clip setpoint if (setPoint > IRIS_MAX_SETPOINT) setPoint = IRIS_MAX_SETPOINT; if (setPoint < IRIS_MIN_SETPOINT) setPoint = IRIS_MIN_SETPOINT; sp = (Int16) (IRIS_FULL_SCALE * setPoint); // update DM item if new value if (sp != spLast) { setPointDmi->write(&sp); spLast = sp; } return OK; } /****************************************************************************** Function : Light::Init Purpose : Common Light constructor Inputs : Outputs : *******************************************************************************/ Light::Init(DeviceConfig* cfg) { power = 0; // create data manager group group = new DmGroup(); String string(dmName); // initialize pan Dm items string += POWER_DM; powerDmi = new DmInt16Object(string.str); powerDmi->startConsume(); powerDmi->read(&power); } /****************************************************************************** Function : Light::Update Purpose : Updates Light object Inputs : Outputs : *******************************************************************************/ STATUS Light::Update(MBool comLinkError) { Light::comLinkError = comLinkError; Device::Update(); return OK; } /****************************************************************************** Function : Controller::Controller Purpose : Constructor for Controller class Inputs : loc - controller location, either "PORT" or "STBD" : cameraDevice - indicates which camera : cameraConfig - device configuration struct for camera : cameraSetPoint - initial setpoint for camera : lightDevice - indicates which light : lightConfig - device configuration struct for light : lightSetPoint - initial setpoint for light Outputs : None *******************************************************************************/ Controller::Controller(char* loc, IviewDevice cameraDevice, DeviceConfig* cameraConfig, IviewDevice lightDevice, DeviceConfig* lightConfig) : location(loc) { standby = TRUE; comLinkError = TRUE; String string = location; string += ".CAMERA"; camera = new Camera(cameraDevice, string, cameraConfig); string = location; string += ".LIGHT"; light = new Light(lightDevice, string, lightConfig); velocityCmd[0] = 0; velocityCmd[1] = 0; velocityCmd[2] = 0; velocityCmd[3] = 0; string = PAN_TILT_DM_PREFIX; string += location; string += ".VELOCITY_ALL_CMD"; velocityCmdDmi = new DmInt16Object(string.str, 4); velocityCmdDmi->startProvide(IVIEW_DM_PERIOD, ExclusiveProvider); group = new DmGroup(); string = PAN_TILT_DM_PREFIX; string += location; string += ".COM_LINK_ERROR"; comLinkErrorDmi = new DmBooleanObject(string.str); comLinkErrorDmi->startConsume(IVIEW_DM_PERIOD); group->addItem(comLinkErrorDmi); } /****************************************************************************** Function : Controller::~Controller Purpose : Controller class destructor Inputs : None Outputs : None *******************************************************************************/ Controller::~Controller() { delete group; delete camera; delete light; delete velocityCmdDmi; delete comLinkErrorDmi; } /****************************************************************************** Function : Controller::Update Purpose : Updates all devices in controller Inputs : None Outputs : None *******************************************************************************/ STATUS Controller::Update() { // find out if controllers are running group->getChanges(); if (group->itemChanged(comLinkErrorDmi)) comLinkErrorDmi->read(&comLinkError); if (!standby) { camera->Update(comLinkError); light->Update(comLinkError); Velocity(); return OK; } return ERROR; } /****************************************************************************** Function : Controller::Velocity Purpose : Sends all controller axes out at once Inputs : None Outputs : None *******************************************************************************/ STATUS Controller::Velocity() { velocityCmd[0] = light->pan->VelocityCmd(); velocityCmd[1] = light->tilt->VelocityCmd(); velocityCmd[2] = camera->pan->VelocityCmd(); velocityCmd[3] = camera->tilt->VelocityCmd(); velocityCmdDmi->write(velocityCmd); return OK; } /***************************************************************************** Function : Controller::Log Purpose : Logs buffer data for each device axis Inputs : None Outputs : None ******************************************************************************/ STATUS Controller::Log() { Standby(); camera->Log(); light->Log(); Resume(); return OK; } /****************************************************************************** Function : Controller::logParameters Purpose : logs parameters for each device Inputs : None Outputs : None *******************************************************************************/ STATUS Controller::logParameters() { Standby(); if (location == "PORT") { camera->logParameters("portCamera"); light->logParameters("portLight"); } else { camera->logParameters("stbdCamera"); light->logParameters("stbdLight"); } Resume(); return OK; }