usermotor.cpp
1 /***************************************************************************
2  Copyright (C) 2002-2015 Kentaro Kitagawa
3  kitagawa@phys.s.u-tokyo.ac.jp
4 
5  This program is free software; you can redistribute it and/or
6  modify it under the terms of the GNU Library General Public
7  License as published by the Free Software Foundation; either
8  version 2 of the License, or (at your option) any later version.
9 
10  You should have received a copy of the GNU Library General
11  Public License and a list of authors along with this program;
12  see the files COPYING and AUTHORS.
13 ***************************************************************************/
14 
15 #include "usermotor.h"
16 //---------------------------------------------------------------------------
17 
18 REGISTER_TYPE(XDriverList, FlexCRK, "OrientalMotor FLEX CRK motor controller");
19 REGISTER_TYPE(XDriverList, FlexAR, "OrientalMotor FLEX AR/DG2 motor controller");
20 REGISTER_TYPE(XDriverList, EMP401, "OrientalMotor EMP401 motor controller");
21 
22 XFlexCRK::XFlexCRK(const char *name, bool runtime,
23  Transaction &tr_meas, const shared_ptr<XMeasure> &meas) :
24  XModbusRTUDriver<XMotorDriver>(name, runtime, ref(tr_meas), meas) {
25  interface()->setSerialBaudRate(57600);
26  interface()->setSerialStopBits(1);
27  interface()->setSerialParity(XCharInterface::PARITY_EVEN);
28 }
29 void
30 XFlexCRK::storeToROM() {
31  XScopedLock<XInterface> lock( *interface());
32  interface()->presetSingleResistor(0x45, 1); //RAM to NV.
33  interface()->presetSingleResistor(0x45, 0);
34 }
35 void
36 XFlexCRK::clearPosition() {
37  XScopedLock<XInterface> lock( *interface());
38  interface()->presetSingleResistor(0x4b, 1); //counter clear.
39  interface()->presetSingleResistor(0x4b, 0);
40 }
41 void
42 XFlexCRK::getStatus(const Snapshot &shot, double *position, bool *slipping, bool *ready) {
43  XScopedLock<XInterface> lock( *interface());
44  uint32_t output = interface()->readHoldingTwoResistors(0x20); //reading status1:status2
45  *slipping = output & 0x2000000u;
46  if(output & 0x80) {
47  uint16_t alarm = interface()->readHoldingSingleResistor(0x100);
48  gErrPrint(getLabel() + i18n(" Alarm %1 has been emitted").arg((int)alarm));
49  interface()->presetSingleResistor(0x40, 1); //clears alarm.
50  interface()->presetSingleResistor(0x40, 0);
51  }
52  if(output & 0x40) {
53  uint16_t warn = interface()->readHoldingSingleResistor(0x10b);
54  gWarnPrint(getLabel() + i18n(" Code = %1").arg((int)warn));
55  }
56 // uint32_t ierr = interface()->readHoldingTwoResistors(0x128);
57 // if(ierr) {
58 // gErrPrint(getLabel() + i18n(" Interface error %1 has been emitted").arg((int)ierr));
59 // }
60  *ready = (output & 0x20000000u);
61 // fprintf(stderr, "0x20:%x\n", (unsigned int)output);
62  if(shot[ *hasEncoder()])
63  *position = static_cast<int32_t>(interface()->readHoldingTwoResistors(0x11e))
64  * 360.0 / (double)shot[ *stepEncoder()];
65  else
66  *position = static_cast<int32_t>(interface()->readHoldingTwoResistors(0x118))
67  * 360.0 / (double)shot[ *stepMotor()];
68 }
69 void
70 XFlexCRK::changeConditions(const Snapshot &shot) {
71  XScopedLock<XInterface> lock( *interface());
72  interface()->presetSingleResistor(0x21e, lrint(shot[ *currentRunning()]));
73  interface()->presetSingleResistor(0x21f, lrint(shot[ *currentStopping()]));
74  interface()->presetSingleResistor(0x236, 0); //common setting for acc/dec.
75  interface()->presetTwoResistors(0x224, lrint(shot[ *timeAcc()] * 1e3));
76  interface()->presetTwoResistors(0x226, lrint(shot[ *timeDec()] * 1e3));
77  interface()->presetTwoResistors(0x312, lrint((double)shot[ *stepEncoder()]));
78  interface()->presetTwoResistors(0x314, lrint((double)shot[ *stepMotor()]));
79  interface()->presetTwoResistors(0x502, lrint(shot[ *speed()]));
80  unsigned int microstep = shot[ *microStep()] ? 6 : 0;
81  if(interface()->readHoldingSingleResistor(0x311) != microstep) {
82  gWarnPrint(i18n("Store settings to NV memory and restart, microstep div.=10."));
83  interface()->presetSingleResistor(0x311, microstep); //division = 10.
84  }
85 }
86 void
87 XFlexCRK::getConditions(Transaction &tr) {
88  XScopedLock<XInterface> lock( *interface());
89  interface()->diagnostics();
90  tr[ *currentRunning()] = interface()->readHoldingSingleResistor(0x21e);
91  tr[ *currentStopping()] = interface()->readHoldingSingleResistor(0x21f);
92  tr[ *microStep()] = (interface()->readHoldingSingleResistor(0x311) != 0);
93  tr[ *timeAcc()] = interface()->readHoldingTwoResistors(0x224) * 1e-3;
94  tr[ *timeDec()] = interface()->readHoldingTwoResistors(0x226) * 1e-3;
95  tr[ *stepEncoder()] = interface()->readHoldingTwoResistors(0x312);
96  tr[ *stepMotor()] = interface()->readHoldingTwoResistors(0x314);
97  tr[ *speed()] = interface()->readHoldingTwoResistors(0x502);
98  tr[ *target()] = static_cast<int32_t>(interface()->readHoldingTwoResistors(0x402))
99  * 360.0 / tr[ *stepMotor()];
100  tr[ *round()].setUIEnabled(false);
101  tr[ *roundBy()].setUIEnabled(false);
102  tr[ *active()] = (interface()->readHoldingSingleResistor(0x1e) & 0x2000u) == 1; //CON
103  interface()->presetSingleResistor(0x203, 0); //STOP I/O normally open.
104  interface()->presetSingleResistor(0x200, 0); //START by RS485.
105  interface()->presetSingleResistor(0x20b, 0); //C-ON by RS485.
106  interface()->presetSingleResistor(0x20c, 0); //HOME/FWD/RVS by RS485.
107  interface()->presetSingleResistor(0x20d, 0); //No. by RS485.
108  interface()->presetSingleResistor(0x202, 1); //Dec. after STOP.
109  interface()->presetSingleResistor(0x601, 1); //Absolute.
110 }
111 void
112 XFlexCRK::stopRotation() {
113  sendStopSignal(false);
114 }
115 void
116 XFlexCRK::sendStopSignal(bool wait) {
117  for(int i = 0;; ++i) {
118  uint32_t output = interface()->readHoldingTwoResistors(0x20); //reading status1:status2
119  bool isready = (output & 0x20000000u);
120  if(isready) break;
121  if(i ==0) {
122  interface()->presetSingleResistor(0x1e, 0x3001u); //C-ON, STOP, M0
123  interface()->presetSingleResistor(0x1e, 0x2001u); //C-ON, M0
124  if( !wait)
125  break;
126  }
127  msecsleep(100);
128  if(i > 10) {
129  throw XInterface::XInterfaceError(i18n("Motor is still not ready"), __FILE__, __LINE__);
130  }
131  }
132 }
133 void
134 XFlexCRK::setForward() {
135  XScopedLock<XInterface> lock( *interface());
136  interface()->presetSingleResistor(0x1e, 0x2201u); //C-ON, FWD, M0
137 }
138 void
139 XFlexCRK::setReverse() {
140  XScopedLock<XInterface> lock( *interface());
141  interface()->presetSingleResistor(0x1e, 0x2401u); //C-ON, RVS, M0
142 }
143 void
144 XFlexCRK::setTarget(const Snapshot &shot, double target) {
145  XScopedLock<XInterface> lock( *interface());
146  sendStopSignal(true);
147  interface()->presetTwoResistors(0x402, lrint(target / 360.0 * shot[ *stepMotor()]));
148  interface()->presetSingleResistor(0x1e, 0x2101u); //C-ON, START, M0
149  interface()->presetSingleResistor(0x1e, 0x2001u); //C-ON, M0
150 }
151 void
152 XFlexCRK::setActive(bool active) {
153  XScopedLock<XInterface> lock( *interface());
154  if(active) {
155  interface()->presetSingleResistor(0x1e, 0x2001u); //C-ON, M0
156  }
157  else {
158  sendStopSignal(true);
159  interface()->presetSingleResistor(0x1e, 0x0001u); //M0
160  }
161 }
162 void
163 XFlexCRK::setAUXBits(unsigned int bits) {
164  interface()->presetSingleResistor(0x206, 11); //OUT1 to R-OUT1
165  interface()->presetSingleResistor(0x207, 12); //OUT2 to R-OUT2
166  interface()->presetSingleResistor(0x208, 15); //OUT3 to R-OUT3
167  interface()->presetSingleResistor(0x209, 16); //OUT4 to R-OUT4
168  interface()->presetSingleResistor(0x1f, bits & 0xfu);
169 }
170 
171 void
172 XFlexAR::storeToROM() {
173  XScopedLock<XInterface> lock( *interface());
174  interface()->presetTwoResistors(0x192, 1); //RAM to NV.
175  interface()->presetTwoResistors(0x192, 0);
176 }
177 void
178 XFlexAR::clearPosition() {
179  XScopedLock<XInterface> lock( *interface());
180  interface()->presetTwoResistors(0x18a, 1); //counter clear.
181  interface()->presetTwoResistors(0x18a, 0);
182 }
183 void
184 XFlexAR::getStatus(const Snapshot &shot, double *position, bool *slipping, bool *ready) {
185  XScopedLock<XInterface> lock( *interface());
186  uint32_t output = interface()->readHoldingTwoResistors(0x7e);
187  *ready = output & 0x20;
188  *slipping = output & 0x8000;
189  if(output & 0x80) {
190  uint32_t alarm = interface()->readHoldingTwoResistors(0x80);
191  gErrPrint(getLabel() + i18n(" Alarm %1 has been emitted").arg((int)alarm));
192  interface()->presetTwoResistors(0x180, 1); //clears alarm.
193  interface()->presetTwoResistors(0x180, 0);
194  interface()->presetTwoResistors(0x182, 1); //clears abs. pos. alarm.
195  interface()->presetTwoResistors(0x182, 0);
196  interface()->presetTwoResistors(0x184, 1); //clears alarm history.
197  interface()->presetTwoResistors(0x184, 0);
198  }
199  if(output & 0x40) {
200  uint32_t warn = interface()->readHoldingTwoResistors(0x96);
201  gWarnPrint(getLabel() + i18n(" Code = %1").arg((int)warn));
202  }
203  if(shot[ *hasEncoder()])
204  *position = static_cast<int32_t>(interface()->readHoldingTwoResistors(0xcc))
205  * 360.0 / (double)shot[ *stepEncoder()];
206  else
207  *position = static_cast<int32_t>(interface()->readHoldingTwoResistors(0xc6))
208  * 360.0 / (double)shot[ *stepMotor()];
209 }
210 void
211 XFlexAR::changeConditions(const Snapshot &shot) {
212  XScopedLock<XInterface> lock( *interface());
213  interface()->presetTwoResistors(0x240, lrint(shot[ *currentRunning()] * 10.0));
214  interface()->presetTwoResistors(0x242, lrint(shot[ *currentStopping()] * 10.0));
215  interface()->presetTwoResistors(0x28c, 0); //common setting for acc/dec.
216  interface()->presetTwoResistors(0x280, lrint(shot[ *timeAcc()] * 1e3));
217  interface()->presetTwoResistors(0x282, lrint(shot[ *timeDec()] * 1e3));
218  interface()->presetTwoResistors(0x284, 0); //starting speed.
219  interface()->presetTwoResistors(0x480, lrint(shot[ *speed()]));
220 
221  bool conf_needed = false;
222  if(shot[ *stepMotor()] != interface()->readHoldingTwoResistors(0x380) * 1000.0 / interface()->readHoldingTwoResistors(0x382)) {
223  conf_needed = true;
224  int b = 1;
225  if(int x = shot[ *stepMotor()] % 1000)
226  b = 1000 / x;
227  b = std::min(b, 10);
228  int a = lrint(shot[ *stepMotor()]/1000.0*b);
229  interface()->presetTwoResistors(0x380, a); //A
230  interface()->presetTwoResistors(0x382, b); //B, rot=1000B/A
231  }
232  interface()->presetTwoResistors(0x1002, shot[ *stepEncoder()] / shot[ *stepMotor()]); //Multiplier is stored in MS2 No.
233  int b_micro = shot[ *microStep()] ? 1 : 0;
234  if(interface()->readHoldingTwoResistors(0x1028) != b_micro) {
235  conf_needed = true;
236  interface()->presetTwoResistors(0x1028, b_micro);
237  }
238  int b_round = shot[ *round()] ? 1 : 0;
239  if(interface()->readHoldingTwoResistors(0x38e) != b_round) {
240  conf_needed = true;
241  interface()->presetTwoResistors(0x38e, b_round);
242  }
243  int num_round = std::max(lrint((double)shot[ *roundBy()]), 1L);
244  if(interface()->readHoldingTwoResistors(0x390) != num_round) {
245  conf_needed = true;
246  interface()->presetTwoResistors(0x390, num_round);
247  interface()->presetTwoResistors(0x20a, lrint((double)shot[ *roundBy()]) / 2); //AREA1+
248  interface()->presetTwoResistors(0x20c, 0); //AREA1-
249  }
250 
251  if(conf_needed) {
252  sendStopSignal(true);
253  interface()->presetTwoResistors(0x18c, 1);
254  interface()->presetTwoResistors(0x18c, 0);
255  }
256 }
257 void
258 XFlexAR::getConditions(Transaction &tr) {
259  XScopedLock<XInterface> lock( *interface());
260  interface()->diagnostics();
261  tr[ *currentRunning()] = interface()->readHoldingTwoResistors(0x240) * 0.1;
262  tr[ *currentStopping()] = interface()->readHoldingTwoResistors(0x242) * 0.1;
263  tr[ *microStep()] = (interface()->readHoldingTwoResistors(0x1028) != 0);
264  tr[ *timeAcc()] = interface()->readHoldingTwoResistors(0x280) * 1e-3;
265  tr[ *timeDec()] = interface()->readHoldingTwoResistors(0x282) * 1e-3;
266  tr[ *stepMotor()] = interface()->readHoldingTwoResistors(0x380) * 1000.0 / interface()->readHoldingTwoResistors(0x382);
267  tr[ *stepEncoder()] = tr[ *stepMotor()] * interface()->readHoldingTwoResistors(0x1002); //Multiplier is stored in MS2 No.
268  tr[ *hasEncoder()] = true;
269  tr[ *speed()] = interface()->readHoldingTwoResistors(0x480);
270  tr[ *target()] = static_cast<int32_t>(interface()->readHoldingTwoResistors(0x400))
271  * 360.0 / tr[ *stepMotor()];
272  tr[ *round()] = (interface()->readHoldingTwoResistors(0x38e) == 1);
273  tr[ *roundBy()] = interface()->readHoldingTwoResistors(0x390);
274  tr[ *active()] = (interface()->readHoldingTwoResistors(0x7c) & 0x40u) == 0; //FREE
275 
276  interface()->presetTwoResistors(0x200, 3); //Inactive after stop.
277  interface()->presetTwoResistors(0x500, 1); //Absolute.
278  interface()->presetTwoResistors(0x119e, 71); //NET-OUT15 = TLC
279  interface()->presetTwoResistors(0x1140, 32); //OUT0 to R0
280  interface()->presetTwoResistors(0x1142, 33); //OUT1 to R1
281 // interface()->presetTwoResistors(0x1144, 34); //OUT2 to R2
282  interface()->presetTwoResistors(0x1146, 35); //OUT3 to R3
283  interface()->presetTwoResistors(0x1148, 36); //OUT4 to R4
284  interface()->presetTwoResistors(0x114a, 37); //OUT5 to R5
285  interface()->presetTwoResistors(0x1160, 32); //NET-IN0 to R0
286  interface()->presetTwoResistors(0x1162, 33); //NET-IN1 to R1
287 // interface()->presetTwoResistors(0x1164, 34); //NET-IN2 to R2
288  interface()->presetTwoResistors(0x1166, 35); //NET-IN3 to R3
289  interface()->presetTwoResistors(0x1168, 36); //NET-IN4 to R4
290  interface()->presetTwoResistors(0x116a, 37); //NET-IN5 to R5
291 }
292 void
293 XFlexAR::setTarget(const Snapshot &shot, double target) {
294  XScopedLock<XInterface> lock( *interface());
295  sendStopSignal(true);
296  int steps = shot[ *hasEncoder()] ? shot[ *stepEncoder()] : shot[ *stepMotor()];
297  interface()->presetTwoResistors(0x400, lrint(target / 360.0 * steps));
298  interface()->presetTwoResistors(0x7c, 0x100u); //MS0
299  interface()->presetTwoResistors(0x7c, 0x0u);
300 }
301 void
302 XFlexAR::stopRotation() {
303  sendStopSignal(false);
304 }
305 void
306 XFlexAR::sendStopSignal(bool wait) {
307  for(int i = 0;; ++i) {
308  uint32_t output = interface()->readHoldingTwoResistors(0x7e);
309  bool isready = output & 0x20;
310  if(isready) break;
311  if(i ==0) {
312  interface()->presetTwoResistors(0x7c, 0x20u); //STOP
313  interface()->presetTwoResistors(0x7c, 0x0u);
314  if( !wait)
315  break;
316  }
317  msecsleep(150);
318  if(i > 10) {
319  throw XInterface::XInterfaceError(i18n("Motor is still not ready"), __FILE__, __LINE__);
320  }
321  }
322 }
323 void
324 XFlexAR::setForward() {
325  XScopedLock<XInterface> lock( *interface());
326  interface()->presetTwoResistors(0x7c, 0x4000u); //FWD
327 }
328 void
329 XFlexAR::setReverse() {
330  XScopedLock<XInterface> lock( *interface());
331  interface()->presetTwoResistors(0x7c, 0x8000u); //RVS
332 }
333 void
334 XFlexAR::setActive(bool active) {
335  XScopedLock<XInterface> lock( *interface());
336  if(active) {
337  interface()->presetTwoResistors(0x7c, 0x0u);
338  }
339  else {
340  sendStopSignal(true);
341  interface()->presetTwoResistors(0x7c, 0x40u); //FREE
342  }
343 }
344 void
345 XFlexAR::setAUXBits(unsigned int bits) {
346  interface()->presetSingleResistor(0x7d, bits & 0x3fu);
347 }
348 
349 XEMP401::XEMP401(const char *name, bool runtime,
350  Transaction &tr_meas, const shared_ptr<XMeasure> &meas) :
351  XCharDeviceDriver<XMotorDriver>(name, runtime, ref(tr_meas), meas) {
352  interface()->setSerialBaudRate(9600);
353 // interface()->setSerialStopBits(1);
354  interface()->setSerialParity(XCharInterface::PARITY_NONE);
355  interface()->setSerialHasEchoBack(true);
356  interface()->setEOS("\r\n");
357  stepEncoder()->disable();
358  hasEncoder()->disable();
359  timeDec()->disable();
360  round()->disable();
361  roundBy()->disable();
362  currentRunning()->disable();
363  currentStopping()->disable();
364  store()->disable();
365 }
366 void
367 XEMP401::storeToROM() {
368 }
369 void
370 XEMP401::clearPosition() {
371  XScopedLock<XInterface> lock( *interface());
372  interface()->send("RTNCR");
373  waitForCursor();
374 }
375 void
376 XEMP401::waitForCursor() {
377  for(;;) {
378  interface()->receive(1);
379  if(interface()->buffer()[0] == '>')
380  break;
381  }
382  msecsleep(10);
383 }
384 void
385 XEMP401::getStatus(const Snapshot &shot, double *position, bool *slipping, bool *ready) {
386  XScopedLock<XInterface> lock( *interface());
387  interface()->send("R");
388  for(;;) {
389  interface()->receive();
390  int x;
391  if(interface()->scanf(" PC =%d", &x) == 1) {
392  *position = x; // / (double)shot[ *stepMotor()];
393  break;
394  }
395  if(interface()->scanf(" Ready =%d", &x) == 1) {
396  *ready = (x != 0);
397  }
398  *slipping = false;
399  }
400  waitForCursor();
401 }
402 void
403 XEMP401::changeConditions(const Snapshot &shot) {
404  XScopedLock<XInterface> lock( *interface());
405  interface()->queryf("T,%d", (int)lrint(shot[ *timeAcc()] * 10));
406  double n2 = 1.0;
407  if(shot[ *microStep()])
408  n2 = 10;
409  waitForCursor();
410  interface()->queryf("UNIT,%.4f,%.1f", 1.0 / shot[ *stepMotor()], n2);
411  waitForCursor();
412  interface()->queryf("V,%d", (int)lrint(shot[ *speed()]));
413  waitForCursor();
414  interface()->queryf("VS,%d", (int)lrint(shot[ *speed()]));
415  waitForCursor();
416 }
417 void
418 XEMP401::getConditions(Transaction &tr) {
419  XScopedLock<XInterface> lock( *interface());
420  interface()->query("T");
421  int x;
422  if(interface()->scanf("%*d: T = %d", &x) != 1)
423  throw XInterface::XConvError(__FILE__, __LINE__);
424  tr[ *timeAcc()] = x * 0.1;
425 
426  waitForCursor();
427  interface()->query("V");
428  if(interface()->scanf("%*d: V = %d", &x) != 1)
429  throw XInterface::XConvError(__FILE__, __LINE__);
430  tr[ *speed()] = x;
431 
432  waitForCursor();
433  interface()->query("UNIT");
434  double n1,n2;
435  if(interface()->scanf("%*d: UNIT = %lf,%lf", &n1, &n2) != 2)
436  throw XInterface::XConvError(__FILE__, __LINE__);
437  tr[ *microStep()] = (n2 > 1.1);
438  tr[ *stepMotor()] = 1.0 / n1;
439  waitForCursor();
440 }
441 void
442 XEMP401::stopRotation() {
443  XScopedLock<XInterface> lock( *interface());
444  interface()->setSerialHasEchoBack(false);
445  interface()->write("\x1b", 1); //ESC.
446  interface()->setSerialHasEchoBack(true);
447  waitForCursor();
448  interface()->send("S");
449  waitForCursor();
450  interface()->query("ACTL 0,0,0,0");
451  waitForCursor();
452 }
453 void
454 XEMP401::setForward() {
455  XScopedLock<XInterface> lock( *interface());
456  stopRotation();
457  interface()->query("H,+");
458  waitForCursor();
459  interface()->send("SCAN");
460  waitForCursor();
461 }
462 void
463 XEMP401::setReverse() {
464  XScopedLock<XInterface> lock( *interface());
465  stopRotation();
466  interface()->query("H,-");
467  waitForCursor();
468  interface()->send("SCAN");
469  waitForCursor();
470 }
471 void
472 XEMP401::setTarget(const Snapshot &shot, double target) {
473  XScopedLock<XInterface> lock( *interface());
474  stopRotation();
475  interface()->queryf("D,%+.2f", target);
476  waitForCursor();
477  interface()->send("ABS");
478  waitForCursor();
479 }
480 void
481 XEMP401::setActive(bool active) {
482  XScopedLock<XInterface> lock( *interface());
483  if(active) {
484  }
485  else {
486  stopRotation();
487  }
488 }
489 void
490 XEMP401::setAUXBits(unsigned int bits) {
491  interface()->queryf("OUT,%1u%1u%1u%1u%1u%1u",
492  (bits / 32u) % 2u, (bits / 16u) % 2u, (bits / 8u) % 2u, (bits / 4u) % 2u, (bits / 2u) % 2u, bits % 2u);
493  waitForCursor();
494 }
Generated for KAME4 by  doxygen 1.8.3