[Robotflow-cvs] RobotFlow/Devices/src Makefile.am,1.15,1.16 SNCRZ30RS232.cc,1.7,1.8

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

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

Modified Files:
	Makefile.am SNCRZ30RS232.cc 
Log Message:
converting values properly to 1/10 of degrees

Index: Makefile.am
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/Makefile.am,v
retrieving revision 1.15
retrieving revision 1.16
diff -C2 -d -r1.15 -r1.16
*** Makefile.am	15 Aug 2005 19:22:41 -0000	1.15
--- Makefile.am	19 Sep 2005 16:06:12 -0000	1.16
***************
*** 15,19 ****
  	SNCRZ30.cc \
  	SNCRZ30RS232.cc \
! 	IMU400CC_200.cc

  libDevices_la_LDFLAGS = -release $(LT_RELEASE)
--- 15,20 ----
  	SNCRZ30.cc \
  	SNCRZ30RS232.cc \
! 	IMU400CC_200.cc \
! 	libvisca.c

  libDevices_la_LDFLAGS = -release $(LT_RELEASE)

Index: SNCRZ30RS232.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SNCRZ30RS232.cc,v
retrieving revision 1.7
retrieving revision 1.8
diff -C2 -d -r1.7 -r1.8
*** SNCRZ30RS232.cc	19 Sep 2005 13:38:43 -0000	1.7
--- SNCRZ30RS232.cc	19 Sep 2005 16:06:12 -0000	1.8
***************
*** 59,63 ****
   * @input_name TILT_ABS_POS
   * @input_type int
!  * @input_description Absolute Tilt of the camera -725 to +425. (1/10 of degrees)
   *
   * @input_name PAN_REL_POS
--- 59,63 ----
   * @input_name TILT_ABS_POS
   * @input_type int
!  * @input_description Absolute Tilt of the camera -575 to +425. (1/10 of degrees)
   *
   * @input_name PAN_REL_POS
***************
*** 67,71 ****
   * @input_name TILT_REL_POS
   * @input_type int
!  * @input_description Absolute Tilt of the camera -725 to +425. (1/10 of degrees)
   *
   * @parameter_name PAN_SPEED
--- 67,71 ----
   * @input_name TILT_REL_POS
   * @input_type int
!  * @input_description Absolute Tilt of the camera -575 to +425. (1/10 of degrees)
   *
   * @parameter_name PAN_SPEED
***************
*** 85,89 ****
   * @input_name ZOOM_ABS_POS
   * @input_type int
!  * @input_description Absolute Zoom of the camera 0 (1X) to 16384 (24X)
   *
   * @input_name ZOOM_REL_POS
--- 85,89 ----
   * @input_name ZOOM_ABS_POS
   * @input_type int
!  * @input_description Absolute Zoom of the camera 0 (1X) to 16384 (25X)
   *
   * @input_name ZOOM_REL_POS
***************
*** 93,109 ****
   * @output_name PAN_POSITION
   * @output_type int
!  * @output_description Returns the current pan position of the Camera
   *
   * @output_name TILT_POSITION
   * @output_type int
!  * @output_description Returns the current tilt position of the Camera
   *
   * @output_name ZOOM_POSITION
   * @output_type int
!  * @output_description Returns the current zoom position of the Camera
   *
   END*/

! 

--- 93,110 ----
   * @output_name PAN_POSITION
   * @output_type int
!  * @output_description Returns the current pan position of the Camera (1/10 of degrees)
   *
   * @output_name TILT_POSITION
   * @output_type int
!  * @output_description Returns the current tilt position of the Camera (1/10 of degrees)
   *
   * @output_name ZOOM_POSITION
   * @output_type int
!  * @output_description Returns the current zoom position of the Camera (1/10 of degrees)
   *
   END*/

! #define PAN_TILT_UNIT_CONVERTION_FROM_CAMERA 0.6944
! #define PAN_TILT_UNIT_CONVERTION_TO_CAMERA 1.44

***************
*** 190,196 ****
  	// Wait for a buffer to be freed up by the cam
  	pseudosem_wait(&m_buffer_empty);	
