/****************************************************************************/ /* Copyright (c) 2015 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : EZ17.cc */ /* Author : Henthorn */ /* Project : i2MAP */ /* Version : 1.0 */ /* Created : 06/01/2015 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include "EZ17.h" #include "Syslog.h" #define EZ17_CMD_PREFIX "/1V80m90" // Standard velocity of 80, torque of 90 #define EZ17_MAX_STEPS 660 #define EZ17_HOME_POS "A660" #define EZ17_F56_POS "A250" /* CLASS EZ17 DESCRIPTION EZ17 interface AUTHOR Henthorn */ EZ17::EZ17(int fd) // File descriptor to UDP socket connected to EZ17 : _fd(fd), _home(False), _position(0) { Syslog::write("EZ17::EZ17() - comms on %d", _fd); } EZ17::~EZ17() {} // Useful when the ez17 has been power-cycled // void EZ17::reset() { _home = False; _position = 0; } // Blocking wait. Returns when move is complete or // when the move timed-out, whichever happens first. // Not that useful in real situations, but nice for testing. // int EZ17::wait_for_move(unsigned int timeout_secs) { Boolean debug = True; if (timeout_secs > 120) timeout_secs = 120; int result = 1; // Loop until the current position is equal to the // previous position. Or when the move times out. // int pos = -1, old_pos = 0; for (int i = 0; i < timeout_secs; i++) { pos = read_position(); if (pos != old_pos) { old_pos = pos; dprintf("EZ17::wait_for_move() - position is %d", pos); } else { dprintf("EZ17::wait_for_move() - Done! Completed in %d seconds", i); result = 0; break; // Position hasn't changed - done! } sleep(1); } return result; } // Helper function inserts standard prefix and postfix // characters to a command. Resulting string is stashed // in _outbuf. // const char* EZ17::build_motion_cmd(const char* ez_cmd) { sprintf(_outbuf, "%s%sR\r", EZ17_CMD_PREFIX, ez_cmd); return _outbuf; } // Issue the 'Home' command // int EZ17::home() { Boolean debug = True; build_motion_cmd("Z"); send_msg(); return (read_msg() > 0); } int EZ17::last_known_position() { return _position; } #define EZ17_MIN_BYTES 8 // Request the current motor position from the stepper controller. // Parse the position out of the returned packet and store it. // Return -1 on any type of error, otherwise return the position. // int EZ17::read_position(char ntries) { Boolean debug = True; int nbytes = read_msg(); // Flush the controller buffer // Multiple attempts are often necessary to receive a valid response // for (char i = 0; i < ntries; i++) { send_msg("/1?8"); nbytes = read_msg(); // Got to be a minimum of 8 bytes // if (nbytes >= 8) { // Ensure the packet has the correct structure // if (_inbuf[3] == '`' || _inbuf[3] == '@') { _inbuf[nbytes-3] = '\0'; sscanf(_inbuf+4, "%d", &_position); dprintf("EZ17::read_position() - position is %d", _position); return _position; } } else { dprintf("EZ17::read_position() - %d bytes read from EZ17, expecting %d", nbytes, EZ17_MIN_BYTES); } } return -1; } // Go to a named position (like F-stop 5.6, home, etc.) // int EZ17::goto_pos(EZ17_Pos pos) { Boolean debug = True; switch (pos) { case F56: dprintf("EZ17::goto_pos() - F56"); build_motion_cmd(EZ17_F56_POS); send_msg(); return (read_msg() > 0); break; case Home: dprintf("EZ17::goto_pos() - Home"); build_motion_cmd(EZ17_HOME_POS); send_msg(); return (read_msg() > 0); break; } return 0; } // Go to a specific position expressed in steps from 0 // (i.e., absolute position) // int EZ17::goto_steps(int steps) { if (steps <= EZ17_MAX_STEPS && steps >= 0) { char cmd[10]; sprintf(cmd, "A%d", steps); build_motion_cmd(cmd);send_msg(); return (read_msg() > 0); } return 0; } // Send a message to the controller. // If msg is NULL, send the string already in _outbuf. // Otherwise, the contents of msg must be a complete and valid // EZ17 command, including the R character if applicable. // Examples: // send_msg(); // Sends the message contained in _outbuf // send_msg("/1?8"); // This is a position query // send_msg("/1A20R"); // Moves to motor to absolute step 20 // void EZ17::send_msg(const char *msg) { Boolean debug = False; // If there, place the msg string into _outbuf // if (msg) { sprintf(_outbuf, "%s\r", msg); } if (!strstr(_outbuf, "\r")) strcat(_outbuf, "\r"); int cl = strlen(_outbuf); dprintf("EZ17::send_msg() - sending %d bytes: '%s'", cl, _outbuf); // Send the message string // int sl = send(_fd, _outbuf, cl, 0); if (sl < 0 || sl != cl) { dprintf("EZ17::send_msg() - There was an issue sending %s: %d", msg, sl); perror(NULL); } } // Save the position so that the EZ17 remembers where it is next time // it powers up. void EZ17::save_position() { Boolean debug = True; read_position(); sprintf(_outbuf, "/1s0V80m90z%dR", _position); dprintf("EZ17::save_position() - Saving the position: %d", _position); send_msg(); } // Return 0 on error. Else 1. // int EZ17::set_position(int steps) { if (steps >= 0 && steps <= 660) { sprintf(_outbuf, "/1z%dR", steps); send_msg(); read_position(); int pos = read_position(); return (pos == steps); } return 0; } // Read the bytes, if any, in the socket buffer. Return the number of // bytes read and place the data in the _inbuf. // int EZ17::read_msg() { Boolean debug = False; memset(_inbuf, '\0', sizeof(_inbuf)); int sl = 0, nbytes = 0; while ((sl = recv(_fd, _inbuf+nbytes, sizeof(_inbuf)-nbytes, 0)) > 0) nbytes += sl; if (debug) for (int i = 0; i < nbytes; i++) printf("%d:%02x ", i, _inbuf[i]); dprintf("\n"); return nbytes; }