[Robotflow-cvs] RobotFlow/Devices/src SNCRZ30RS232.cc,1.3,1.4

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-serv30345

Modified Files:
	SNCRZ30RS232.cc 
Log Message:
fixed relative command squash

Index: SNCRZ30RS232.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SNCRZ30RS232.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** SNCRZ30RS232.cc	29 Jun 2005 19:20:13 -0000	1.3
--- SNCRZ30RS232.cc	30 Jun 2005 19:16:43 -0000	1.4
***************
*** 161,164 ****
--- 161,165 ----
    int destructor_loop_counter;

+ 
    //Functions
    void Send() 
***************
*** 177,190 ****

          m_sending.pop_front();
!       
!         //unlock here to avoid blocking on write (long)
!         pthread_mutex_unlock(&m_list_lock);

! 	// lock... wait for ack signal before sending... Unlock is done in receive()
! 	//pthread_mutex_lock(&m_ack_lock);
  	// Wait for a buffer to be freed up by the cam
! 	//pseudosem_wait(&m_buffer_empty);
! 
! 

--- 178,194 ----

          m_sending.pop_front();
! 
! 	//unlock here to avoid blocking on write (long)
! 	pthread_mutex_unlock(&m_list_lock);
!        
! 	//lock... wait for ack signal before sending... 
! 	//Unlock is done in receive()
! 	//cerr<<"locking ack"<<endl;
! 	pthread_mutex_lock(&m_ack_lock);

! 	    
  	// Wait for a buffer to be freed up by the cam
! 	pseudosem_wait(&m_buffer_empty);	
! 	//cerr<<"Sending...size"<<m_sending.size()<<endl;

***************
*** 195,199 ****
           }

- 
       }//if !empty
       else
