1 #include "DPPanTiltControl.hh"
2 #include "Base/Common/W32Compat.hh"
3 #include "Base/Common/BIASpragma.hh"
12 DPPanTiltControl::DPPanTiltControl()
14 COMstream_ = PORT_NOT_OPENED;
15 bDefaultOrientation_ =
true;
16 maxSpeed_ = DPPTU_MAXSPEED;
17 panResolution_ = tiltResolution_ = DPPTU_DEF_RES;
20 DPPanTiltControl::~DPPanTiltControl()
24 int DPPanTiltControl::SetMode(
int nMode)
28 set_mode(POSITION_LIMITS_MODE, ON_MODE);
29 if (nMode == SPEEDMODE)
31 status = set_mode(SPEED_CONTROL_MODE,PTU_PURE_VELOCITY_SPEED_CONTROL_MODE);
33 std::cout <<
"Error : Set SPEED_CONTROL_MODE failed!" << std::endl;
36 std::cout <<
"DPPanTiltControl : SPEED_CONTROL_MODE set" << std::endl;
39 else if(nMode == ABSOLUTEMODE)
41 status = set_mode(COMMAND_EXECUTION_MODE,EXECUTE_IMMEDIATELY);
43 std::cout <<
"Error : Set COMMAND_EXECUTION_MODE/EXECUTE IMMEDIATELY failed!" << std::endl;
46 std::cout <<
"DPPanTiltControl : COMMAND_EXECUTION_MODE/EXECUTE IMMEDIATELY set" << std::endl;
49 std::cout <<
"Error : Wrong mode set for DPPanTiltControl!" << endl;
54 bool DPPanTiltControl::ResetPanTiltUnit()
60 switch (reset_PTU_parser(5000))
64 set_desired(PAN,SPEED, (PTU_PARM_PTR *) &maxSpeed_, ABSOLUTE);
65 set_desired(TILT,SPEED, (PTU_PARM_PTR *) &maxSpeed_, ABSOLUTE);
67 SetMode(ABSOLUTEMODE);
68 set_desired(PAN, POSITION, (PTU_PARM_PTR *) &steps[0], ABSOLUTE);
69 set_desired(TILT, POSITION, (PTU_PARM_PTR *) &steps[1], ABSOLUTE);
73 case PTU_FIRMWARE_VERSION_TOO_LOW:
76 case PTU_NOT_RESPONDING:
83 void DPPanTiltControl::StopPanTiltUnit()
86 set_desired(PAN,SPEED, (PTU_PARM_PTR *) &speed, ABSOLUTE);
87 set_desired(TILT,SPEED, (PTU_PARM_PTR *) &speed, ABSOLUTE);
90 void DPPanTiltControl::AwaitCompletion()
95 bool DPPanTiltControl::InitPanTiltUnit(
int nPort,
bool bUSB)
97 std::string portPrefix;
105 portPrefix =
"/dev/ttyUSB";
107 portPrefix =
"/dev/ttyS";
110 sprintf(portName,
"%s%d", portPrefix.c_str(), nPort);
111 return InitPanTiltUnit(portName);
114 bool DPPanTiltControl::InitPanTiltUnit(
const char *portName)
118 std::cout <<
"DPPanTiltControl : Opening port " << portName << std::endl;
120 COMstream_ = open_host_port((
char*)portName);
121 if (COMstream_ == PORT_NOT_OPENED) {
124 set_mode(DEFAULTS, RESTORE_FACTORY_SETTINGS);
125 switch (reset_PTU_parser(5000))
130 case PTU_FIRMWARE_VERSION_TOO_LOW:
131 std::cout <<
"Error : PTU firmware version is too low!" << std::endl;
134 case PTU_NOT_RESPONDING:
135 std::cout <<
"Error : PTU is not responding!" << std::endl;
140 char *fw = firmware_version();
141 char ok = firmware_version_OK();
144 std::cout <<
"Error : PTU firmware version is not OK!" << std::endl;
148 close_host_port(COMstream_);
151 QueryResolution(a,b,
true);
156 void DPPanTiltControl::ClosePanTiltUnit()
160 set_desired(PAN,POSITION,(PTU_PARM_PTR *) &nPanPos, ABSOLUTE);
161 set_desired(TILT,POSITION,(PTU_PARM_PTR *) &nTiltPos, ABSOLUTE);
162 set_desired(PAN,SPEED,(PTU_PARM_PTR *) &nPanPos, ABSOLUTE);
163 set_desired(TILT,SPEED,(PTU_PARM_PTR *) &nTiltPos, ABSOLUTE);
165 close_host_port(COMstream_);
168 char DPPanTiltControl::SetSpeed(
int nPanSpeed,
int nTiltSpeed)
171 status = set_desired(PAN,SPEED,(PTU_PARM_PTR *) &nPanSpeed, ABSOLUTE);
172 status = set_desired(TILT,SPEED,(PTU_PARM_PTR *) &nTiltSpeed, ABSOLUTE);
176 void DPPanTiltControl::GetSpeed(
long &panSpeed,
long &tiltSpeed)
178 panSpeed = get_current(PAN, SPEED);
179 tiltSpeed = get_current(TILT, SPEED);
182 char DPPanTiltControl::SetAcceleration(
int nPanAcc,
int nTiltAcc)
184 char status = set_desired(PAN,ACCELERATION,(PTU_PARM_PTR *) &nPanAcc, ABSOLUTE);
186 status = set_desired(TILT,ACCELERATION,(PTU_PARM_PTR *) &nTiltAcc, ABSOLUTE);
190 void DPPanTiltControl::GetAcceleration(
long &panAcc,
long &tiltAcc)
192 panAcc = get_current(PAN, ACCELERATION);
193 tiltAcc = get_current(TILT, ACCELERATION);
196 void DPPanTiltControl::MoveUp(
int nValue)
198 if (!bDefaultOrientation_)
200 set_desired(TILT,POSITION,(PTU_PARM_PTR *) &nValue, RELATIVE);
204 void DPPanTiltControl::MoveDown(
int nValue)
206 if (bDefaultOrientation_)
208 set_desired(TILT,POSITION,(PTU_PARM_PTR *) &nValue, RELATIVE);
212 void DPPanTiltControl::MoveLeft(
int nValue)
214 if (!bDefaultOrientation_)
216 set_desired(PAN,POSITION,(PTU_PARM_PTR *) &nValue, RELATIVE);
220 void DPPanTiltControl::MoveRight(
int nValue)
222 if (bDefaultOrientation_)
224 set_desired(PAN,POSITION,(PTU_PARM_PTR *) &nValue, RELATIVE);
228 void DPPanTiltControl::
229 GetCurrentPosition(
long &lPanPos,
long &lTiltPos)
231 lPanPos = get_current(PAN,POSITION);
232 lTiltPos = get_current(TILT,POSITION);
235 void DPPanTiltControl::
236 GetCurrentPositionDegrees(
double &dPanDeg,
double &dTiltDeg)
238 dPanDeg = get_current(PAN,POSITION) * panResolution_;
239 dTiltDeg = get_current(TILT,POSITION) * tiltResolution_;
242 void DPPanTiltControl::
243 GetCurrentPositionRadians(
double &dPanRad,
double &dTiltRad)
245 dPanRad = get_current(PAN,POSITION) * panResolution_ * M_PI / 180.0;
246 dTiltRad = get_current(TILT,POSITION) * tiltResolution_ * M_PI / 180.0;
249 void DPPanTiltControl::
250 SetPosition(
int nPanPos,
int nTiltPos,
bool bWaitComplete)
252 char status = set_desired(PAN,POSITION,(PTU_PARM_PTR *) &nPanPos, ABSOLUTE);
253 if (status != PTU_OK)
254 std::cout <<
"DPPanTiltControl : Setting pan position failed!" << std::endl;
256 status = set_desired(TILT,POSITION,(PTU_PARM_PTR *) &nTiltPos, ABSOLUTE);
257 if (status != PTU_OK)
258 std::cout <<
"DPPanTiltControl : Setting tilt position failed!" << std::endl;
264 void DPPanTiltControl::SendRawCommand(
string cmd)
266 SerialStringOut(COMstream_, (
unsigned char*) cmd.c_str());
269 int DPPanTiltControl::ReadRawLine(
string &cmd)
273 ReadSerialLine(COMstream_, (
unsigned char*)result, 1, &len);
274 cmd = string(result, len);
278 int DPPanTiltControl::
279 SetMotorStepping(
int panStepping,
int tiltStepping)
282 std::string command =
"";
284 SendRawCommand(
"PR\n");
287 switch (panStepping) {
289 if (s.find(
"A")!=string::npos) {
290 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
297 if (s.find(
"F")!=string::npos) {
298 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
305 if (s.find(
"H")!=string::npos) {
306 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
313 if (s.find(
"Q")!=string::npos) {
314 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
321 if (s.find(
"E")!=string::npos) {
322 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
336 if (panStepping != -1)
338 SendRawCommand(command);
342 if (ReadRawLine(s) != 0) {
343 std::cout <<
"DPPanTiltControl : Read " << s << std::endl;
344 if (s.find(
"*") != string::npos) {
348 std::cout <<
"DPPanTiltControl : Sleeping for 1 second" << std::endl;
353 SendRawCommand(
"TR\n");
356 switch (tiltStepping) {
358 if (s.find(
"A")!=string::npos) {
359 std::cout <<
"DPPanTiltControl : Tilt stepping is already set!" << std::endl;
366 if (s.find(
"F")!=string::npos) {
367 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
374 if (s.find(
"H")!=string::npos) {
375 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
382 if (s.find(
"Q")!=string::npos) {
383 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
390 if (s.find(
"E")!=string::npos) {
391 std::cout <<
"DPPanTiltControl : Pan stepping is already set!" << std::endl;
405 if (tiltStepping != -1)
407 SendRawCommand(command);
411 if (ReadRawLine(s) != 0) {
412 std::cout <<
"DPPanTiltControl : Read " << s << std::endl;
413 if (s.find(
"*") != string::npos) {
417 std::cout <<
"DPPanTiltControl : Sleeping for 1 second" << std::endl;
423 std::cout <<
"DPPanTiltControl : Sleeping for 1 second" << std::endl;
425 QueryResolution(a, b,
true);
429 void DPPanTiltControl::
430 QueryResolution(
double &panResolution,
double &tiltResolution,
bool updateInternal)
432 if (updateInternal) {
435 SendRawCommand(
"PR\n");
443 panResolution_ = argsec / 3600.0;
444 SendRawCommand(
"TR\n");
452 tiltResolution_ = argsec / 3600.0;
454 panResolution = panResolution_;
455 tiltResolution = tiltResolution_;