packet.cc

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  
00004  *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
00005  *                      
00006  *
00007  *  This program is free software; you can redistribute it and/or modify
00008  *  it under the terms of the GNU General Public License as published by
00009  *  the Free Software Foundation; either version 2 of the License, or
00010  *  (at your option) any later version.
00011  *
00012  *  This program is distributed in the hope that it will be useful,
00013  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *  GNU General Public License for more details.
00016  *
00017  *  You should have received a copy of the GNU General Public License
00018  *  along with this program; if not, write to the Free Software
00019  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  *
00021  */
00022 
00023 /*
00024  * $Id: packet.cc 3999 2007-03-01 20:41:47Z gerkey $
00025  *   GRASP version of the P2OS packet class.  this class has methods 
00026  *   for building, printing, sending and receiving GRASP Board packets.
00027  *
00028  */
00029 
00030 #include <stdio.h>
00031 #include <errno.h>
00032 #include <string.h>
00033 #include <packet.h>
00034 #include <unistd.h>
00035 #include <stdlib.h> /* for exit() */
00036 #include "clodbuster.h"
00037 
00038 //extern bool debug;
00039 
00040 void GRASPPacket::Print() {
00041   if (packet) {
00042     printf("\"");
00043     for(int i=0;i<(int)size;i++) {
00044       printf("%u ", packet[i]);
00045     }
00046     puts("\"");
00047   }
00048 }
00049 
00050 void GRASPPacket::PrintHex() {
00051   if (packet) {
00052     printf("\"");
00053     for(int i=0;i<(int)size;i++) {
00054       printf("0x%.2x ", packet[i]);
00055     }
00056     puts("\"");
00057   }
00058 }
00059 
00060 
00061 
00062 int GRASPPacket::Receive( int fd,unsigned char command) 
00063 {
00064   switch(command)
00065   { 
00066     case ECHO_SERVO_VALUES:
00067     case ECHO_MAX_SERVO_LIMITS:
00068     case ECHO_MIN_SERVO_LIMITS:
00069     case ECHO_CEN_SERVO_LIMITS:
00070       retsize=8;
00071       break;
00072   case ECHO_ENCODER_COUNTS: retsize=6; break;
00073   case ECHO_ENCODER_COUNTS_TS:
00074   case ECHO_ADC: retsize=10;break;
00075   case READ_ID:retsize=1;break;
00076   case ECHO_SLEEP_MODE: retsize=1;break;
00077   default:
00078     retsize=0;return(1);
00079   }
00080 
00081   memset(packet,0,retsize);
00082 
00083     int cnt = 0;
00084    
00085     cnt=read( fd, packet,  retsize); 
00086     if (cnt!=(int)retsize)
00087       printf("wrong read size: asked %d got %d\n",retsize,cnt);
00088        
00089   return(0);
00090 }
00091 
00092 int GRASPPacket::Build(unsigned char command, unsigned char data)
00093 {
00094   /* header */
00095   packet[0]=0xFF;
00096   packet[1]=command;
00097   packet[2]=data;
00098   size=3; 
00099   return(0);
00100 }
00101 
00102 int GRASPPacket::Build(unsigned char command)
00103 {
00104   /* header */
00105   packet[0]=0xFF;
00106   packet[1]=command;
00107   packet[2]=0x00;
00108   size=3; 
00109   return(0);
00110 }
00111 
00112 int GRASPPacket::Send(int fd) 
00113 {
00114   int cnt=0;
00115   cnt = write( fd, packet, size );
00116  
00117    if(cnt !=(int)size) 
00118     {
00119       perror("Send");
00120       return(1);
00121     }
00122   
00123 /*  if(debug)
00124   {
00125     struct timeval dummy;
00126     GlobalTime->GetTime(&dummy);
00127     printf("GRASPPacket::Send():%ld:%ld\n", dummy.tv_sec, dummy.tv_usec);
00128   }*/
00129   return(0);
00130 }
Last updated 12 September 2005 21:38:45