Line data Source code
1 : /** \file smc100ccCtrl.hpp
2 : * \brief The smc controller communicator
3 : * \author Chris Bohlman (cbohlman@pm.me)
4 : *
5 : * \ingroup smc100ccCtrl_files
6 : *
7 : * History:
8 : * - 2019-01-10 created by CJB
9 : *
10 : */
11 : #ifndef smc100ccCtrl_hpp
12 : #define smc100ccCtrl_hpp
13 :
14 : #include "../../libMagAOX/libMagAOX.hpp" //Note this is included on command line to trigger pch
15 : #include "../../magaox_git_version.h"
16 :
17 : #include <iostream>
18 : #include <string>
19 : #include <fstream>
20 : #include <vector>
21 : #include <sstream>
22 : #include <algorithm>
23 : #include <iterator>
24 : #include <bitset>
25 :
26 : namespace MagAOX
27 : {
28 : namespace app
29 : {
30 :
31 : /** TS command: Checks if there were any errors during initialization
32 : * Solid orange LED: everything is okay, TS should return 1TS00000A
33 : * PW command: change all stage and motor configuration parameters
34 : * OR command: gets controller to ready state (must go through homing first)
35 : * In ready state, can move relative and move absolute
36 : * RS command: TO get from ready to not referenced
37 :
38 : Change to stateCodes::OPERATING and stateCodes::READY
39 :
40 : */
41 : class smc100ccCtrl : public MagAOXApp<>, public tty::usbDevice, public dev::ioDevice,
42 : public dev::stdMotionStage<smc100ccCtrl>, public dev::telemeter<smc100ccCtrl>
43 : {
44 :
45 : friend class dev::stdMotionStage<smc100ccCtrl>;
46 :
47 : friend class dev::telemeter<smc100ccCtrl>;
48 :
49 : protected:
50 :
51 : /** \name Configurable Parameters
52 : *
53 : *@{
54 : */
55 : double m_homingOffset {0};
56 :
57 : double m_opDelta {0}; ///< The threshold for switching to OPERATING
58 :
59 : ///@}
60 :
61 :
62 : pcf::IndiProperty m_indiP_position; ///< Indi variable for reporting the stage position.
63 :
64 : std::vector<std::string> validStateCodes{};
65 :
66 : double m_position {0};
67 :
68 : double m_target {0};
69 :
70 : bool m_wasHoming {0};
71 :
72 : bool m_powerOnHomed{false};
73 :
74 : bool m_moveOp {true}; ///< Flag indicating that OPERATING should not be set for a move, because it's less than m_opDelta.
75 : public:
76 :
77 0 : INDI_NEWCALLBACK_DECL(smc100ccCtrl, m_indiP_position);
78 :
79 :
80 : /// Default c'tor.
81 : smc100ccCtrl();
82 :
83 15 : ~smc100ccCtrl() noexcept
84 15 : {
85 15 : }
86 :
87 : /// Setup the configuration system (called by MagAOXApp::setup())
88 : virtual void setupConfig();
89 :
90 : /// Load the configuration system results (called by MagAOXApp::setup())
91 : virtual void loadConfig();
92 :
93 : /// Checks if the device was found during loadConfig.
94 : virtual int appStartup();
95 :
96 : /// Changes device state based on testing connection and device status
97 : virtual int appLogic();
98 :
99 : /// Do any needed shutdown tasks. Currently nothing in this app.
100 : virtual int appShutdown();
101 :
102 0 : virtual int onPowerOff()
103 : {
104 0 : m_powerOnHomed=false;
105 0 : recordStage(true);
106 0 : recordPosition(true);
107 0 : return 0;
108 : }
109 :
110 : int makeCom( std::string &str,
111 : const std::string & com
112 : );
113 :
114 : int splitResponse( int &axis,
115 : std::string &com,
116 : std::string &val,
117 : std::string &resp
118 : );
119 :
120 :
121 : int getCtrlState( std::string &state );
122 :
123 : /// Tests if device is cabale of recieving/executing IO commands
124 : /** Sends command for device to return serial number, and compares to device serial number indi property
125 : *
126 : * \returns -1 on serial numbers being different, thus ensuring connection test was sucsessful
127 : * \returns 0 on serial numbers being equal
128 : */
129 : int testConnection();
130 :
131 : /// Verifies current status of controller
132 : /** Checks if controller is moving or has moved to correct position
133 : *
134 : * \returns 0 if controller is currently moving or has moved correctly.
135 : * \returns -1 on error with sending commands or if current position does not match target position.
136 : */
137 : int getPosition( double & pos /**< [out] on output, the current position*/);
138 :
139 : /// Returns any error controller has
140 : /** Called after every command is sent
141 : *
142 : * \returns 0 if no error is reported
143 : * \returns -1 if an error is reported and error string is set in reference
144 : */
145 : int getLastError( std::string& errStr /** [out] the last error string */);
146 :
147 : /** \name Standard Motion Stage Interface
148 : * @{
149 : *
150 : */
151 :
152 : int stop();
153 :
154 : int startHoming();
155 :
156 : double presetNumber();
157 :
158 : int moveTo(double position);
159 :
160 :
161 : ///@}
162 :
163 :
164 : /** \name Telemeter Interface
165 : *
166 : * @{
167 : */
168 : int checkRecordTimes();
169 :
170 : int recordTelem( const telem_stage * );
171 :
172 : int recordTelem( const telem_position * );
173 :
174 : int recordStage( bool force = false );
175 :
176 : int recordPosition( bool force = false);
177 :
178 : ///@}
179 : };
180 :
181 45 : inline smc100ccCtrl::smc100ccCtrl() : MagAOXApp(MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED)
182 : {
183 15 : m_powerMgtEnabled = true;
184 15 : m_powerOnWait = 5; // default to 5 seconds for controller boot up.
185 :
186 15 : m_defaultPositions = false;
187 :
188 15 : return;
189 0 : }
190 :
191 0 : void smc100ccCtrl::setupConfig()
192 : {
193 0 : config.add("stage.homingOffset", "", "stage.homingOffset", argType::Required, "stage", "homingOffset", false, "float", "Homing offset, a.k.a. default starting position.");
194 0 : config.add("stage.opDelta", "", "stage.opDelta", argType::Required, "stage", "opDelta", false, "float", "Threshold move size for switching to OPERATING.");
195 :
196 0 : tty::usbDevice::setupConfig(config);
197 0 : dev::ioDevice::setupConfig(config);
198 0 : dev::stdMotionStage<smc100ccCtrl>::setupConfig(config);
199 0 : dev::telemeter<smc100ccCtrl>::setupConfig(config);
200 0 : }
201 :
202 0 : void smc100ccCtrl::loadConfig()
203 : {
204 :
205 0 : config(m_homingOffset, "stage.homingOffset");
206 :
207 0 : config(m_opDelta, "stage.opDelta");
208 0 : this->m_baudRate = B57600; //default for SMC100CC controller. Will be overridden by any config setting.
209 :
210 0 : int rv = tty::usbDevice::loadConfig(config);
211 :
212 0 : if(rv != 0 && rv != TTY_E_NODEVNAMES && rv != TTY_E_DEVNOTFOUND) //Ignore error if not plugged in
213 : {
214 0 : log<software_error>({ __FILE__, __LINE__, "error loading USB device configs"});
215 0 : log<software_error>( {__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
216 0 : m_shutdown = 1;
217 : }
218 :
219 0 : rv = dev::ioDevice::loadConfig(config);
220 0 : if(rv != 0)
221 : {
222 0 : log<software_error>({ __FILE__, __LINE__, "error loading io device configs"});
223 0 : m_shutdown = 1;
224 : }
225 :
226 0 : rv = dev::stdMotionStage<smc100ccCtrl>::loadConfig(config);
227 0 : if(rv != 0)
228 : {
229 0 : log<software_error>({ __FILE__, __LINE__, "error loading stdMotionStage configs"});
230 0 : m_shutdown = 1;
231 : }
232 :
233 0 : rv = dev::telemeter<smc100ccCtrl>::loadConfig(config);
234 0 : if(rv != 0)
235 : {
236 0 : log<software_error>({ __FILE__, __LINE__, "error loading telemeter configs"});
237 0 : m_shutdown = 1;
238 : }
239 0 : }
240 :
241 0 : int smc100ccCtrl::appStartup()
242 : {
243 :
244 0 : REG_INDI_NEWPROP(m_indiP_position, "position", pcf::IndiProperty::Number);
245 0 : m_indiP_position.add (pcf::IndiElement("current"));
246 0 : m_indiP_position["current"].set(0);
247 0 : m_indiP_position.add (pcf::IndiElement("target"));
248 :
249 :
250 0 : if( state() == stateCodes::UNINITIALIZED )
251 : {
252 0 : log<text_log>( "In appStartup but in state UNINITIALIZED.", logPrio::LOG_CRITICAL );
253 0 : return -1;
254 : }
255 :
256 0 : if(m_presetNames.size() != m_presetPositions.size())
257 : {
258 0 : return log<text_log,-1>("must set a position for each preset", logPrio::LOG_CRITICAL);
259 : }
260 :
261 0 : m_presetNames.insert(m_presetNames.begin(), "none");
262 0 : m_presetPositions.insert(m_presetPositions.begin(), -1);
263 :
264 0 : int rv = dev::stdMotionStage<smc100ccCtrl>::appStartup();
265 0 : if(rv != 0)
266 : {
267 0 : log<software_error>({ __FILE__, __LINE__, "error starting up stdMotionStage"});
268 0 : m_shutdown = 1;
269 : }
270 :
271 0 : rv = dev::telemeter<smc100ccCtrl>::appStartup();
272 0 : if(rv != 0)
273 : {
274 0 : log<software_error>({ __FILE__, __LINE__, "error starting up telemeter"});
275 0 : m_shutdown = 1;
276 : }
277 :
278 0 : return 0;
279 : }
280 :
281 0 : int smc100ccCtrl::appLogic()
282 : {
283 0 : if( state() == stateCodes::INITIALIZED )
284 : {
285 0 : log<text_log>( "In appLogic but in state INITIALIZED.", logPrio::LOG_CRITICAL );
286 0 : return -1;
287 : }
288 :
289 0 : if(stdMotionStage<smc100ccCtrl>::appLogic() < 0)
290 : {
291 0 : log<software_error>({__FILE__, __LINE__});
292 0 : return -1;
293 : }
294 :
295 0 : if( state() == stateCodes::POWERON)
296 : {
297 0 : if(!powerOnWaitElapsed()) return 0;
298 :
299 0 : if(m_deviceName == "")
300 : {
301 0 : state(stateCodes::NODEVICE);
302 : }
303 : else
304 : {
305 0 : state(stateCodes::NOTCONNECTED);
306 0 : if(!stateLogged())
307 : {
308 0 : std::stringstream logs;
309 0 : logs << "USB Device " << m_idVendor << ":" << m_idProduct << ":" << m_serial << " found in udev as " << m_deviceName;
310 0 : log<text_log>(logs.str());
311 0 : }
312 : }
313 :
314 : }
315 :
316 0 : if( state() == stateCodes::NODEVICE )
317 : {
318 : #ifdef XWC_SIM_MODE
319 0 : m_deviceName = "/dev/simTTY";
320 0 : int rv = TTY_E_NOERROR;
321 : #else
322 : int rv = tty::usbDevice::getDeviceName();
323 : #endif
324 :
325 0 : if(rv < 0 && rv != TTY_E_DEVNOTFOUND && rv != TTY_E_NODEVNAMES)
326 : {
327 0 : state(stateCodes::FAILURE);
328 0 : if(!stateLogged())
329 : {
330 0 : log<software_critical>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
331 : }
332 0 : return -1;
333 : }
334 :
335 0 : if(rv == TTY_E_DEVNOTFOUND || rv == TTY_E_NODEVNAMES)
336 : {
337 0 : state(stateCodes::NODEVICE);
338 0 : if(!stateLogged())
339 : {
340 0 : std::stringstream logs;
341 0 : logs << "USB Device " << m_idVendor << ":" << m_idProduct << ":" << m_serial << " not found in udev";
342 0 : log<text_log>(logs.str());
343 0 : }
344 0 : return 0;
345 : }
346 : else
347 : {
348 0 : state(stateCodes::NOTCONNECTED);
349 0 : if(!stateLogged())
350 : {
351 0 : std::stringstream logs;
352 0 : logs << "USB Device " << m_idVendor << ":" << m_idProduct << ":" << m_serial << " found in udev as " << m_deviceName;
353 0 : log<text_log>(logs.str());
354 0 : }
355 : }
356 : }
357 :
358 0 : if( state() == stateCodes::NOTCONNECTED )
359 : {
360 : int rv;
361 : #ifdef XWC_SIM_MODE
362 0 : rv = 0;
363 : #else
364 : {//scope for elPriv
365 : elevatedPrivileges elPriv(this);
366 : rv = connect();
367 : }
368 : #endif
369 :
370 0 : if(rv < 0)
371 : {
372 0 : int nrv = tty::usbDevice::getDeviceName();
373 0 : if(nrv < 0 && nrv != TTY_E_DEVNOTFOUND && nrv != TTY_E_NODEVNAMES)
374 : {
375 0 : state(stateCodes::FAILURE);
376 0 : if(!stateLogged()) log<software_critical>({__FILE__, __LINE__, nrv, tty::ttyErrorString(nrv)});
377 0 : return -1;
378 : }
379 :
380 0 : if(nrv == TTY_E_DEVNOTFOUND || nrv == TTY_E_NODEVNAMES)
381 : {
382 0 : state(stateCodes::NODEVICE);
383 :
384 0 : if(!stateLogged())
385 : {
386 0 : std::stringstream logs;
387 0 : logs << "USB Device " << m_idVendor << ":" << m_idProduct << ":" << m_serial << " no longer found in udev";
388 0 : log<text_log>(logs.str());
389 0 : }
390 0 : return 0;
391 : }
392 :
393 : //if connect failed, and there is a device, then we have some other problem.
394 0 : state(stateCodes::FAILURE);
395 0 : if(!stateLogged()) log<software_error>({__FILE__,__LINE__,rv, tty::ttyErrorString(rv)});
396 0 : return -1;
397 :
398 : }
399 :
400 0 : if( testConnection() == 0 )
401 : {
402 0 : state(stateCodes::CONNECTED);
403 : }
404 : else
405 : {
406 0 : std::string errorString;
407 0 : if (getLastError(errorString) != 0)
408 : {
409 0 : log<software_error>({__FILE__, __LINE__,errorString});
410 : }
411 :
412 0 : return 0;
413 0 : }
414 :
415 :
416 0 : if(state() == stateCodes::CONNECTED && !stateLogged())
417 : {
418 0 : std::stringstream logs;
419 0 : logs << "Connected to stage(s) on " << m_deviceName;
420 0 : log<text_log>(logs.str());
421 0 : }
422 : }
423 :
424 :
425 0 : if(state() == stateCodes::HOMING || state() == stateCodes::READY || state() == stateCodes::OPERATING)
426 : {
427 0 : if(telemeter<smc100ccCtrl>::appLogic() < 0)
428 : {
429 0 : log<software_error>({__FILE__, __LINE__});
430 0 : return -1;
431 : }
432 : }
433 :
434 : //If we're here, we can get state from controller...
435 0 : std::string axState;
436 : //mutex scope
437 : {
438 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
439 0 : if(getCtrlState(axState) < 0)
440 : {
441 0 : if(m_powerTargetState == 0) return 0;
442 0 : return log<software_error, 0>({__FILE__,__LINE__});
443 : }
444 0 : }
445 :
446 :
447 0 : if(axState[0] == '0')
448 : {
449 0 : state(stateCodes::NOTHOMED); //This always means this.
450 : }
451 0 : else if (axState[0] == '1' && axState[1] == '0')
452 : {
453 : //Need to download stage info
454 0 : log<text_log>("getting stage information");
455 0 : std::string com;
456 0 : if(makeCom(com, "PW1") < 0)
457 : {
458 0 : log<software_error>({__FILE__, __LINE__,"Error making command PW1" });
459 0 : return 0;
460 : }
461 :
462 0 : int rv = MagAOX::tty::ttyWrite( com, m_fileDescrip, m_writeTimeout);
463 0 : if (rv != TTY_E_NOERROR)
464 : {
465 0 : if(m_powerTargetState == 0) return 0;
466 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
467 0 : return -1;
468 : }
469 :
470 0 : sleep(5);
471 0 : if(makeCom(com, "ZX2") < 0)
472 : {
473 0 : log<software_error>({__FILE__, __LINE__,"Error making command ZX2" });
474 0 : return 0;
475 : }
476 :
477 0 : rv = MagAOX::tty::ttyWrite( com, m_fileDescrip, m_writeTimeout);
478 0 : if (rv != TTY_E_NOERROR)
479 : {
480 0 : if(m_powerTargetState == 0) return 0;
481 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
482 0 : return -1;
483 : }
484 :
485 0 : sleep(5);
486 0 : if(makeCom(com, "PW0") < 0)
487 : {
488 0 : log<software_error>({__FILE__, __LINE__,"Error making command PW0" });
489 0 : return 0;
490 : }
491 :
492 0 : rv = MagAOX::tty::ttyWrite( com, m_fileDescrip, m_writeTimeout);
493 0 : if (rv != TTY_E_NOERROR)
494 : {
495 0 : if(m_powerTargetState == 0) return 0;
496 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
497 0 : return -1;
498 : }
499 :
500 0 : sleep(5);
501 0 : log<text_log>("stage information loaded");
502 0 : return 0;
503 :
504 0 : }
505 0 : else if (axState[0] == '1' && (axState[1] == 'E' || axState[1] == 'F'))
506 : {
507 0 : state(stateCodes::HOMING);
508 0 : m_moving = 1;
509 0 : m_wasHoming = 1;
510 : }
511 0 : else if (axState[0] == '2')
512 : {
513 0 : m_moving = 1;
514 0 : if(m_moveOp)
515 : {
516 0 : state(stateCodes::OPERATING);
517 : }
518 : }
519 0 : else if (axState[0] == '3' && isdigit(axState[1]))
520 : {
521 0 : if(m_wasHoming)
522 : {
523 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
524 0 : moveTo(m_homingOffset);
525 0 : m_wasHoming = 0;
526 0 : }
527 : else
528 : {
529 0 : m_moving = 0;
530 0 : state(stateCodes::READY);
531 0 : m_moveOp = true;
532 : }
533 : }
534 0 : else if (axState[0] == '3')
535 : {
536 0 : log<text_log>("Stage disabled. Enabling");
537 0 : std::string com;
538 0 : if(makeCom(com, "MM1") < 0)
539 : {
540 0 : log<software_error>({__FILE__, __LINE__,"Error making command PW1" });
541 0 : return 0;
542 : }
543 0 : int rv = MagAOX::tty::ttyWrite( com, m_fileDescrip, m_writeTimeout);
544 0 : if (rv != TTY_E_NOERROR)
545 : {
546 0 : if(m_powerTargetState == 0) return 0;
547 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
548 0 : return -1;
549 : }
550 0 : }
551 0 : else if( axState[0] == '\0' && axState[1] == '\0' )
552 : {
553 : //a non-response, this means we should go back around and ask again
554 : //this doesn't seem to be an error, but isn't documented.
555 : //Occurs after a power off, but also sometimes after homing completes.
556 0 : return 0;
557 : }
558 : else
559 : {
560 0 : sleep(1);
561 0 : if(m_powerState == 0) return 0;
562 :
563 0 : log<software_error>({__FILE__,__LINE__, "Invalid state: |" + std::to_string(axState[0]) + "|" + std::to_string(axState[1]) + "|"});
564 0 : state(stateCodes::ERROR);
565 : }
566 :
567 0 : if( state() == stateCodes::NOTHOMED)
568 : {
569 0 : if(m_powerOnHome && !m_powerOnHomed)
570 : {
571 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
572 0 : startHoming();
573 0 : m_powerOnHomed = true;
574 0 : }
575 0 : return 0;
576 : }
577 :
578 0 : if( state() == stateCodes::READY || state() == stateCodes::OPERATING)
579 : {
580 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
581 :
582 :
583 0 : int rv = getPosition(m_position);
584 :
585 0 : if(rv < 0)
586 : {
587 0 : sleep(1);
588 0 : if(m_powerState == 0) return 0;
589 :
590 0 : std::string errorString;
591 :
592 0 : if (getLastError(errorString) != 0 && errorString.size() != 0)
593 : {
594 0 : log<software_error>({__FILE__, __LINE__,errorString});
595 : }
596 :
597 0 : log<software_error>({__FILE__, __LINE__,"There's been an error with getting current controller position."});
598 0 : }
599 :
600 0 : recordPosition();
601 :
602 0 : updateIfChanged(m_indiP_position, "current", m_position);
603 :
604 : static int last_moving = -1;
605 :
606 0 : bool changed = false;
607 0 : if(last_moving != m_moving)
608 : {
609 0 : changed = true;
610 0 : last_moving = m_moving;
611 : }
612 :
613 0 : if(changed)
614 : {
615 0 : if(m_moving)
616 : {
617 0 : m_indiP_position.setState(INDI_BUSY);
618 : }
619 : else
620 : {
621 0 : m_indiP_position.setState(INDI_IDLE);
622 0 : m_indiP_position["target"] = m_position;
623 : }
624 0 : m_indiDriver->sendSetProperty(m_indiP_position);
625 : }
626 :
627 :
628 0 : int n = presetNumber();
629 0 : if(n == -1)
630 : {
631 0 : m_preset = 0;
632 0 : m_preset_target = 0;
633 : }
634 : else
635 : {
636 0 : m_preset = n;
637 0 : m_preset_target = n;
638 : }
639 :
640 0 : dev::stdMotionStage<smc100ccCtrl>::updateINDI();
641 0 : recordStage();
642 :
643 0 : return 0;
644 0 : }
645 :
646 0 : if( state() == stateCodes::ERROR )
647 : {
648 0 : if(m_powerTargetState == 0) return 0;
649 0 : sleep(1);
650 0 : if(m_powerState == 0) return 0;
651 :
652 0 : int rv = tty::usbDevice::getDeviceName();
653 0 : if(rv < 0 && rv != TTY_E_DEVNOTFOUND && rv != TTY_E_NODEVNAMES)
654 : {
655 0 : state(stateCodes::FAILURE);
656 0 : if(!stateLogged())
657 : {
658 0 : log<software_critical>({__FILE__, __LINE__, rv, tty::ttyErrorString(rv)});
659 : }
660 0 : return rv;
661 : }
662 :
663 0 : if(rv == TTY_E_DEVNOTFOUND || rv == TTY_E_NODEVNAMES)
664 : {
665 0 : state(stateCodes::NODEVICE);
666 :
667 0 : if(!stateLogged())
668 : {
669 0 : std::stringstream logs;
670 0 : logs << "USB Device " << m_idVendor << ":" << m_idProduct << ":" << m_serial << " not found in udev";
671 0 : log<text_log>(logs.str());
672 0 : }
673 0 : return 0;
674 : }
675 :
676 0 : sleep(1);
677 0 : if(m_powerState == 0) return 0;
678 :
679 0 : state(stateCodes::FAILURE);
680 0 : if(!stateLogged())
681 : {
682 0 : log<text_log>("Error NOT due to loss of USB connection. I can't fix it myself.", logPrio::LOG_CRITICAL);
683 : }
684 0 : return -1;
685 : }
686 :
687 0 : return 0;
688 0 : }
689 :
690 0 : int smc100ccCtrl::testConnection()
691 : {
692 : #ifdef XWC_SIM_MODE
693 0 : return 0;
694 : #endif
695 :
696 : std::string buffer{"1TS\r\n"};
697 : std::string output;
698 :
699 : int rv = MagAOX::tty::ttyWriteRead( output, buffer, "\r\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
700 :
701 : if (rv != TTY_E_NOERROR)
702 : {
703 : if(m_powerTargetState == 0) return -1;
704 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
705 : return -1;
706 : }
707 :
708 : /*int axis;
709 : std::string com;
710 : std::string val;
711 :
712 : splitResponse( axis, com, val, output);*/
713 :
714 : return 0;
715 : }
716 :
717 0 : int smc100ccCtrl::appShutdown()
718 : {
719 0 : return 0;
720 : }
721 :
722 0 : int smc100ccCtrl::makeCom( std::string &str,
723 : const std::string & com
724 : )
725 : {
726 : char tmp[10];
727 :
728 0 : int axis = 1;
729 :
730 0 : snprintf(tmp, 10, "%i", axis);
731 :
732 0 : str = tmp;
733 :
734 0 : str += com;
735 :
736 0 : str += "\r\n";
737 :
738 0 : return 0;
739 : }
740 0 : int smc100ccCtrl::splitResponse(int &axis, std::string &com, std::string &val, std::string &resp)
741 : {
742 0 : if(resp.length() < 3)
743 : {
744 0 : log<software_error>({__FILE__,__LINE__, "Invalid response"});
745 0 : return -1;
746 : }
747 :
748 0 : if(isalpha(resp[0]))
749 : {
750 0 : log<software_error>({__FILE__,__LINE__, "Invalid response"});
751 0 : axis = 0;
752 0 : com = "";
753 0 : val = resp;
754 0 : return 0;
755 : }
756 :
757 0 : if(isalpha(resp[1]))
758 : {
759 0 : axis = resp[0] - '0';
760 : }
761 : else
762 : {
763 0 : axis = atoi(resp.substr(0,2).c_str());
764 : }
765 :
766 0 : if(axis < 10)
767 : {
768 :
769 0 : com = resp.substr(1,2);
770 0 : if(resp.length() < 4 ) val = "";
771 0 : else val = resp.substr(3, resp.length()-3);
772 0 : if(val.size() > 1)
773 : {
774 0 : while(val[val.size()-1] == '\r' || val[val.size()-1] == '\n')
775 : {
776 0 : val.erase(val.size()-1);
777 0 : if(val.size() < 1) break;
778 : }
779 : }
780 : }
781 : else
782 : {
783 0 : if(resp.length() < 4)
784 : {
785 0 : log<software_error>({__FILE__,__LINE__, "Invalid response"});
786 0 : com = "";
787 0 : val = "";
788 0 : return -1;
789 : }
790 0 : com = resp.substr(2,2);
791 0 : if(resp.length() < 5) val = "";
792 0 : else val = resp.substr(4, resp.length()-4);
793 :
794 0 : if(val.size() > 1)
795 : {
796 0 : while(val[val.size()-1] == '\r' || val[val.size()-1] == '\n')
797 : {
798 0 : val.erase(val.size()-1);
799 0 : if(val.size() < 1) break;
800 : }
801 : }
802 : }
803 :
804 0 : return 0;
805 : }
806 :
807 0 : int smc100ccCtrl::getCtrlState( std::string &state )
808 : {
809 0 : std::string com, resp;
810 :
811 0 : if(makeCom(com, "TS") < 0)
812 : {
813 0 : log<software_error>({__FILE__, __LINE__,"Error making command TS" });
814 0 : return 0;
815 : }
816 :
817 0 : int rv = MagAOX::tty::ttyWriteRead( resp, com, "\r\n", false, m_fileDescrip, m_readTimeout, m_writeTimeout);
818 0 : if (rv != TTY_E_NOERROR)
819 : {
820 0 : if(m_powerTargetState == 0) return -1;
821 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
822 0 : return -1;
823 : }
824 :
825 : //std::cerr << "TS Response: " << resp << "\n";
826 : int raxis;
827 0 : std::string rcom, rval;
828 :
829 0 : splitResponse(raxis, rcom, rval, resp);
830 :
831 0 : if(rcom == "")
832 : {
833 0 : log<software_error>({__FILE__, __LINE__, "An Error occurred"});
834 0 : return -1;
835 : }
836 :
837 0 : if(raxis != 1)
838 : {
839 0 : log<software_error>({__FILE__, __LINE__, "Wrong axis returned"});
840 0 : return -1;
841 : }
842 :
843 0 : if(rcom != "TS")
844 : {
845 0 : log<software_error>({__FILE__, __LINE__, "Wrong command returned"});
846 0 : return -1;
847 : }
848 :
849 0 : if(rval.length() != 6)
850 : {
851 0 : log<software_error>({__FILE__, __LINE__,"Incorrect response length" });
852 0 : return -1;
853 : }
854 :
855 0 : state = rval.substr(4, 2);
856 :
857 0 : return 0;
858 0 : }
859 :
860 :
861 0 : int smc100ccCtrl::getPosition(double& current)
862 : {
863 0 : std::string buffer{"1TP\r\n"};
864 0 : std::string output;
865 0 : int rv = MagAOX::tty::ttyWriteRead( output, buffer, "\r\n", false, m_fileDescrip, m_writeTimeout, m_readTimeout);
866 :
867 0 : if (rv != TTY_E_NOERROR)
868 : {
869 0 : if(m_powerTargetState == 0) return -1;
870 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
871 0 : return -1;
872 : }
873 :
874 : // Parse current and place into argument
875 : try
876 : {
877 0 : current = std::stod(output.substr(3));
878 : }
879 0 : catch (...)
880 : {
881 0 : log<software_error>({__FILE__, __LINE__,"Error occured: Unexpected output in getPosition()"});
882 0 : return -1;
883 0 : }
884 0 : return 0;
885 0 : }
886 :
887 0 : int smc100ccCtrl::getLastError( std::string& errorString)
888 : {
889 0 : std::string buffer{"1TE\r\n"};
890 0 : std::string output;
891 0 : int rv = MagAOX::tty::ttyWriteRead( output, buffer, "\r\n",false, m_fileDescrip, m_writeTimeout, m_readTimeout);
892 :
893 0 : if (rv != TTY_E_NOERROR)
894 : {
895 0 : if(m_powerTargetState == 0) return -1;
896 0 : log<software_error>({__FILE__, __LINE__});
897 0 : std::cerr << __FILE__ << " " << __LINE__ << " " << rv << "\n";
898 :
899 0 : errorString = MagAOX::tty::ttyErrorString(rv);
900 0 : return -1;
901 : }
902 :
903 : char status;
904 : try
905 : {
906 0 : status = output.at(3);
907 : }
908 0 : catch (const std::out_of_range& oor)
909 : {
910 0 : log<software_error>({__FILE__, __LINE__});
911 0 : errorString = "Unknown output; controller not responding correctly.";
912 0 : return -1;
913 0 : }
914 :
915 0 : if (status == '@')
916 : {
917 0 : return 0;
918 : }
919 : else
920 : {
921 0 : switch(status)
922 : {
923 0 : case 'A':
924 0 : errorString = "Unknown message code or floating point controller address.";
925 0 : break;
926 0 : case 'B':
927 0 : errorString = "Controller address not correct.";
928 0 : break;
929 0 : case 'C':
930 0 : errorString = "Parameter missing or out of range.";
931 0 : break;
932 0 : case 'D':
933 0 : errorString = "Command not allowed.";
934 0 : break;
935 0 : case 'E':
936 0 : errorString = "Home sequence already started.";
937 0 : break;
938 0 : case 'F':
939 0 : errorString = "ESP stage name unknown.";
940 0 : break;
941 0 : case 'G':
942 0 : errorString = "Displacement out of limits.";
943 0 : break;
944 0 : case 'H':
945 0 : errorString = "Command not allowed in NOT REFERENCED state.";
946 0 : break;
947 0 : case 'I':
948 0 : errorString = "Command not allowed in CONFIGURATION state.";
949 0 : break;
950 0 : case 'J':
951 0 : errorString = "Command not allowed in DISABLE state.";
952 0 : break;
953 0 : case 'K':
954 0 : errorString = "Command not allowed in READY state.";
955 0 : break;
956 0 : case 'L':
957 0 : errorString = "Command not allowed in HOMING state.";
958 0 : break;
959 0 : case 'M':
960 0 : errorString = "UCommand not allowed in MOVING state.";
961 0 : break;
962 0 : case 'N':
963 0 : errorString = "Current position out of software limit.";
964 0 : break;
965 0 : case 'S':
966 0 : errorString = "Communication Time Out.";
967 0 : break;
968 0 : case 'U':
969 0 : errorString = "Error during EEPROM access.";
970 0 : break;
971 0 : case 'V':
972 0 : errorString = "Error during command execution.";
973 0 : break;
974 0 : case 'W':
975 0 : errorString = "Command not allowed for PP version.";
976 0 : break;
977 0 : case 'X':
978 0 : errorString = "Command not allowed for CC version.";
979 0 : break;
980 0 : default:
981 0 : errorString = "unknown status";
982 0 : std::cerr << "unkown status: " << status << "\n";
983 : }
984 :
985 0 : log<software_error>({__FILE__, __LINE__});
986 0 : return -1;
987 : }
988 0 : }
989 :
990 3 : INDI_NEWCALLBACK_DEFN(smc100ccCtrl, m_indiP_position)(const pcf::IndiProperty &ipRecv)
991 : {
992 3 : INDI_VALIDATE_CALLBACK_PROPS(m_indiP_position, ipRecv);
993 :
994 : if(!( state() == stateCodes::READY || state() == stateCodes::OPERATING))
995 : {
996 : log<text_log>("can not command position in current state");
997 : return 0;
998 : }
999 :
1000 :
1001 : float current = -1e55, target = -1e55;
1002 :
1003 : try
1004 : {
1005 : current = ipRecv["current"].get<float>();
1006 : }
1007 : catch(...){}
1008 :
1009 : try
1010 : {
1011 : target = ipRecv["target"].get<float>();
1012 : }
1013 : catch(...){}
1014 :
1015 : if(target == -1e55) target = current;
1016 :
1017 : if(target == -1e55) return 0;
1018 :
1019 : //Lock the mutex, waiting if necessary
1020 : std::unique_lock<std::mutex> lock(m_indiMutex);
1021 :
1022 : updateIfChanged(m_indiP_position, "target", target, INDI_BUSY);
1023 : m_target = target;
1024 :
1025 :
1026 : return moveTo(target);
1027 :
1028 : }
1029 :
1030 0 : int smc100ccCtrl::stop()
1031 : {
1032 0 : recordStage(true);
1033 0 : recordPosition(true);
1034 :
1035 : //don't lock mutex -- this must go through
1036 :
1037 0 : std::string buffer{"1ST\r\n"};
1038 0 : int rv = MagAOX::tty::ttyWrite(buffer, m_fileDescrip, m_writeTimeout);
1039 :
1040 0 : updateSwitchIfChanged(m_indiP_stop, "request", pcf::IndiElement::Off, INDI_IDLE);
1041 :
1042 0 : if (rv != TTY_E_NOERROR)
1043 : {
1044 0 : if(m_powerTargetState == 0) return -1;
1045 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
1046 0 : return -1;
1047 : }
1048 0 : return 0;
1049 0 : }
1050 :
1051 0 : int smc100ccCtrl::startHoming()
1052 : {
1053 0 : updateSwitchIfChanged(m_indiP_home, "request", pcf::IndiElement::Off, INDI_IDLE);
1054 :
1055 0 : recordStage(true);
1056 0 : recordPosition(true);
1057 :
1058 0 : std::string buffer{"1OR\r\n"};
1059 0 : int rv = MagAOX::tty::ttyWrite(buffer, m_fileDescrip, m_writeTimeout);
1060 :
1061 0 : if (rv != TTY_E_NOERROR)
1062 : {
1063 0 : if(m_powerTargetState == 0) return -1;
1064 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
1065 0 : return -1;
1066 : }
1067 0 : return 0;
1068 0 : }
1069 :
1070 0 : double smc100ccCtrl::presetNumber()
1071 : {
1072 0 : for( size_t n=1; n < m_presetPositions.size(); ++n)
1073 : {
1074 0 : if( fabs(m_position-m_presetPositions[n]) < 1e-3) return n;
1075 : }
1076 :
1077 0 : return 0;
1078 : }
1079 :
1080 0 : int smc100ccCtrl::moveTo(double position)
1081 : {
1082 0 : recordStage(true);
1083 0 : recordPosition(true);
1084 :
1085 0 : if(fabs(position-m_position) > m_opDelta)
1086 : {
1087 0 : m_moveOp = true;
1088 : }
1089 : else
1090 : {
1091 0 : m_moveOp = false;
1092 : }
1093 :
1094 0 : std::string buffer{"1PA"};
1095 0 : buffer = buffer + std::to_string(position) + "\r\n";
1096 :
1097 0 : int rv = MagAOX::tty::ttyWrite( buffer, m_fileDescrip, m_writeTimeout);
1098 :
1099 0 : if (rv != TTY_E_NOERROR)
1100 : {
1101 0 : if(m_powerTargetState == 0) return -1;
1102 0 : log<software_error>({__FILE__, __LINE__,MagAOX::tty::ttyErrorString(rv)});
1103 0 : return -1;
1104 : }
1105 :
1106 0 : std::string errorString;
1107 0 : if (getLastError(errorString) == 0)
1108 : {
1109 0 : if(m_moveOp)
1110 : {
1111 0 : state(stateCodes::OPERATING);
1112 : }
1113 0 : updateIfChanged(m_indiP_position, "target", position);
1114 0 : return 0;
1115 : }
1116 : else
1117 : {
1118 0 : log<software_error>({__FILE__, __LINE__,errorString});
1119 0 : return -1;
1120 : }
1121 0 : }
1122 :
1123 0 : int smc100ccCtrl::checkRecordTimes()
1124 : {
1125 0 : return telemeter<smc100ccCtrl>::checkRecordTimes(telem_stage(), telem_position());
1126 : }
1127 :
1128 0 : int smc100ccCtrl::recordTelem( const telem_stage * )
1129 : {
1130 0 : return recordStage(true);
1131 : }
1132 :
1133 0 : int smc100ccCtrl::recordTelem( const telem_position * )
1134 : {
1135 0 : return recordPosition(true);
1136 : }
1137 :
1138 0 : int smc100ccCtrl::recordStage(bool force)
1139 : {
1140 0 : return stdMotionStage<smc100ccCtrl>::recordStage(force);
1141 : }
1142 :
1143 0 : int smc100ccCtrl::recordPosition(bool force)
1144 : {
1145 : static float last_position = 1e30;
1146 :
1147 0 : float fpos = m_position;
1148 :
1149 0 : if( fpos != last_position || force )
1150 : {
1151 0 : telem<telem_position>(fpos);
1152 0 : last_position = fpos;
1153 : }
1154 :
1155 0 : return 0;
1156 : }
1157 :
1158 : } //namespace app
1159 : } //namespace MagAOX
1160 :
1161 : #endif //smc100ccCtrl_hpp
|