! 	//cerr<<"Sending...size"<<m_sending.size()<<endl;
! 	  
! 	
          if(write(fd, to_send.c_str(),to_send.size()) < 0)
          {
--- 191,195 ----
  	// Wait for a buffer to be freed up by the cam
  	pseudosem_wait(&m_buffer_empty);	
! 		  		
          if(write(fd, to_send.c_str(),to_send.size()) < 0)
          {
***************
*** 308,312 ****
  	    m_zoom_position = message[2]<<12 | message[3]<<8 | message[4]<<4 | message[5];

! 	    cerr<<"Zoom inquiry reply"<<endl;

  	    pthread_mutex_unlock(&m_ack_lock);
--- 307,311 ----
  	    m_zoom_position = message[2]<<12 | message[3]<<8 | message[4]<<4 | message[5];

! 	    //cerr<<"Zoom inquiry reply"<<endl;

  	    pthread_mutex_unlock(&m_ack_lock);
***************
*** 321,337 ****

  	    // next 4 are pan
! 	    convpan = message[5];
! 	    convpan |= (message[4] << 4);
! 	    convpan |= (message[3] << 8);
! 	    convpan |= (message[2] << 12);
  	    m_pan_position = max(-1700,min(convpan,1700));
! 	    
  	    // next 4 are tilt
! 	    convtilt = message[9];
! 	    convtilt |= (message[8] << 4);
! 	    convtilt |= (message[7] << 8);
! 	    convtilt |= (message[6] << 12);

! 	    m_tilt_position= max(-725,min(convtilt,425));

  	    pthread_mutex_unlock(&m_ack_lock);
--- 320,345 ----

  	    // next 4 are pan
! 	    convpan = (short) message[5];
! 	    convpan |= ((short)message[4] << 4);
! 	    convpan |= ((short)message[3] << 8);
! 	    convpan |= ((short)message[2] << 12);
! 
! 	    //CONVERSION REQUIRED FROM CAMERA UNITS TO 1/10 OF DEGREES
! 	    convpan = (short) (PAN_TILT_UNIT_CONVERTION_FROM_CAMERA * (float) convpan + 0.5);
! 
  	    m_pan_position = max(-1700,min(convpan,1700));
! 	    	     
! 
  	    // next 4 are tilt
! 	    convtilt = (short)message[9];
! 	    convtilt |= ((short)message[8] << 4);
! 	    convtilt |= ((short)message[7] << 8);
! 	    convtilt |= ((short)message[6] << 12);
! 
! 	    //CONVERSION REQUIRED FROM CAMERA UNITS TO 1/10 OF DEGREES
! 	    convtilt = (short) (PAN_TILT_UNIT_CONVERTION_FROM_CAMERA * (float) convtilt + 0.5);
! 

! 	    m_tilt_position= max(-575,min(convtilt,425));

  	    pthread_mutex_unlock(&m_ack_lock);
***************
*** 482,485 ****
--- 490,499 ----
    }

+   void cleanup(void)
+   {
+ 
+     
+   }
+ 
    void init_wb(void)
    {
***************
*** 523,564 ****
    {

-     //LIMIT PAN & TILT SPEED VALUES
-      pan_speed = max(0,min(pan_speed,24));
-      tilt_speed = max(0,min(tilt_speed,20));    
-      pan_position = max(min(pan_position,1700), -1700);
-      tilt_position = max(min(tilt_position,425), -725);

!      //CREATE VISCA COMMAND
!      ostringstream my_stream;
!      static const unsigned int mask1 = 0x000F;
!      static const unsigned int mask2 = 0x00F0;
!      static const unsigned int mask3 = 0x0F00;
!      static const unsigned int mask4 = 0xF000;
!      //header
!      my_stream << 0x81;
!      my_stream << 0x01;
!      my_stream << 0x06;
!      my_stream << 0x02;
!      //pan & tilt speeds
!      my_stream << std::hex<<((mask2 & pan_speed)>>4);//pan speed
!      my_stream << std::hex<<((mask1 & pan_speed));

!      my_stream << std::hex<<((mask2 & tilt_speed)>>4);//tilt speed
!      my_stream << std::hex<<((mask1 & tilt_speed));
!      //pan & tilt commands
!      my_stream << "0"<<std::hex<<((mask4 & pan_position)>>12); //pan position
!      my_stream << "0"<<std::hex<<((mask3 & pan_position)>>8);
!      my_stream << "0"<<std::hex<<((mask2 & pan_position)>>4);
!      my_stream << "0"<<std::hex<<((mask1 & pan_position));

!      my_stream << "0"<<std::hex<<((mask4 & tilt_position)>>12); //tilt position
!      my_stream << "0"<<std::hex<<((mask3 & tilt_position)>>8);
!      my_stream << "0"<<std::hex<<((mask2 & tilt_position)>>4);
!      my_stream << "0"<<std::hex<<((mask1 & tilt_position));

!      my_stream << "FF\r\n"; //end message

-      // add it to the list
-      Add_message_to_send(my_stream.str(),false);
    }

--- 537,583 ----
    {

!     //cerr<<"Abs pan "<<pan_position<<" Abs tilt "<<tilt_position<<endl;     
!      
!      string message;
!     
!      int Sep1 = 0x000F;
!      int Sep2 = 0x00F0;
!      int Sep3 = 0x0F00;
!      int Sep4 = 0xF000;
!     
!      message += 0x81;
!      message += 0x01;
!      message += 0x06;
!      message += 0x02;
!     
!      message += max(0,min(pan_speed,24));
!      message += max(0,min(tilt_speed,20));

!      pan_position = max(min(pan_position,1700), -1700);
!      tilt_position = max(min(tilt_position,425), -575);

!      //CONVERSION REQUIRED TO CAMERA UNITS
!      pan_position =  (int) (PAN_TILT_UNIT_CONVERTION_TO_CAMERA * (float) pan_position + 0.5);
!      tilt_position = (int) (PAN_TILT_UNIT_CONVERTION_TO_CAMERA * (float) tilt_position + 0.5);

!      //pan
!      message += (Sep4 & pan_position)>>12;
!      message += (Sep3 & pan_position)>>8;
!      message += (Sep2 & pan_position)>>4;
!      message += (Sep1 & pan_position);
!      
!      //tilt
!      message += (Sep4 & tilt_position)>>12;
!      message += (Sep3 & tilt_position)>>8;
!      message += (Sep2 & tilt_position)>>4;
!      message += (Sep1 & tilt_position);
!     
!      message += 0xFF;
!     
!     
!      // add it to the list 
!      Add_message_to_send(message,false);

    }

***************
*** 584,589 ****

       pan_position = max(min(pan_position,1700), -1700);
!      tilt_position = max(min(tilt_position,425), -725);

       //pan
--- 603,611 ----

       pan_position = max(min(pan_position,1700), -1700);
!      tilt_position = max(min(tilt_position,425), -575);

+      //CONVERSION REQUIRED TO CAMERA UNITS
+      pan_position =  (int) (PAN_TILT_UNIT_CONVERTION_TO_CAMERA * (float) pan_position + 0.5);
+      tilt_position = (int) (PAN_TILT_UNIT_CONVERTION_TO_CAMERA * (float) tilt_position + 0.5);

       //pan
***************
*** 813,817 ****
  	cerr << "SNCRZ30RS232 : restoring default camera values" << endl;
  	Absolute_movement(24,20, 0, 0); // put the camera at the neutral position
! 	//Zoom(0); 

  	//sleep?
--- 835,839 ----
  	cerr << "SNCRZ30RS232 : restoring default camera values" << endl;
  	Absolute_movement(24,20, 0, 0); // put the camera at the neutral position
! 	Zoom(0); 

  	//sleep?
***************
*** 882,885 ****
--- 904,910 ----
    void initialize_camera()
    {
+     //useful to make camera stop its current command if
+     //not completed.
+     cleanup();
      // Clear the interface of camera
      init_clear_if();