[Robotflow-cvs] RobotFlow/Player/src PlayerPTZ.cc,1.9,1.10

[Dominic L. <ma...@us...>]

Update of /cvsroot/robotflow/RobotFlow/Player/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv20329

Modified Files:
	PlayerPTZ.cc 
Log Message:
still have problems with the camera

Index: PlayerPTZ.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Player/src/PlayerPTZ.cc,v
retrieving revision 1.9
retrieving revision 1.10
diff -C2 -d -r1.9 -r1.10
*** PlayerPTZ.cc	2 Jun 2004 18:39:35 -0000	1.9
--- PlayerPTZ.cc	2 Jun 2004 20:51:55 -0000	1.10
***************
*** 146,230 ****
     }
  
!    void calculate(int output_id, int count, Buffer &out)
!    {
!       //must update our readings.
!       ObjectRef ClientValue = getInput(m_clientID,count);
! 
!       if(ClientValue->isNil())
        {
! 	 out[count] = nilObject;
! 	 return;
        }
! 
!       m_ptzProxy = object_cast<PlayerClientWrap>(ClientValue).getPtzProxy();
!       if (m_ptzProxy == NULL)
        {
!          throw new GeneralException("PlayerPTZ::calculate NULL PTZ proxy",__FILE__,__LINE__);
        }
! 
!       if(!m_isInit)
        {
! 	 m_ptzProxy->SetSpeed(m_panSpeed, m_tiltSpeed);
! 	 m_isInit = true;
        }
! 
!       m_panSpeed = dereference_cast<int>(parameters.get("PAN_SPEED"));
!       m_tiltSpeed = dereference_cast<int>(parameters.get("TILT_SPEED"));
! 
! 
!       int pan_command = (int)(m_ptzProxy->pan * M_PI / 180.0);
!       int tilt_command = (int)(m_ptzProxy->tilt * M_PI / 180.0);
!       int zoom_command = (int)(m_ptzProxy->zoom);
! 
!       //reading & checking validity of inputs.
!       ObjectRef RelPanValue  = getInput(m_relPanID,count);
!       ObjectRef RelTiltValue = getInput(m_relTiltID,count);
!       ObjectRef RelZoomValue = getInput(m_relZoomID,count);
!       ObjectRef AbsPanValue  = getInput(m_absPanID,count);
!       ObjectRef AbsTiltValue = getInput(m_absTiltID,count);
!       ObjectRef AbsZoomValue = getInput(m_absZoomID,count);
! 
!       //check for absolute movements
!       if (!AbsPanValue->isNil())
        {
!          pan_command = dereference_cast<int>(AbsPanValue) / 10;
        }
!       if (!AbsTiltValue->isNil())
        {
!          tilt_command = dereference_cast<int>(AbsTiltValue) / 10;
        }
!       if (!AbsZoomValue->isNil())
        {
!          zoom_command = dereference_cast<int>(AbsZoomValue);
        }
! 
!       //add relative movements
!       if (!RelPanValue->isNil())
        {
!          pan_command += dereference_cast<int>(RelPanValue) / 10;
        }
!       if (!RelTiltValue->isNil())
        {
!          tilt_command += dereference_cast<int>(RelTiltValue) / 10;
        }
!       if (!RelZoomValue->isNil())
!       {
!          zoom_command += dereference_cast<int>(RelZoomValue);
        }
  
!       //checking camera limits
!       pan_command  = max(-88,min(pan_command,88));
!       tilt_command = max(-30,min(tilt_command,30));
!       zoom_command = max(0,min(zoom_command,1023));
! 
!       //writing ptz command
!       m_ptzProxy->SetCam(pan_command,tilt_command,zoom_command);
! 
!       //output current camera position
!       (*outputs[m_panOutID].buffer)[count]  = ObjectRef(Int::alloc((int) (m_ptzProxy->pan * 10 * M_PI / 180.0)));
!       (*outputs[m_tiltOutID].buffer)[count] = ObjectRef(Int::alloc((int) (m_ptzProxy->tilt * 10 * M_PI / 180.0 )));
!       (*outputs[m_zoomOutID].buffer)[count] = ObjectRef(Int::alloc((int) (m_ptzProxy->zoom * 10)));
! 
!    }
  
  };
