Line data Source code
1 : /** \file pi335Ctrl.hpp
2 : * \brief The MagAO-X XXXXXX header file
3 : *
4 : * \ingroup pi335Ctrl_files
5 : */
6 :
7 : #ifndef pi335Ctrl_hpp
8 : #define pi335Ctrl_hpp
9 :
10 : #include "../../libMagAOX/libMagAOX.hpp" //Note this is included on command line to trigger pch
11 : #include "../../magaox_git_version.h"
12 :
13 : /** \defgroup pi335Ctrl
14 : * \brief The MagAO-X application to interface with a PI-335 Controller
15 : *
16 : * <a href="..//apps_html/page_module_pi335Ctrl.html">Application Documentation</a>
17 : *
18 : * \ingroup apps
19 : *
20 : */
21 :
22 : /** \defgroup pi335Ctrl_files
23 : * \ingroup pi335Ctrl
24 : */
25 :
26 : namespace MagAOX
27 : {
28 : namespace app
29 : {
30 :
31 : /// The MagAO-X PI 335 Controller Interface
32 : /**
33 : * \ingroup pi335Ctrl
34 : */
35 : class pi335Ctrl : public MagAOXApp<true>, public tty::usbDevice, public dev::ioDevice, public dev::dm<pi335Ctrl, float>, public dev::shmimMonitor<pi335Ctrl>, public dev::telemeter<pi335Ctrl>
36 : {
37 :
38 : // Give the test harness access.
39 : friend class pi335Ctrl_test;
40 :
41 : friend class dev::dm<pi335Ctrl, float>;
42 :
43 : typedef dev::dm<pi335Ctrl, float> dmT;
44 :
45 : friend class dev::shmimMonitor<pi335Ctrl>;
46 :
47 : typedef dev::shmimMonitor<pi335Ctrl> shmimMonitorT;
48 :
49 : friend class dev::telemeter<pi335Ctrl>;
50 :
51 : typedef dev::telemeter<pi335Ctrl> telemeterT;
52 :
53 : protected:
54 : /** \name Configurable Parameters
55 : *@{
56 : */
57 :
58 : float m_posTol{0.05}; ///< The tolerance for reporting a raw position rather than the setpoint.
59 :
60 : float m_homePos1{17.5}; ///< Home position of axis 1. Default is 17.5
61 : float m_homePos2{17.5}; ///< Home position of axis 2. Default is 17.5
62 : float m_homePos3{0.0}; ///< Home position of axis 2. Default is 17.5
63 :
64 : ///@}
65 :
66 : private:
67 : std::string m_ctrl; ///< The controller connected.
68 : std::string m_stage; ///< The stage connected.
69 : int m_naxes{2}; ///< The number of axes, default is 2. Max is 3.
70 :
71 : bool m_pos_3_sent{false};
72 :
73 : bool m_actuallyATZ{true};
74 :
75 : protected:
76 : float m_min1{0}; ///< The minimum value for axis 1
77 : float m_max1{35}; ///< The maximum value for axis 1
78 :
79 : float m_min2{0}; ///< The minimum value for axis 2
80 : float m_max2{35}; ///< The maximum value for axis 2
81 :
82 : float m_min3{0}; ///< The minimum value for axis 3
83 : float m_max3{0}; ///< The maximum value for axis 3
84 :
85 : double m_homingStart{0};
86 : int m_homingState{0};
87 :
88 : int m_servoState{0};
89 :
90 : float m_pos1Set{0};
91 : float m_pos1{0};
92 :
93 : float m_pos2Set{0};
94 : float m_pos2{0};
95 :
96 : float m_pos3Set{0};
97 : float m_pos3{0};
98 :
99 : float m_sva1{0};
100 : float m_sva2{0};
101 : float m_sva3{0};
102 :
103 : public:
104 : /// Default c'tor.
105 : pi335Ctrl();
106 :
107 : /// D'tor, declared and defined for noexcept.
108 0 : ~pi335Ctrl() noexcept
109 0 : {
110 0 : }
111 :
112 : virtual void setupConfig();
113 :
114 : /// Implementation of loadConfig logic, separated for testing.
115 : /** This is called by loadConfig().
116 : */
117 : int loadConfigImpl(mx::app::appConfigurator &_config /**< [in] an application configuration from which to load values*/);
118 :
119 : virtual void loadConfig();
120 :
121 : /// Startup function
122 : /**
123 : *
124 : */
125 : virtual int appStartup();
126 :
127 : /// Implementation of the FSM for pi335Ctrl.
128 : /**
129 : * \returns 0 on no critical error
130 : * \returns -1 on an error requiring shutdown
131 : */
132 : virtual int appLogic();
133 :
134 : /// Shutdown the app.
135 : /**
136 : *
137 : */
138 : virtual int appShutdown();
139 :
140 : /// Test the connection to the device
141 : /** Uses the *IDN? query
142 : *
143 : * \returns 0 if the E227 is found
144 : * \returns -1 on an error
145 : */
146 : int testConnection();
147 :
148 : int initDM();
149 :
150 : /// Start the homing procedure
151 : /** Checks that servos are off (the manual says this doesn't matter),
152 : * and then begins homing by calling 'home_1()'.
153 : * Sets FSM state to homing.
154 : *
155 : * \returns 0 on success (from 'home_1()')
156 : * \returns -1 on error
157 : */
158 : int home();
159 :
160 : /// Get the status of homing on an axiz
161 : /** Homing is also autozero, ATZ. This uses the ATZ? query to
162 : * check if the autozero procedure has completed on the given axis.
163 : *
164 : * \returns 0 if ATZ not complete
165 : * \returns 1 if ATZ complete
166 : * \returns -1 on an error
167 : */
168 : int homeState(int axis /**< [in] the axis to check, 0 or 1 */);
169 :
170 : /// Begin homing (ATZ) axis 1
171 : /** Repeats check that servos are off (the manual says this doesn't matter)
172 : * and checks that 'm_homingStatte' is 0. Then sends 'ATZ 1 NaN'.
173 : *
174 : * \returns 0 on success
175 : * \returns -1 on error
176 : */
177 : int home_1();
178 :
179 : /// Begin homing (ATZ) axis 2
180 : /** Repeats check that servos are off (the manual says this doesn't matter)
181 : * and checks that 'm_homingStatte' is 1. Then sends 'ATZ 2 NaN'.
182 : *
183 : * \returns 0 on success
184 : * \returns -1 on error
185 : */
186 : int home_2();
187 :
188 : /// Begin homing (ATZ) axis 3
189 : /** Repeats check that servos are off (the manual says this doesn't matter)
190 : * and checks that 'm_homingStatte' is 1. Then sends 'ATZ 3 NaN'.
191 : *
192 : * \returns 0 on success
193 : * \returns -1 on error
194 : */
195 : int home_3();
196 :
197 : int finishInit();
198 :
199 : /// Zero all commands on the DM
200 : /** This does not update the shared memory buffer.
201 : *
202 : * \returns 0 on success
203 : * \returns -1 on error
204 : */
205 : int zeroDM();
206 :
207 : /// Send a command to the DM
208 : /** This is called by the shmim monitoring thread in response to a semaphore trigger.
209 : *
210 : * \returns 0 on success
211 : * \returns -1 on error
212 : */
213 : int commandDM(void *curr_src);
214 :
215 : /// Release the DM, making it safe to turn off power.
216 : /** The application will be state READY at the conclusion of this.
217 : *
218 : * \returns 0 on success
219 : * \returns -1 on error
220 : */
221 : int releaseDM();
222 :
223 : int setCom(const std::string &com);
224 :
225 : int setCom(const std::string &com,
226 : int axis);
227 :
228 : int setCom(const std::string &com,
229 : int axis,
230 : const std::string &arg);
231 :
232 : int getCom(std::string &resp,
233 : const std::string &com,
234 : int axis);
235 :
236 : int getPos(float &pos,
237 : int n);
238 :
239 : /// Get the open loop control value
240 : int getSva(float &sva,
241 : int n);
242 :
243 : /// Update the flat command and propagate
244 : /** Writes the new desired position to the flat shmim, which is then propagated via the dmcomb
245 : */
246 : int updateFlat(float absPos1, ///< [in] The new position of axis 1
247 : float absPos2, ///< [in] The new position of axis 2
248 : float absPos3 ///< [in] The new position of axis 3
249 : );
250 :
251 : /// Send command to device to move axis 1
252 : int move_1(float absPos);
253 :
254 : /// Send command to device to move axis 2
255 : int move_2(float absPos);
256 :
257 : /// Send command to device to move axis 3
258 : int move_3(float absPos);
259 :
260 : protected:
261 : // declare our properties
262 : pcf::IndiProperty m_indiP_pos1;
263 : pcf::IndiProperty m_indiP_pos2;
264 : pcf::IndiProperty m_indiP_pos3;
265 :
266 : public:
267 0 : INDI_NEWCALLBACK_DECL(pi335Ctrl, m_indiP_pos1);
268 0 : INDI_NEWCALLBACK_DECL(pi335Ctrl, m_indiP_pos2);
269 0 : INDI_NEWCALLBACK_DECL(pi335Ctrl, m_indiP_pos3);
270 :
271 : /** \name Telemeter Interface
272 : * @{
273 : */
274 :
275 : int checkRecordTimes();
276 :
277 : int recordTelem(const telem_pi335 *);
278 :
279 : int recordPI335(bool force = false);
280 :
281 : ///@}
282 : };
283 :
284 0 : pi335Ctrl::pi335Ctrl() : MagAOXApp(MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED)
285 : {
286 0 : m_powerMgtEnabled = true;
287 :
288 0 : return;
289 0 : }
290 :
291 0 : void pi335Ctrl::setupConfig()
292 : {
293 0 : dev::ioDevice::setupConfig(config);
294 0 : tty::usbDevice::setupConfig(config);
295 :
296 0 : DM_SETUP_CONFIG(config);
297 :
298 0 : TELEMETER_SETUP_CONFIG(config);
299 :
300 0 : config.add("stage.naxes", "", "stage.naxes", argType::Required, "stage", "naxes", false, "int", "Number of axes. Default is 2. Max is 3.");
301 :
302 0 : config.add("stage.homePos1", "", "stage.homePos1", argType::Required, "stage", "homePos1", false, "float", "Home position of axis 1. Default is 17.5.");
303 0 : config.add("stage.homePos2", "", "stage.homePos2", argType::Required, "stage", "homePos2", false, "float", "Home position of axis 2. Default is 17.5.");
304 0 : config.add("stage.homePos3", "", "stage.homePos3", argType::Required, "stage", "homePos3", false, "float", "Home position of axis 3. Default is 17.5.");
305 :
306 0 : config.add("dm.calibRelDir", "", "dm.calibRelDir", argType::Required, "dm", "calibRelDir", false, "string", "Used to find the default calib directory.");
307 : }
308 :
309 0 : int pi335Ctrl::loadConfigImpl(mx::app::appConfigurator &_config)
310 : {
311 :
312 0 : this->m_baudRate = B115200; // default for E727 controller. Will be overridden by any config setting.
313 :
314 0 : int rv = tty::usbDevice::loadConfig(_config);
315 :
316 0 : if (rv != 0 && rv != TTY_E_NODEVNAMES && rv != TTY_E_DEVNOTFOUND)
317 : {
318 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
319 : }
320 :
321 0 : dev::ioDevice::loadConfig(_config);
322 :
323 0 : m_calibRelDir = "ttmpupil";
324 0 : config(m_calibRelDir, "dm.calibRelDir");
325 :
326 0 : DM_LOAD_CONFIG(_config);
327 :
328 0 : config(m_naxes, "stage.naxes");
329 0 : config(m_homePos1, "stage.homePos1");
330 0 : config(m_homePos2, "stage.homePos2");
331 0 : config(m_homePos3, "stage.homePos3");
332 :
333 0 : TELEMETER_LOAD_CONFIG(_config);
334 :
335 0 : return 0;
336 : }
337 :
338 0 : void pi335Ctrl::loadConfig()
339 : {
340 0 : loadConfigImpl(config);
341 0 : }
342 :
343 0 : int pi335Ctrl::appStartup()
344 : {
345 0 : if (state() == stateCodes::UNINITIALIZED)
346 : {
347 0 : log<text_log>("In appStartup but in state UNINITIALIZED.", logPrio::LOG_CRITICAL);
348 0 : return -1;
349 : }
350 :
351 : ///\todo promote usbDevice to dev:: and make this part of its appStartup
352 : // Get the USB device if it's in udev
353 0 : if (m_deviceName != "")
354 : {
355 0 : log<text_log>(std::string("USB Device ") + m_idVendor + ":" + m_idProduct + ":" + m_serial + " found in udev as " + m_deviceName);
356 : }
357 :
358 0 : DM_APP_STARTUP;
359 0 : SHMIMMONITOR_APP_STARTUP;
360 :
361 : // set up the INDI properties
362 0 : REG_INDI_NEWPROP(m_indiP_pos1, "pos_1", pcf::IndiProperty::Number);
363 0 : m_indiP_pos1.add(pcf::IndiElement("current"));
364 0 : m_indiP_pos1.add(pcf::IndiElement("target"));
365 0 : m_indiP_pos1["current"] = -99999;
366 0 : m_indiP_pos1["target"] = -99999;
367 :
368 0 : REG_INDI_NEWPROP(m_indiP_pos2, "pos_2", pcf::IndiProperty::Number);
369 0 : m_indiP_pos2.add(pcf::IndiElement("current"));
370 0 : m_indiP_pos2.add(pcf::IndiElement("target"));
371 0 : m_indiP_pos2["current"] = -99999;
372 0 : m_indiP_pos2["target"] = -99999;
373 :
374 : // Note: 3rd axis added in testConnection if it's found
375 :
376 0 : TELEMETER_APP_STARTUP;
377 :
378 0 : return 0;
379 : }
380 :
381 0 : int pi335Ctrl::appLogic()
382 : {
383 0 : DM_APP_LOGIC;
384 0 : SHMIMMONITOR_APP_LOGIC;
385 :
386 0 : TELEMETER_APP_LOGIC;
387 :
388 0 : if (state() == stateCodes::POWERON)
389 : {
390 0 : if (!powerOnWaitElapsed())
391 : {
392 0 : return 0;
393 : }
394 : else
395 : {
396 : ///\todo promote usbDevice to dev:: and make this part of its appStartup
397 : // Get the USB device if it's in udev
398 0 : if (m_deviceName == "")
399 : {
400 0 : state(stateCodes::NODEVICE);
401 : }
402 : else
403 : {
404 0 : state(stateCodes::NOTCONNECTED);
405 : }
406 : }
407 : }
408 :
409 : ///\todo promote usbDevice to dev:: and make this part of its appLogic
410 0 : if (state() == stateCodes::NODEVICE)
411 : {
412 0 : int rv = tty::usbDevice::getDeviceName();
413 0 : if (rv < 0 && rv != TTY_E_DEVNOTFOUND && rv != TTY_E_NODEVNAMES)
414 : {
415 0 : state(stateCodes::FAILURE);
416 0 : if (!stateLogged())
417 : {
418 0 : log<software_critical>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
419 : }
420 0 : return -1;
421 : }
422 :
423 0 : if (rv == TTY_E_DEVNOTFOUND || rv == TTY_E_NODEVNAMES)
424 : {
425 0 : state(stateCodes::NODEVICE);
426 :
427 0 : if (!stateLogged())
428 : {
429 0 : log<text_log>(std::string("USB Device ") + m_idVendor + ":" + m_idProduct + ":" + m_serial + " not found in udev");
430 : }
431 0 : return 0;
432 : }
433 : else
434 : {
435 0 : state(stateCodes::NOTCONNECTED);
436 0 : if (!stateLogged())
437 : {
438 0 : log<text_log>(std::string("USB Device ") + m_idVendor + ":" + m_idProduct + ":" + m_serial + " found in udev as " + m_deviceName);
439 : }
440 : }
441 : }
442 :
443 0 : if (state() == stateCodes::NOTCONNECTED)
444 : {
445 : int rv;
446 : { // scope for elPriv
447 0 : elevatedPrivileges elPriv(this);
448 0 : rv = connect();
449 0 : }
450 :
451 0 : if (rv < 0)
452 : {
453 0 : int nrv = tty::usbDevice::getDeviceName();
454 0 : if (nrv < 0 && nrv != TTY_E_DEVNOTFOUND && nrv != TTY_E_NODEVNAMES)
455 : {
456 0 : state(stateCodes::FAILURE);
457 0 : if (!stateLogged())
458 0 : log<software_critical>({__FILE__, __LINE__, nrv, tty::ttyErrorString(nrv)});
459 0 : return -1;
460 : }
461 :
462 0 : if (nrv == TTY_E_DEVNOTFOUND || nrv == TTY_E_NODEVNAMES)
463 : {
464 0 : state(stateCodes::NODEVICE);
465 :
466 0 : if (!stateLogged())
467 : {
468 0 : std::stringstream logs;
469 0 : logs << "USB Device " << m_idVendor << ":" << m_idProduct << ":" << m_serial << " no longer found in udev";
470 0 : log<text_log>(logs.str());
471 0 : }
472 0 : return 0;
473 : }
474 :
475 : // if connect failed, and there is a device, then we have some other problem.
476 0 : state(stateCodes::FAILURE);
477 0 : if (!stateLogged())
478 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
479 0 : return -1;
480 : }
481 :
482 0 : if (testConnection() == 0)
483 : {
484 0 : state(stateCodes::CONNECTED);
485 : }
486 : else
487 : {
488 0 : return 0;
489 : }
490 : }
491 :
492 0 : if (state() == stateCodes::CONNECTED)
493 : {
494 0 : state(stateCodes::NOTHOMED);
495 : }
496 :
497 0 : if (state() == stateCodes::HOMING)
498 : {
499 0 : int ax = m_homingState + 1;
500 :
501 0 : int atz = homeState(ax);
502 :
503 0 : if (!(atz == 0 || atz == 1))
504 : {
505 0 : state(stateCodes::ERROR);
506 0 : log<software_error, -1>({__FILE__, __LINE__, "error getting ATZ? home state."});
507 : }
508 :
509 0 : if (atz == 1)
510 : {
511 0 : ++m_homingState;
512 :
513 0 : if (m_homingState == 1) // x complete
514 : {
515 0 : home_2();
516 : }
517 0 : else if (m_homingState == 2 && m_naxes == 2) // y complete, done
518 : {
519 0 : finishInit();
520 : }
521 0 : else if (m_homingState == 2 && m_naxes == 3) // y complete
522 : {
523 0 : home_3();
524 : }
525 0 : else if (m_homingState > 2)
526 : {
527 0 : finishInit();
528 : }
529 : }
530 : }
531 :
532 0 : if (state() == stateCodes::READY || state() == stateCodes::OPERATING)
533 : {
534 0 : if (m_flatSet)
535 0 : state(stateCodes::OPERATING);
536 : else
537 0 : state(stateCodes::READY);
538 : }
539 :
540 0 : if (state() == stateCodes::READY)
541 : {
542 : float pos1;
543 : float sva1;
544 : float pos2;
545 : float sva2;
546 :
547 :
548 : // Get a lock if we can
549 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
550 :
551 0 : if (getPos(pos1, 1) < 0)
552 : {
553 0 : log<software_error>({__FILE__, __LINE__});
554 0 : state(stateCodes::ERROR);
555 0 : return 0;
556 : }
557 :
558 0 : lock.unlock();
559 0 : mx::sys::milliSleep(1); // Put this thread to sleep to make sure other thread gets a lock
560 :
561 0 : m_pos1 = pos1;
562 0 : if (fabs(m_pos1Set - m_pos1) > m_posTol)
563 : {
564 0 : updateIfChanged(m_indiP_pos1, "current", m_pos1, INDI_BUSY);
565 : }
566 : else
567 : {
568 0 : updateIfChanged(m_indiP_pos1, "current", m_pos1Set, INDI_IDLE);
569 : }
570 :
571 0 : lock.lock();
572 0 : if (getSva(sva1, 1) < 0)
573 : {
574 0 : log<software_error>({__FILE__, __LINE__});
575 0 : state(stateCodes::ERROR);
576 0 : return 0;
577 : }
578 0 : m_sva1 = sva1;
579 :
580 0 : lock.unlock();
581 0 : mx::sys::milliSleep(1); // Put this thread to sleep to make sure other thread gets a lock
582 :
583 0 : lock.lock();
584 0 : if (getPos(pos2, 2) < 0)
585 : {
586 0 : log<software_error>({__FILE__, __LINE__});
587 0 : state(stateCodes::ERROR);
588 0 : return 0;
589 : }
590 0 : lock.unlock();
591 0 : mx::sys::milliSleep(1); // Put this thread to sleep to make sure other thread gets a lock
592 :
593 0 : m_pos2 = pos2;
594 0 : if (fabs(m_pos2Set - m_pos2) > m_posTol) // sva2 != m_sva2)
595 : {
596 0 : updateIfChanged(m_indiP_pos2, "current", m_pos2, INDI_BUSY);
597 : }
598 : else
599 : {
600 0 : updateIfChanged(m_indiP_pos2, "current", m_pos2Set, INDI_IDLE);
601 : }
602 :
603 0 : lock.lock();
604 0 : if (getSva(sva2, 2) < 0)
605 : {
606 0 : log<software_error>({__FILE__, __LINE__});
607 0 : state(stateCodes::ERROR);
608 0 : return 0;
609 : }
610 0 : lock.unlock();
611 0 : mx::sys::milliSleep(1); // Put this thread to sleep to make sure other thread gets a lock
612 :
613 0 : m_sva2 = sva2;
614 :
615 0 : if (m_naxes == 3)
616 : {
617 : float pos3;
618 : float sva3;
619 :
620 0 : lock.lock();
621 0 : if (getPos(pos3, 3) < 0)
622 : {
623 0 : log<software_error>({__FILE__, __LINE__});
624 0 : state(stateCodes::ERROR);
625 0 : return 0;
626 : }
627 0 : lock.unlock();
628 0 : mx::sys::milliSleep(1); // Put this thread to sleep to make sure other thread gets a lock
629 :
630 0 : m_pos3 = pos3;
631 0 : if (fabs(m_pos3Set - m_pos3) > m_posTol)
632 : {
633 0 : updateIfChanged(m_indiP_pos3, "current", m_pos3, INDI_BUSY);
634 : }
635 : else
636 : {
637 0 : updateIfChanged(m_indiP_pos3, "current", m_pos3Set, INDI_IDLE);
638 : }
639 :
640 0 : lock.lock();
641 0 : if (getSva(sva3, 2) < 0)
642 : {
643 0 : log<software_error>({__FILE__, __LINE__});
644 0 : state(stateCodes::ERROR);
645 0 : return 0;
646 : }
647 0 : lock.unlock();
648 0 : mx::sys::milliSleep(1); // Put this thread to sleep to make sure other thread gets a lock
649 :
650 0 : m_sva3 = sva3;
651 : }
652 :
653 0 : recordPI335();
654 :
655 : /*std::cerr << m_pos1Set << " " << pos1 << " " << m_sva1 << " " << m_pos2Set << " " << pos2 << " " << m_sva2;
656 : if(m_naxes == 3) std::cerr << " " << m_pos3Set << " " << pos3 << " " << m_sva3;
657 : std::cerr << "\n";*/
658 0 : }
659 0 : else if (state() == stateCodes::OPERATING)
660 : {
661 0 : updateIfChanged<float>(m_indiP_pos1, std::vector<std::string>({"current", "target"}), {m_pos1, m_pos1Set}, INDI_BUSY);
662 0 : updateIfChanged<float>(m_indiP_pos2, std::vector<std::string>({"current", "target"}), {m_pos2, m_pos2Set}, INDI_BUSY);
663 :
664 0 : if (m_naxes == 3)
665 : {
666 0 : updateIfChanged<float>(m_indiP_pos3, std::vector<std::string>({"current", "target"}), {m_pos3, m_pos3Set}, INDI_BUSY);
667 : }
668 : }
669 :
670 0 : SHMIMMONITOR_UPDATE_INDI;
671 0 : DM_UPDATE_INDI;
672 :
673 0 : return 0;
674 0 : }
675 :
676 0 : int pi335Ctrl::appShutdown()
677 : {
678 0 : DM_APP_SHUTDOWN;
679 0 : SHMIMMONITOR_APP_SHUTDOWN;
680 0 : TELEMETER_APP_SHUTDOWN;
681 :
682 0 : return 0;
683 : }
684 :
685 0 : int pi335Ctrl::testConnection()
686 : {
687 : int rv;
688 0 : std::string resp;
689 :
690 0 : if ((rv = tty::ttyWriteRead(resp, "*IDN?\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
691 : {
692 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
693 : }
694 :
695 : size_t st;
696 0 : if ((st = resp.find("E-727.3SDA")) == std::string::npos)
697 : {
698 0 : return log<text_log, -1>("Unknown device found: " + resp, logPrio::LOG_CRITICAL);
699 : }
700 0 : m_ctrl = mx::ioutils::removeWhiteSpace(resp.substr(st));
701 0 : log<text_log>(std::string("Connected to " + m_ctrl + " on ") + m_deviceName);
702 :
703 0 : std::string resp1, resp2, resp3;
704 :
705 0 : if ((rv = tty::ttyWriteRead(resp1, "CST? 1\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
706 : {
707 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
708 : }
709 0 : resp1 = mx::ioutils::removeWhiteSpace(resp1);
710 :
711 0 : if ((rv = tty::ttyWriteRead(resp2, "CST? 2\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
712 : {
713 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
714 : }
715 0 : resp2 = mx::ioutils::removeWhiteSpace(resp2);
716 :
717 0 : if ((rv = tty::ttyWriteRead(resp3, "CST? 3\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
718 : {
719 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
720 : }
721 0 : resp3 = mx::ioutils::removeWhiteSpace(resp3);
722 :
723 0 : updateIfChanged(m_indiP_pos1, "current", 0.0);
724 0 : updateIfChanged(m_indiP_pos1, "target", 0.0);
725 :
726 0 : updateIfChanged(m_indiP_pos2, "current", 0.0);
727 0 : updateIfChanged(m_indiP_pos2, "target", 0.0);
728 :
729 0 : if (resp1.find("1=S-335") == 0 && resp2.find("2=S-335") == 0 && resp3.find("3=0") == 0)
730 : {
731 0 : m_stage = resp1.substr(2);
732 0 : m_naxes = 2;
733 0 : m_actuallyATZ = true;
734 : }
735 0 : else if (resp1.find("1=S-325") == 0 && resp2.find("2=S-325") == 0 && resp3.find("3=S-325") == 0)
736 : {
737 0 : m_stage = resp1.substr(2);
738 0 : m_naxes = 3;
739 0 : m_actuallyATZ = false;
740 0 : if (!m_pos_3_sent)
741 : {
742 : ///\todo this needs to only happen once, and then never again
743 0 : REG_INDI_NEWPROP(m_indiP_pos3, "pos_3", pcf::IndiProperty::Number);
744 0 : m_indiP_pos3.add(pcf::IndiElement("current"));
745 0 : m_indiP_pos3.add(pcf::IndiElement("target"));
746 0 : m_indiP_pos3["current"] = -99999999;
747 0 : m_indiP_pos3["target"] = -99999999;
748 0 : updateIfChanged(m_indiP_pos3, "current", 0.0);
749 0 : updateIfChanged(m_indiP_pos3, "target", 0.0);
750 0 : m_pos_3_sent = true;
751 : }
752 : }
753 : else
754 : {
755 0 : return log<text_log, -1>("Unknown stage found: " + resp1 + " " + resp2 + " " + resp3, logPrio::LOG_CRITICAL);
756 : }
757 :
758 0 : log<text_log>("Found " + m_stage + " with " + std::to_string(m_naxes) + " axes");
759 :
760 : //-------- now get axis limits
761 :
762 : // axis 1
763 :
764 0 : if ((rv = tty::ttyWriteRead(resp, "TMN? 1\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
765 : {
766 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
767 : }
768 :
769 0 : if ((st = resp.find('=')) == std::string::npos)
770 : {
771 0 : return log<software_critical, -1>({__FILE__, __LINE__, "invalid response"});
772 : }
773 :
774 0 : m_min1 = mx::ioutils::convertFromString<float>(resp.substr(st + 1));
775 0 : log<text_log>("axis 1 min: " + std::to_string(m_min1));
776 :
777 0 : if ((rv = tty::ttyWriteRead(resp, "TMX? 1\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
778 : {
779 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
780 : }
781 :
782 0 : if ((st = resp.find('=')) == std::string::npos)
783 : {
784 0 : return log<software_critical, -1>({__FILE__, __LINE__, "invalid response"});
785 : }
786 :
787 0 : m_max1 = mx::ioutils::convertFromString<float>(resp.substr(st + 1));
788 0 : log<text_log>("axis 1 max: " + std::to_string(m_max1));
789 :
790 0 : if ((rv = tty::ttyWriteRead(resp, "TMN? 2\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
791 : {
792 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
793 : }
794 :
795 0 : if ((st = resp.find('=')) == std::string::npos)
796 : {
797 0 : return log<software_critical, -1>({__FILE__, __LINE__, "invalid response"});
798 : }
799 :
800 0 : m_min2 = mx::ioutils::convertFromString<float>(resp.substr(st + 1));
801 0 : log<text_log>("axis 2 min: " + std::to_string(m_min2));
802 :
803 0 : if ((rv = tty::ttyWriteRead(resp, "TMX? 2\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
804 : {
805 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
806 : }
807 :
808 0 : if ((st = resp.find('=')) == std::string::npos)
809 : {
810 0 : return log<software_critical, -1>({__FILE__, __LINE__, "invalid response"});
811 : }
812 :
813 0 : m_max2 = mx::ioutils::convertFromString<float>(resp.substr(st + 1));
814 0 : log<text_log>("axis 2 max: " + std::to_string(m_max2));
815 :
816 0 : if (m_naxes == 3)
817 : {
818 0 : if ((rv = tty::ttyWriteRead(resp, "TMN? 3\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
819 : {
820 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
821 : }
822 :
823 0 : if ((st = resp.find('=')) == std::string::npos)
824 : {
825 0 : return log<software_critical, -1>({__FILE__, __LINE__, "invalid response"});
826 : }
827 :
828 0 : m_min3 = mx::ioutils::convertFromString<float>(resp.substr(st + 1));
829 0 : log<text_log>("axis 3 min: " + std::to_string(m_min3));
830 :
831 0 : if ((rv = tty::ttyWriteRead(resp, "TMX? 3\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout)) < 0)
832 : {
833 0 : return log<software_critical, -1>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
834 : }
835 :
836 0 : if ((st = resp.find('=')) == std::string::npos)
837 : {
838 0 : return log<software_critical, -1>({__FILE__, __LINE__, "invalid response"});
839 : }
840 :
841 0 : m_max3 = mx::ioutils::convertFromString<float>(resp.substr(st + 1));
842 0 : log<text_log>("axis 3 max: " + std::to_string(m_max3));
843 : }
844 :
845 0 : m_flatCommand.resize(3, 1);
846 0 : if (m_naxes == 2)
847 : {
848 0 : m_flatCommand(0, 0) = m_homePos1;
849 0 : m_flatCommand(1, 0) = m_homePos2;
850 0 : m_flatCommand(2, 0) = 0.0;
851 : }
852 0 : else if (m_naxes == 3)
853 : {
854 0 : m_flatCommand(0, 0) = m_homePos1;
855 0 : m_flatCommand(1, 0) = m_homePos2;
856 0 : m_flatCommand(2, 0) = m_homePos3;
857 : }
858 0 : m_flatLoaded = true;
859 :
860 0 : return 0;
861 0 : }
862 :
863 0 : int pi335Ctrl::initDM()
864 : {
865 : int rv;
866 0 : std::string resp;
867 :
868 : // get open-loop position of axis 1 (should be zero)
869 0 : rv = tty::ttyWriteRead(resp, "SVA? 1\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
870 :
871 0 : if (rv < 0)
872 : {
873 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
874 : }
875 :
876 : // get open-loop position of axis 2 (should be zero)
877 0 : rv = tty::ttyWriteRead(resp, "SVA? 2\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
878 :
879 0 : if (rv < 0)
880 : {
881 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
882 : }
883 :
884 0 : if (m_naxes == 3)
885 : {
886 : // get open-loop position of axis 2 (should be zero)
887 0 : rv = tty::ttyWriteRead(resp, "SVA? 3\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
888 :
889 0 : if (rv < 0)
890 : {
891 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
892 : }
893 : }
894 :
895 : // make sure axis 1 has servo off
896 0 : rv = tty::ttyWrite("SVO 1 0\n", m_fileDescrip, m_writeTimeout);
897 :
898 0 : if (rv < 0)
899 : {
900 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
901 : }
902 :
903 : // make sure axis 2 has servo off
904 0 : rv = tty::ttyWrite("SVA 2 0\n", m_fileDescrip, m_writeTimeout);
905 :
906 0 : if (rv < 0)
907 : {
908 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
909 : }
910 :
911 0 : if (m_naxes == 0)
912 : {
913 : // make sure axis 3 has servo off
914 0 : rv = tty::ttyWrite("SVA 3 0\n", m_fileDescrip, m_writeTimeout);
915 :
916 0 : if (rv < 0)
917 : {
918 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
919 : }
920 : }
921 :
922 0 : m_servoState = 0;
923 :
924 0 : log<text_log>("servos off", logPrio::LOG_NOTICE);
925 :
926 0 : return home();
927 0 : }
928 :
929 0 : int pi335Ctrl::home()
930 : {
931 0 : if (m_servoState != 0)
932 : {
933 0 : log<text_log>("home requested but servos are not off", logPrio::LOG_ERROR);
934 0 : return -1;
935 : }
936 :
937 0 : m_homingStart = 0;
938 0 : m_homingState = 0;
939 :
940 0 : state(stateCodes::HOMING);
941 :
942 0 : return home_1();
943 : }
944 :
945 0 : int pi335Ctrl::homeState(int axis)
946 : {
947 0 : if (!m_actuallyATZ)
948 0 : return 1;
949 0 : std::string resp;
950 :
951 0 : if (getCom(resp, "ATZ?", axis) < 0)
952 : {
953 0 : log<software_error>({__FILE__, __LINE__});
954 :
955 0 : return -1;
956 : }
957 :
958 : ///\todo this should be a separate unit-tested parser
959 0 : size_t st = resp.find('=');
960 0 : if (st == std::string::npos || st > resp.size() - 2)
961 : {
962 0 : log<software_error>({__FILE__, __LINE__, "error parsing response"});
963 0 : return -1;
964 : }
965 0 : st += 1;
966 :
967 0 : return mx::ioutils::convertFromString<double>(resp.substr(st));
968 0 : }
969 :
970 0 : int pi335Ctrl::home_1()
971 : {
972 0 : if (m_servoState != 0)
973 : {
974 0 : log<text_log>("home_1 requested but servos are not off", logPrio::LOG_ERROR);
975 0 : return -1;
976 : }
977 :
978 0 : if (m_homingState != 0)
979 : {
980 0 : log<text_log>("home_1 requested but not in correct homing state", logPrio::LOG_ERROR);
981 0 : return -1;
982 : }
983 :
984 0 : if (m_actuallyATZ)
985 : {
986 : // zero range found in axis 1 (NOTE this moves mirror full range) TAKES 1min
987 0 : int rv = tty::ttyWrite("ATZ 1 NaN\n", m_fileDescrip, m_writeTimeout);
988 :
989 0 : if (rv < 0)
990 : {
991 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
992 : }
993 : }
994 :
995 0 : m_homingStart = mx::sys::get_curr_time(); ///\todo remmove m_homingStart once ATZ? works.
996 0 : m_homingState = 0;
997 0 : log<text_log>("commenced homing x");
998 :
999 0 : return 0;
1000 : }
1001 :
1002 0 : int pi335Ctrl::home_2()
1003 : {
1004 : int rv;
1005 :
1006 0 : if (m_servoState != 0)
1007 : {
1008 0 : log<text_log>("home_2 requested but servos are not off", logPrio::LOG_ERROR);
1009 0 : return -1;
1010 : }
1011 :
1012 0 : if (m_homingState != 1)
1013 : {
1014 0 : log<text_log>("home_2 requested but not in correct homing state", logPrio::LOG_ERROR);
1015 0 : return -1;
1016 : }
1017 :
1018 0 : if (m_actuallyATZ)
1019 : {
1020 : // zero range found in axis 2 (NOTE this moves mirror full range) TAKES 1min
1021 0 : rv = tty::ttyWrite("ATZ 2 NaN\n", m_fileDescrip, m_writeTimeout);
1022 :
1023 0 : if (rv < 0)
1024 : {
1025 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1026 : }
1027 : }
1028 :
1029 0 : m_homingStart = mx::sys::get_curr_time();
1030 0 : log<text_log>("commenced homing y");
1031 :
1032 0 : return 0;
1033 : }
1034 :
1035 0 : int pi335Ctrl::home_3()
1036 : {
1037 : int rv;
1038 :
1039 0 : if (m_servoState != 0)
1040 : {
1041 0 : log<text_log>("home_3 requested but servos are not off", logPrio::LOG_ERROR);
1042 0 : return -1;
1043 : }
1044 :
1045 0 : if (m_homingState != 2)
1046 : {
1047 0 : log<text_log>("home_3 requested but not in correct homing state", logPrio::LOG_ERROR);
1048 0 : return -1;
1049 : }
1050 :
1051 0 : if (m_actuallyATZ)
1052 : {
1053 : // zero range found in axis 3 (NOTE this moves mirror full range) TAKES 1min
1054 0 : rv = tty::ttyWrite("ATZ 3 NaN\n", m_fileDescrip, m_writeTimeout);
1055 :
1056 0 : if (rv < 0)
1057 : {
1058 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1059 : }
1060 : }
1061 :
1062 0 : m_homingStart = mx::sys::get_curr_time();
1063 0 : log<text_log>("commenced homing z");
1064 :
1065 0 : return 0;
1066 : }
1067 :
1068 0 : int pi335Ctrl::finishInit()
1069 : {
1070 : int rv;
1071 0 : std::string resp;
1072 :
1073 0 : if (m_servoState != 0)
1074 : {
1075 0 : log<text_log>("finishInit requested but servos are not off", logPrio::LOG_ERROR);
1076 0 : return -1;
1077 : }
1078 :
1079 0 : if (m_naxes == 2 && m_homingState != 2)
1080 : {
1081 0 : log<text_log>("finishInit requested but not in correct homing state", logPrio::LOG_ERROR);
1082 0 : return -1;
1083 : }
1084 0 : if (m_naxes == 3 && m_homingState != 3)
1085 : {
1086 0 : log<text_log>("finishInit requested but not in correct homing state", logPrio::LOG_ERROR);
1087 0 : return -1;
1088 : }
1089 :
1090 : // goto openloop pos zero (0 V) axis 1
1091 0 : rv = tty::ttyWrite("SVA 1 0.0\n", m_fileDescrip, m_writeTimeout);
1092 :
1093 0 : if (rv < 0)
1094 : {
1095 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1096 : }
1097 :
1098 0 : mx::sys::milliSleep(2000);
1099 :
1100 : // goto openloop pos zero (0 V) axis 2
1101 0 : rv = tty::ttyWrite("SVA 2 0.0\n", m_fileDescrip, m_writeTimeout);
1102 :
1103 0 : if (rv < 0)
1104 : {
1105 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1106 : }
1107 :
1108 0 : mx::sys::milliSleep(2000);
1109 :
1110 0 : if (m_naxes == 3)
1111 : {
1112 : // goto openloop pos zero (0 V) axis 3
1113 0 : rv = tty::ttyWrite("SVA 3 0.0\n", m_fileDescrip, m_writeTimeout);
1114 :
1115 0 : if (rv < 0)
1116 : {
1117 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1118 : }
1119 :
1120 0 : mx::sys::milliSleep(2000);
1121 : }
1122 :
1123 : // Get the real position of axis 1 (should be 0mrad st start)
1124 0 : rv = tty::ttyWriteRead(resp, "SVA? 1\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
1125 :
1126 0 : if (rv < 0)
1127 : {
1128 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1129 : }
1130 :
1131 : // Get the real position of axis 2 (should be 0mrad st start)
1132 0 : rv = tty::ttyWriteRead(resp, "SVA? 2\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
1133 :
1134 0 : if (rv < 0)
1135 : {
1136 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1137 : }
1138 :
1139 0 : if (m_naxes == 3)
1140 : {
1141 : // Get the real position of axis 3 (should be 0mrad st start)
1142 0 : rv = tty::ttyWriteRead(resp, "SVA? 3\n", "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
1143 :
1144 0 : if (rv < 0)
1145 : {
1146 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1147 : }
1148 : }
1149 :
1150 : // now safe to engage servos
1151 : //(IMPORTANT: NEVER EVER enable servos on axis 3 -- will damage S-335)
1152 : //(CAVEAT: for S-325 you CAN enable servors on axis 3)
1153 :
1154 : // turn on servo to axis 1 (green servo LED goes on 727)
1155 0 : rv = tty::ttyWrite("SVO 1 1\n", m_fileDescrip, m_writeTimeout);
1156 :
1157 0 : if (rv < 0)
1158 : {
1159 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1160 : }
1161 :
1162 0 : mx::sys::milliSleep(250);
1163 :
1164 : // turn on servo to axis 2 (green servo LED goes on 727)
1165 0 : rv = tty::ttyWrite("SVO 2 1\n", m_fileDescrip, m_writeTimeout);
1166 :
1167 0 : if (rv < 0)
1168 : {
1169 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1170 : }
1171 :
1172 0 : if (m_naxes == 3)
1173 : {
1174 0 : mx::sys::milliSleep(250);
1175 :
1176 : // turn on servo to axis 3 (green servo LED goes on 727)
1177 0 : rv = tty::ttyWrite("SVO 3 1\n", m_fileDescrip, m_writeTimeout);
1178 :
1179 0 : if (rv < 0)
1180 : {
1181 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1182 : }
1183 : }
1184 :
1185 0 : m_servoState = 1;
1186 0 : log<text_log>("servos engaged", logPrio::LOG_NOTICE);
1187 :
1188 0 : mx::sys::milliSleep(1000);
1189 :
1190 : // now safe for closed loop moves
1191 : // center axis 1 (to configured home position)
1192 :
1193 0 : std::string com = "MOV 1 " + std::to_string(m_homePos1) + "\n";
1194 0 : rv = tty::ttyWrite(com, m_fileDescrip, m_writeTimeout);
1195 :
1196 0 : if (rv < 0)
1197 : {
1198 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1199 : }
1200 :
1201 0 : m_pos1Set = m_homePos1;
1202 0 : updateIfChanged(m_indiP_pos1, "target", m_homePos1);
1203 :
1204 : // center axis 2 (to configured home position)
1205 0 : com = "MOV 2 " + std::to_string(m_homePos2) + "\n";
1206 0 : rv = tty::ttyWrite(com, m_fileDescrip, m_writeTimeout);
1207 :
1208 0 : if (rv < 0)
1209 : {
1210 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1211 : }
1212 :
1213 0 : m_pos2Set = m_homePos2;
1214 0 : updateIfChanged(m_indiP_pos2, "target", m_homePos2);
1215 :
1216 0 : if (m_naxes == 3)
1217 : {
1218 : // center axis 3 (to configured home position)
1219 0 : com = "MOV 3 " + std::to_string(m_homePos3) + "\n";
1220 0 : rv = tty::ttyWrite(com, m_fileDescrip, m_writeTimeout);
1221 :
1222 0 : if (rv < 0)
1223 : {
1224 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1225 : }
1226 :
1227 0 : m_pos3Set = m_homePos3;
1228 0 : updateIfChanged(m_indiP_pos3, "target", m_homePos3);
1229 : }
1230 :
1231 0 : state(stateCodes::READY);
1232 :
1233 0 : return 0;
1234 0 : }
1235 :
1236 0 : int pi335Ctrl::zeroDM()
1237 : {
1238 0 : move_1(0.0);
1239 0 : move_2(0.0);
1240 0 : if (m_naxes == 3)
1241 0 : move_3(0.0);
1242 :
1243 0 : log<text_log>("DM zeroed");
1244 0 : return 0;
1245 : }
1246 :
1247 0 : int pi335Ctrl::commandDM(void *curr_src)
1248 : {
1249 0 : if (state() != stateCodes::OPERATING)
1250 0 : return 0;
1251 0 : float pos1 = (reinterpret_cast<float *>(curr_src))[0];
1252 0 : float pos2 = (reinterpret_cast<float *>(curr_src))[1];
1253 :
1254 0 : float pos3 = 0;
1255 0 : if (m_naxes == 3)
1256 0 : pos3 = (reinterpret_cast<float *>(curr_src))[2];
1257 :
1258 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
1259 :
1260 : int rv;
1261 0 : if ((rv = move_1(pos1)) < 0)
1262 0 : return rv;
1263 :
1264 0 : if ((rv = move_2(pos2)) < 0)
1265 0 : return rv;
1266 :
1267 0 : if (m_naxes == 3)
1268 0 : if ((rv = move_3(pos3)) < 0)
1269 0 : return rv;
1270 :
1271 0 : return 0;
1272 0 : }
1273 :
1274 0 : int pi335Ctrl::releaseDM()
1275 : {
1276 : int rv;
1277 :
1278 0 : if (m_servoState != 0)
1279 : {
1280 0 : if ((rv = tty::ttyWrite("SVO 1 0\n", m_fileDescrip, m_writeTimeout)) < 0)
1281 : {
1282 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1283 : }
1284 :
1285 0 : if ((rv = tty::ttyWrite("SVO 2 0\n", m_fileDescrip, m_writeTimeout)) < 0)
1286 : {
1287 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1288 : }
1289 :
1290 0 : m_servoState = 0;
1291 :
1292 0 : log<text_log>("servos off", logPrio::LOG_NOTICE);
1293 : }
1294 :
1295 0 : if ((rv = tty::ttyWrite("SVA 1 0\n", m_fileDescrip, m_writeTimeout)) < 0)
1296 : {
1297 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1298 : }
1299 :
1300 0 : if ((rv = tty::ttyWrite("SVA 2 0\n", m_fileDescrip, m_writeTimeout)) < 0)
1301 : {
1302 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1303 : }
1304 :
1305 0 : if (m_naxes == 3)
1306 : {
1307 0 : if ((rv = tty::ttyWrite("SVA 2 0\n", m_fileDescrip, m_writeTimeout)) < 0)
1308 : {
1309 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1310 : }
1311 : }
1312 :
1313 0 : m_flatSet = false;
1314 0 : state(stateCodes::NOTHOMED);
1315 :
1316 0 : return 0;
1317 : }
1318 :
1319 0 : int pi335Ctrl::getCom(std::string &resp,
1320 : const std::string &com,
1321 : int axis)
1322 : {
1323 0 : std::string sendcom = com;
1324 0 : if (axis == 1 || axis == 2)
1325 : {
1326 0 : sendcom += " ";
1327 0 : sendcom += std::to_string(axis);
1328 : }
1329 :
1330 0 : sendcom += "\n";
1331 :
1332 0 : int rv = tty::ttyWriteRead(resp, sendcom, "\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
1333 0 : if (rv < 0)
1334 : {
1335 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1336 0 : return -1;
1337 : }
1338 :
1339 0 : return 0;
1340 0 : }
1341 :
1342 0 : int pi335Ctrl::getPos(float &pos,
1343 : int n)
1344 : {
1345 0 : std::string resp;
1346 0 : if (getCom(resp, "POS?", n) < 0)
1347 : {
1348 0 : log<software_error>({__FILE__, __LINE__});
1349 : }
1350 :
1351 : ///\todo this should be a separate unit-tested parser
1352 0 : size_t st = resp.find('=');
1353 0 : if (st == std::string::npos || st > resp.size() - 2)
1354 : {
1355 0 : log<software_error>({__FILE__, __LINE__, "error parsing response"});
1356 0 : return -1;
1357 : }
1358 0 : st += 1;
1359 0 : pos = mx::ioutils::convertFromString<double>(resp.substr(st));
1360 :
1361 0 : return 0;
1362 0 : }
1363 :
1364 0 : int pi335Ctrl::getSva(float &sva,
1365 : int n)
1366 : {
1367 0 : std::string resp;
1368 0 : if (getCom(resp, "SVA?", n) < 0)
1369 : {
1370 0 : log<software_error>({__FILE__, __LINE__});
1371 : }
1372 :
1373 : ///\todo this should be a separate unit-tested parser
1374 0 : size_t st = resp.find('=');
1375 0 : if (st == std::string::npos || st > resp.size() - 2)
1376 : {
1377 0 : log<software_error>({__FILE__, __LINE__, "error parsing response"});
1378 0 : return -1;
1379 : }
1380 0 : st += 1;
1381 0 : sva = mx::ioutils::convertFromString<double>(resp.substr(st));
1382 :
1383 0 : return 0;
1384 0 : }
1385 :
1386 0 : int pi335Ctrl::updateFlat(float absPos1,
1387 : float absPos2,
1388 : float absPos3)
1389 : {
1390 0 : m_flatCommand(0, 0) = absPos1;
1391 0 : m_flatCommand(1, 0) = absPos2;
1392 0 : m_flatCommand(2, 0) = absPos3;
1393 :
1394 0 : if (state() == stateCodes::OPERATING)
1395 : {
1396 0 : return setFlat(true);
1397 : }
1398 : else
1399 : {
1400 0 : return 0;
1401 : }
1402 : }
1403 :
1404 0 : int pi335Ctrl::move_1(float absPos)
1405 : {
1406 : int rv;
1407 :
1408 0 : if (absPos < m_min1 || absPos > m_max1)
1409 : {
1410 0 : log<text_log>("request move on azis 1 out of range", logPrio::LOG_ERROR);
1411 0 : return -1;
1412 : }
1413 :
1414 0 : m_pos1Set = absPos;
1415 :
1416 0 : std::string com = "MOV 1 " + std::to_string(absPos) + "\n";
1417 :
1418 0 : rv = tty::ttyWrite(com, m_fileDescrip, m_writeTimeout);
1419 :
1420 0 : if (rv < 0)
1421 : {
1422 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1423 : }
1424 :
1425 0 : return 0;
1426 0 : }
1427 :
1428 0 : int pi335Ctrl::move_2(float absPos)
1429 : {
1430 : int rv;
1431 :
1432 0 : if (absPos < m_min2 || absPos > m_max2)
1433 : {
1434 0 : log<text_log>("request move on azis 2 out of range", logPrio::LOG_ERROR);
1435 0 : return -1;
1436 : }
1437 :
1438 0 : m_pos2Set = absPos;
1439 0 : std::string com = "MOV 2 " + std::to_string(absPos) + "\n";
1440 :
1441 0 : rv = tty::ttyWrite(com, m_fileDescrip, m_writeTimeout);
1442 :
1443 0 : if (rv < 0)
1444 : {
1445 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1446 : }
1447 :
1448 0 : return 0;
1449 0 : }
1450 :
1451 0 : int pi335Ctrl::move_3(float absPos)
1452 : {
1453 0 : if (m_naxes < 3)
1454 : {
1455 0 : return log<software_error, -1>({__FILE__, __LINE__, "tried to move axis 3 but we don't have that"});
1456 : }
1457 :
1458 : int rv;
1459 :
1460 0 : if (absPos < m_min3 || absPos > m_max3)
1461 : {
1462 0 : log<text_log>("request move on azis 3 out of range", logPrio::LOG_ERROR);
1463 0 : return -1;
1464 : }
1465 :
1466 0 : m_pos3Set = absPos;
1467 0 : std::string com = "MOV 3 " + std::to_string(absPos) + "\n";
1468 :
1469 0 : rv = tty::ttyWrite(com, m_fileDescrip, m_writeTimeout);
1470 :
1471 0 : if (rv < 0)
1472 : {
1473 0 : log<software_error>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
1474 : }
1475 :
1476 0 : return 0;
1477 0 : }
1478 :
1479 0 : INDI_NEWCALLBACK_DEFN(pi335Ctrl, m_indiP_pos1)
1480 : (const pcf::IndiProperty &ipRecv)
1481 : {
1482 0 : if (ipRecv.createUniqueKey() == m_indiP_pos1.createUniqueKey())
1483 : {
1484 0 : float current = -999999, target = -999999;
1485 :
1486 0 : if (ipRecv.find("current"))
1487 : {
1488 0 : current = ipRecv["current"].get<float>();
1489 : }
1490 :
1491 0 : if (ipRecv.find("target"))
1492 : {
1493 0 : target = ipRecv["target"].get<float>();
1494 : }
1495 :
1496 0 : if (target == -999999)
1497 0 : target = current;
1498 :
1499 0 : if (target == -999999)
1500 0 : return 0;
1501 :
1502 0 : if (state() == stateCodes::READY)
1503 : {
1504 : // Lock the mutex, waiting if necessary
1505 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
1506 :
1507 0 : updateIfChanged(m_indiP_pos1, "target", target);
1508 :
1509 0 : updateFlat(target, m_pos2Set, m_pos3Set); // This just changes the values, but doesn't move
1510 :
1511 0 : return move_1(target);
1512 0 : }
1513 0 : else if (state() == stateCodes::OPERATING)
1514 : {
1515 0 : return updateFlat(target, m_pos2Set, m_pos3Set);
1516 : }
1517 : }
1518 0 : return -1;
1519 : }
1520 :
1521 0 : INDI_NEWCALLBACK_DEFN(pi335Ctrl, m_indiP_pos2)
1522 : (const pcf::IndiProperty &ipRecv)
1523 : {
1524 0 : if (ipRecv.createUniqueKey() == m_indiP_pos2.createUniqueKey())
1525 : {
1526 0 : float current = -999999, target = -999999;
1527 :
1528 0 : if (ipRecv.find("current"))
1529 : {
1530 0 : current = ipRecv["current"].get<float>();
1531 : }
1532 :
1533 0 : if (ipRecv.find("target"))
1534 : {
1535 0 : target = ipRecv["target"].get<float>();
1536 : }
1537 :
1538 0 : if (target == -999999)
1539 0 : target = current;
1540 :
1541 0 : if (target == -999999)
1542 0 : return 0;
1543 :
1544 0 : if (state() == stateCodes::READY)
1545 : {
1546 : // Lock the mutex, waiting if necessary
1547 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
1548 :
1549 0 : updateIfChanged(m_indiP_pos2, "target", target);
1550 0 : updateFlat(m_pos1Set, target, m_pos3Set); // This just changes the values, but doesn't move
1551 0 : return move_2(target);
1552 0 : }
1553 0 : else if (state() == stateCodes::OPERATING)
1554 : {
1555 0 : return updateFlat(m_pos1Set, target, m_pos3Set);
1556 : }
1557 : }
1558 0 : return -1;
1559 : }
1560 :
1561 0 : INDI_NEWCALLBACK_DEFN(pi335Ctrl, m_indiP_pos3)
1562 : (const pcf::IndiProperty &ipRecv)
1563 : {
1564 :
1565 0 : if (ipRecv.getName() == m_indiP_pos3.getName())
1566 : {
1567 0 : float current = -999999, target = -999999;
1568 :
1569 0 : if (ipRecv.find("current"))
1570 : {
1571 0 : current = ipRecv["current"].get<float>();
1572 : }
1573 :
1574 0 : if (ipRecv.find("target"))
1575 : {
1576 0 : target = ipRecv["target"].get<float>();
1577 : }
1578 :
1579 0 : if (target == -999999)
1580 0 : target = current;
1581 :
1582 0 : if (target == -999999)
1583 0 : return 0;
1584 :
1585 0 : if (state() == stateCodes::READY)
1586 : {
1587 : // Lock the mutex, waiting if necessary
1588 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
1589 :
1590 0 : updateIfChanged(m_indiP_pos3, "target", target);
1591 :
1592 0 : updateFlat(m_pos1Set, m_pos2Set, target); // This just changes the values, but doesn't move
1593 0 : return move_3(target);
1594 0 : }
1595 0 : else if (state() == stateCodes::OPERATING)
1596 : {
1597 0 : return updateFlat(m_pos1Set, m_pos2Set, target);
1598 : }
1599 : }
1600 0 : return -1;
1601 : }
1602 :
1603 0 : int pi335Ctrl::checkRecordTimes()
1604 : {
1605 0 : return telemeterT::checkRecordTimes(telem_pi335());
1606 : }
1607 :
1608 0 : int pi335Ctrl::recordTelem(const telem_pi335 *)
1609 : {
1610 0 : return recordPI335(true);
1611 : }
1612 :
1613 0 : int pi335Ctrl::recordPI335(bool force)
1614 : {
1615 : static float pos1Set = std::numeric_limits<float>::max();
1616 : static float pos1 = std::numeric_limits<float>::max();
1617 : static float sva1 = std::numeric_limits<float>::max();
1618 : static float pos2Set = std::numeric_limits<float>::max();
1619 : static float pos2 = std::numeric_limits<float>::max();
1620 : static float sva2 = std::numeric_limits<float>::max();
1621 : static float pos3Set = std::numeric_limits<float>::max();
1622 : static float pos3 = std::numeric_limits<float>::max();
1623 : static float sva3 = std::numeric_limits<float>::max();
1624 :
1625 0 : if (force || m_pos1Set != pos1Set || m_pos1 != pos1 || m_sva1 != sva1 ||
1626 0 : m_pos2Set != pos2Set || m_pos2 != pos2 || m_sva2 != sva2 ||
1627 0 : m_pos3Set != pos3Set || m_pos3 != pos3 || m_sva3 != sva3)
1628 : {
1629 0 : telem<telem_pi335>({m_pos1Set, m_pos1, m_sva1, m_pos2Set, m_pos2, m_sva2, m_pos3Set, m_pos3, m_sva3});
1630 :
1631 0 : pos1Set = m_pos1Set;
1632 0 : pos1 = m_pos1;
1633 0 : sva1 = m_sva1;
1634 0 : pos2Set = m_pos2Set;
1635 0 : pos2 = m_pos2;
1636 0 : sva2 = m_sva2;
1637 0 : pos3Set = m_pos3Set;
1638 0 : pos3 = m_pos3;
1639 0 : sva3 = m_sva3;
1640 : }
1641 :
1642 0 : return 0;
1643 : }
1644 :
1645 : } // namespace app
1646 : } // namespace MagAOX
1647 :
1648 : #endif // pi335Ctrl_hpp
|