--- 199,202 ----
***************
*** 207,210 ****
--- 210,215 ----
    void Receive()
    {
+ 
+     
      unsigned char PanReplyMessage[2];
      string message;
***************
*** 219,222 ****
--- 224,228 ----
        //if you read something
        if(newnumread > 0){
+ 	//cerr<<"Receiving one byte"<<endl;
  	message += PanReplyMessage[readTillNow];

***************
*** 227,230 ****
--- 233,237 ----
  	  // Push the message into the list
  	  m_received.push_back(message);
+ 	  //cerr<<"New full message received"<<endl;
  	  // Unlock  
  	  pthread_mutex_unlock(&m_list_rec_lock);      
***************
*** 287,290 ****
--- 294,298 ----

      //reading camera data     
+ 
      while (!m_received.empty())
        {
***************
*** 300,303 ****
--- 308,313 ----
  	    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);
  	    pseudosem_post(&m_buffer_empty);
***************
*** 327,330 ****
--- 337,341 ----
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
+ 	    //cerr<<"pan tilt inquiry reply"<<endl;
  	  }

***************
*** 332,336 ****
--- 343,349 ----
  	if ((message.size() == 3) && (message[1] == 0x41 || message[1] == 0x42))
  	  {
+ 	    //cerr<<"ACK1"<<endl;
  	    pthread_mutex_unlock(&m_ack_lock);
+ 	    //pseudosem_post(&m_buffer_empty);

  	  }
***************
*** 339,342 ****
--- 352,356 ----
  	if ((message.size() == 3) && (message[1] == 0x51 || message[1] == 0x52))
  	  {
+ 	    //cerr<<"COMPLETION"<<endl;
  	    pseudosem_post(&m_buffer_empty);
  	  }
***************
*** 345,348 ****
--- 359,363 ----
  	if ((message.size() == 5) && (message[1] == 0x01 && message[2] == 0x00 && message[3] == 0x01))
  	  {
+ 	    //cerr<< "Clear interface"<<endl;
  	    pseudosem_post(&m_buffer_empty);
  	    pthread_mutex_unlock(&m_ack_lock);
***************
*** 352,356 ****
  	if ((message.size() == 4) && (message[2] == 0x02))
  	  {
! 	    cerr << "Erreur de syntax" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
--- 367,371 ----
  	if ((message.size() == 4) && (message[2] == 0x02))
  	  {
! 	    //cerr << "Erreur de syntax" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
***************
*** 360,364 ****
  	if ((message.size() == 4) && (message[2] == 0x03))
  	  {
! 	    cerr << "Command buffer full" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
--- 375,379 ----
  	if ((message.size() == 4) && (message[2] == 0x03))
  	  {
! 	    //cerr << "Command buffer full" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
***************
*** 367,371 ****
  	if ((message.size() == 4) && (message[2] == 0x04))
  	  {
! 	    cerr << "Command cancel" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
--- 382,386 ----
  	if ((message.size() == 4) && (message[2] == 0x04))
  	  {
! 	    //cerr << "Command cancel" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
***************
*** 374,378 ****
  	if ((message.size() == 4) && (message[2] == 0x05))
  	  {
! 	    cerr << "No socket" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
--- 389,393 ----
  	if ((message.size() == 4) && (message[2] == 0x05))
  	  {
! 	    //cerr << "No socket" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
***************
*** 382,386 ****
  	if ((message.size() == 4) && (message[2] == 0x41))
  	  {
! 	    cerr << "Command not executable" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
--- 397,401 ----
  	if ((message.size() == 4) && (message[2] == 0x41))
  	  {
! 	    //cerr << "Command not executable" << endl;
  	    pthread_mutex_unlock(&m_ack_lock);
  	    pseudosem_post(&m_buffer_empty);
***************
*** 406,416 ****

        // If this is an ABS command replace it by the new one
!       if(((*i)[1] == msg_to_send[1]) && ((*i)[2] == msg_to_send[2]) && ((*i)[3] == msg_to_send[3])){
! 	// Replace the actual stuff with the new one.
! 	(*i) = msg_to_send;
! 	// Dont post any semaphore...
! 	duplicate_item_found = true;
! 	// Get out of here as soon as you can
! 	break;
        }
      }
--- 421,462 ----

        // If this is an ABS command replace it by the new one
!       if ((*i).size() == msg_to_send.size()) {
! 
! 	if(((*i)[0] == msg_to_send[0]) && ((*i)[1] == msg_to_send[1]) 
! 	   && ((*i)[2] == msg_to_send[2]) && ((*i)[3] == msg_to_send[3])){
! 	  
! 	  //check for relative pan-tilt commands
! 	  if ((unsigned char)(*i)[0] == 0x81 && (unsigned char)(*i)[1] == 0x01 
! 	      && (unsigned char)(*i)[2] == 0x06 && (unsigned char)(*i)[3] == 0x03) {
! 	    
! 	    /*
! 	    cerr<<"relative command detected in the list"<<endl;
! 
! 
! 	    short pan_rel1, tilt_rel1, pan_rel2, tilt_rel2 = 0;
! 
! 
! 	    pan_rel1 |= ((unsigned short)(*i)[6]  << 12)& 0xF000;
! 	    pan_rel1 |= ((unsigned short)(*i)[7]  << 8) & 0x0F00;
! 	    pan_rel1 |= ((unsigned short)(*i)[8]  << 4) & 0x00F0;
! 	    pan_rel1 |= (unsigned short)(*i)[9] & 0x000F;
! 	    
! 	    tilt_rel1 |= ((unsigned short)(*i)[10]  << 12)& 0xF000;
! 	    tilt_rel1 |= ((unsigned short)(*i)[11]  << 8) & 0x0F00;
! 	    tilt_rel1 |= ((unsigned short)(*i)[12]  << 4) & 0x00F0;
! 	    tilt_rel1 |= (unsigned short)(*i)[13] & 0x000F;
! 
! 	    cerr<<"detected pan rel "<<pan_rel1<<" tilt rel "<<tilt_rel1<<endl;
! 	    */
! 	  	    
! 	  }
! 	  else {
! 	    //Replace the actual stuff with the new one
! 	    (*i) = msg_to_send;
! 	    duplicate_item_found = true;
! 	    break;
! 	    
! 	  }	 
! 	}
        }
      }
***************
*** 461,465 ****
       message += 0x88;
       message += 0x01;
!      message += (char)0x00;
       message += 0x01;
       message += 0xFF;
--- 507,511 ----
       message += 0x88;
       message += 0x01;
!      message += (char) 0x00;
       message += 0x01;
       message += 0xFF;
***************
*** 472,475 ****
--- 518,524 ----
    {

+ 
+     //cerr<<"Abs pan "<<pan_position<<" Abs tilt "<<tilt_position<<endl;
+ 
       string message; 
       string last;
***************
*** 502,506 ****
       message += 0xFF;

!      // add it to the list dont worry be happy !
       Add_message_to_send(message);
    }
--- 551,555 ----
       message += 0xFF;

!      // add it to the list
       Add_message_to_send(message);
    }
***************
*** 509,514 ****
    void Relative_movement(int pan_speed, int tilt_speed, int pan_position, int tilt_position)
    {
!     //Absolute_movement(pan_speed,tilt_speed,m_pan_position + pan_position, m_tilt_position + tilt_position);
!      

       string message;
--- 558,562 ----
    void Relative_movement(int pan_speed, int tilt_speed, int pan_position, int tilt_position)
    {
!     //cerr<<"Rel pan "<<pan_position<<" Rel tilt "<<tilt_position<<endl;     

       string message;
***************
*** 546,550 ****

!      // add it to the list dont worry be happy !
       Add_message_to_send(message);

--- 594,598 ----

!      // add it to the list 
       Add_message_to_send(message);

***************
*** 554,557 ****
--- 602,607 ----
    void Zoom(int data)
    {
+     //cerr<<"Zoom : "<<data<<endl;
+ 
       string message;
       string last;
***************
*** 583,586 ****
--- 633,637 ----
    void inquiry_pan_tilt()
    {
+     //cerr<<"inquiry pan tilt"<<endl;
       //pan + tilt request
       string PanTiltMessage;
***************
*** 596,599 ****
--- 647,652 ----

    void inquiry_zoom(){
+ 
+     //cerr<<"inquiry zoom"<<endl;
      //zoom request
      string ZoomMessage; 
***************
*** 729,738 ****
       }

         inquiry_pan_tilt();

- 

         inquiry_zoom();
!     

       //output position
--- 782,792 ----
       }

+      
+ 
         inquiry_pan_tilt();

         inquiry_zoom();
!      

       //output position