--- 146,246 ----
     }
  
!   void calculate(int output_id, int count, Buffer &out)
!   {
!     //must update our readings.
!     ObjectRef ClientValue = getInput(m_clientID,count);
!     
!     if(ClientValue->isNil())
        {
! 	out[count] = nilObject;
! 	return;
        }
!     
!     m_ptzProxy = object_cast<PlayerClientWrap>(ClientValue).getPtzProxy();
!     if (m_ptzProxy == NULL)
        {
! 	throw new GeneralException("PlayerPTZ::calculate NULL PTZ proxy",__FILE__,__LINE__);
        }
!     
!     if(!m_isInit)
        {
! 	m_ptzProxy->SetSpeed(m_panSpeed, m_tiltSpeed);
! 	m_isInit = true;
        }
!     
!     m_panSpeed = dereference_cast<int>(parameters.get("PAN_SPEED"));
!     m_tiltSpeed = dereference_cast<int>(parameters.get("TILT_SPEED"));
!     
!     
!     //conversion into rads
!     double pan_command =  m_ptzProxy->pan;
!     double tilt_command = m_ptzProxy->tilt;
!     
!     //conversion into zoom ratio
!     double zoom_command = m_ptzProxy->zoom;
!   
!     //reading & checking validity of inputs.
!     ObjectRef RelPanValue  = getInput(m_relPanID,count);
!     ObjectRef RelTiltValue = getInput(m_relTiltID,count);
!     ObjectRef RelZoomValue = getInput(m_relZoomID,count);
!     ObjectRef AbsPanValue  = getInput(m_absPanID,count);
!     ObjectRef AbsTiltValue = getInput(m_absTiltID,count);
!     ObjectRef AbsZoomValue = getInput(m_absZoomID,count);
!     
!     //check for absolute movements
!     if (!AbsPanValue->isNil())
        {
! 	//converting into rads
! 	pan_command = dereference_cast<int>(AbsPanValue) / 10 * M_PI / 180.0;
        }
!     if (!AbsTiltValue->isNil())
        {
! 	//converting into rads
! 	tilt_command = dereference_cast<int>(AbsTiltValue) / 10 * M_PI / 180.0;
        }
!     if (!AbsZoomValue->isNil())
        {
! 	//converting into zoom ratio
! 	zoom_command = (double) dereference_cast<int>(AbsZoomValue);
        }
!     
!     //add relative movements
!     if (!RelPanValue->isNil())
        {
! 	//converting into rads
! 	pan_command += (double) dereference_cast<int>(RelPanValue) / 10 * M_PI / 180.0;
        }
!     if (!RelTiltValue->isNil())
        {
! 	//converting into rads
! 	tilt_command += (double) dereference_cast<int>(RelTiltValue) / 10 * M_PI / 180.0;
        }
!     
!     if (!RelZoomValue->isNil())
!       {  
! 	//converting into zoom ratio
! 	zoom_command += dereference_cast<int>(RelZoomValue);
        }
+     
+     double pan_limit = 88.0 * M_PI / 180.0;
+     double tilt_limit = 30.0 * M_PI / 180.0;
+     
+     
+     //checking camera limits (rads)
+     pan_command  = max(-pan_limit,min(pan_command,pan_limit));
+     tilt_command = max(-tilt_limit,min(tilt_command,tilt_limit));
+     zoom_command = max(1,min(zoom_command,24));
+     
+     //writing ptz command
+     cerr<<"pan "<<pan_command<<" tilt "<<tilt_command<<" zoom command "<<zoom_command<<endl;
  
!     m_ptzProxy->SetCam(pan_command,tilt_command,zoom_command);
!     
!     //output current camera position
!     (*outputs[m_panOutID].buffer)[count]  = ObjectRef(Int::alloc((int) (m_ptzProxy->pan * 10 * 180.0 / M_PI)));
!     (*outputs[m_tiltOutID].buffer)[count] = ObjectRef(Int::alloc((int) (m_ptzProxy->tilt * 10 * 180.0 / M_PI )));
!     (*outputs[m_zoomOutID].buffer)[count] = ObjectRef(Int::alloc((int) (m_ptzProxy->zoom)));
!     
!   }
  
  };