nfv2.c
Go to the documentation of this file.
00001 #include "nfv2.h"
00002 
00003 uint8_t crcTable[256];
00004 
00005 /*
00006 * Funkcja do wrzucenia w obsluge przychodzacych danych.
00007 * NFComBuf - wskaznik do bufora komunikacyjnego, w ktorym automatycznie beda umieszczane odebrane dane.
00008 * rxBuf - wskaznik do tablicy, w ktorej w przerwaniu znak po znaku zapisywana jest odebrana wiadomosc.
00009 * rxPt - wskaznik do indeksu pola w tablicy, na ktorym zostala zapisany zostal ostatnio odebrany bajt.
00010 * commandArray - wskaznik do tablicy, ktora jest automatycznie wypelniana kodami odebranych zapytan.
00011 * commandCnt - wskaznik do zmiennej zawierajacej liczbe kodow w commandArray[].
00012 * Funkcja zwraca 1, jesli zostala poprawnie odebrana ramka adresowana do naszego modulu, 0 w pozostalych przypadkach.
00013 */
00014 uint8_t NF_Interpreter( NF_STRUCT_ComBuf *NFComBuf,
00015                                                 uint8_t *rxBuf,
00016                                                 uint8_t *rxPt,
00017                                                 uint8_t *commandArray,
00018                                                 uint8_t *commandCnt){
00019         static uint8_t n;       
00020         uint8_t rxAddress, rxParamsCnt;
00021         uint8_t rxBufIter, combufDataIter, rxDataIter, dataBytesIter;
00022         uint8_t *dataPt, *u8TempPt;
00023 
00024         if((*rxPt) == 0 && rxBuf[0]!='#'){
00025                 return 0;
00026         }
00027         else if((*rxPt) == 3){
00028                 if(( (rxBuf[1] & 0xff) == ((~rxBuf[2]) & 0xff) ) && (rxBuf[1] > 3)){
00029                         n = rxBuf[1];
00030                         (*rxPt)++;
00031                 }
00032                 else{
00033                         (*rxPt) = 0;
00034                 }
00035                 return 0;
00036         }
00037         else if((*rxPt) > 3 && (*rxPt) >= n){
00038                 (*rxPt) = 0;
00039                 // if CRC Fail
00040                 if(NFv2_CrcFast(((const uint8_t*)rxBuf) + 3,  n-3) != rxBuf[n]){
00041                         return 0;
00042                 }
00043                 // if Address Fail && not a Broadcast
00044                 //else if(rxBuf[3] != NFComBuf->myAddress && rxBuf[3] != NF_BroadcastAddress){
00045                 //      return 0;
00046                 //}
00047                 // if CRC OK && Address OK go to Command Interpreter
00048         }
00049         else{
00050                 (*rxPt)++;
00051                 return 0;
00052         }
00053         
00054         // ############ NF Command Interpreter
00055         rxAddress = rxBuf[3];
00056         rxBufIter = 4; // First command starts here
00057         *commandCnt = 0;
00058         while(rxBufIter < n){
00059                 dataPt = rxBuf + rxBufIter+2;
00060                 
00061                 // ############### "WRITE" Type Commands
00062                 // if  rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress
00063                 //              Master sends data to me (slave)
00064                 
00065                 // ########     Drives
00066                 // ####         Set Mode
00067                 #ifdef NF_BUFSZ_SetDrivesMode
00068                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMode){
00069                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00070                                         combufDataIter = 0;
00071                                         rxDataIter = 0;
00072                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMode;
00073                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMode)) {
00074                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMode.data[combufDataIter]);
00075                                                 dataBytesIter = 0;
00076                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMode){
00077                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMode + dataBytesIter);
00078                                                         dataBytesIter++;
00079                                                 }
00080                                                 //NFComBuf->SetDrivesMode.data[combufDataIter] = ((NF_STRUCT_SetDrivesMode*)dataPt)->data[rxDataIter];
00081                                                 combufDataIter++;                                               
00082                                                 rxDataIter++;
00083                                         }
00084                                         NFComBuf->SetDrivesMode.updated = 1;
00085                                 }
00086                         }
00087                         else
00088                 #endif
00089                 // ####         Set Speed
00090                 #ifdef NF_BUFSZ_SetDrivesSpeed
00091                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesSpeed){ 
00092                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00093                                         combufDataIter = 0;
00094                                         rxDataIter = 0;
00095                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesSpeed;
00096                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesSpeed)) {
00097                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesSpeed.data[combufDataIter]);
00098                                                 dataBytesIter = 0;
00099                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesSpeed){
00100                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesSpeed + dataBytesIter);
00101                                                         dataBytesIter++;
00102                                                 }
00103                                                 //NFComBuf->SetDrivesSpeed.data[combufDataIter] = ((NF_STRUCT_SetDrivesSpeed*)dataPt)->data[rxDataIter];
00104                                                 combufDataIter++;                                               
00105                                                 rxDataIter++;
00106                                         }
00107                                         NFComBuf->SetDrivesSpeed.updated = 1;
00108                                 }
00109                         }
00110                         else
00111                 #endif
00112                 // ####         Set Current
00113                 #ifdef NF_BUFSZ_SetDrivesCurrent
00114                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesCurrent){                            
00115                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00116                                         combufDataIter = 0;
00117                                         rxDataIter = 0;
00118                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesCurrent;
00119                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesCurrent)) {
00120                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesCurrent.data[combufDataIter]);
00121                                                 dataBytesIter = 0;
00122                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesCurrent){
00123                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesCurrent + dataBytesIter);
00124                                                         dataBytesIter++;
00125                                                 }
00126                                                 //NFComBuf->SetDrivesCurrent.data[combufDataIter] = ((NF_STRUCT_SetDrivesCurrent*)dataPt)->data[rxDataIter];
00127                                                 combufDataIter++;                                               
00128                                                 rxDataIter++;
00129                                         }
00130                                         NFComBuf->SetDrivesCurrent.updated = 1;
00131                                 }
00132                         }
00133                         else
00134                 #endif
00135                 // ####         Set Position
00136                 #ifdef NF_BUFSZ_SetDrivesPosition
00137                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesPosition){                   
00138                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00139                                         combufDataIter = 0;
00140                                         rxDataIter = 0;
00141                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesPosition;
00142                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesPosition)) {
00143                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesPosition.data[combufDataIter]);
00144                                                 dataBytesIter = 0;
00145                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesPosition){
00146                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesPosition + dataBytesIter);
00147                                                         dataBytesIter++;
00148                                                 }
00149                                                 //NFComBuf->SetDrivesPosition.data[combufDataIter] = ((NF_STRUCT_SetDrivesPosition*)dataPt)->data[rxDataIter];
00150                                                 combufDataIter++;                                               
00151                                                 rxDataIter++;
00152                                         }
00153                                         NFComBuf->SetDrivesPosition.updated = 1;
00154                                 }
00155                         }
00156                         else
00157                 #endif
00158                 // ####         Set PWM
00159                 #ifdef NF_BUFSZ_SetDrivesPWM
00160                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesPWM){                        
00161                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00162                                         combufDataIter = 0;
00163                                         rxDataIter = 0;
00164                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesPWM;
00165                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesPWM)) {
00166                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesPWM.data[combufDataIter]);
00167                                                 dataBytesIter = 0;
00168                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesPWM){
00169                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesPWM + dataBytesIter);
00170                                                         dataBytesIter++;
00171                                                 }
00172                                                 //NFComBuf->SetDrivesPWM.data[combufDataIter] = ((NF_STRUCT_SetDrivesPWM*)dataPt)->data[rxDataIter];
00173                                                 combufDataIter++;                                               
00174                                                 rxDataIter++;
00175                                         }
00176                                         NFComBuf->SetDrivesPWM.updated = 1;
00177                                 }
00178                         }
00179                         else
00180                 #endif
00181                 // ####         Set Max Current
00182                 #ifdef NF_BUFSZ_SetDrivesMaxCurrent
00183                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMaxCurrent){                         
00184                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00185                                         combufDataIter = 0;
00186                                         rxDataIter = 0;
00187                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMaxCurrent;
00188                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMaxCurrent)) {
00189                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxCurrent.data[combufDataIter]);
00190                                                 dataBytesIter = 0;
00191                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxCurrent){
00192                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMaxCurrent + dataBytesIter);
00193                                                         dataBytesIter++;
00194                                                 }
00195                                                 //NFComBuf->SetDrivesMaxCurrent.data[combufDataIter] = ((NF_STRUCT_SetDrivesMaxCurrent*)dataPt)->data[rxDataIter];
00196                                                 combufDataIter++;                                               
00197                                                 rxDataIter++;
00198                                         }
00199                                         NFComBuf->SetDrivesMaxCurrent.updated = 1;
00200                                 }
00201                         }
00202                         else
00203                 #endif
00204                 // ####         Set Max Speed
00205                 #ifdef NF_BUFSZ_SetDrivesMaxSpeed
00206                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMaxSpeed){                           
00207                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00208                                         combufDataIter = 0;
00209                                         rxDataIter = 0;
00210                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMaxSpeed;
00211                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMaxSpeed)) {
00212                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxSpeed.data[combufDataIter]);
00213                                                 dataBytesIter = 0;
00214                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxSpeed){
00215                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMaxSpeed + dataBytesIter);
00216                                                         dataBytesIter++;
00217                                                 }
00218                                                 //NFComBuf->SetDrivesMaxSpeed.data[combufDataIter] = ((NF_STRUCT_SetDrivesMaxSpeed*)dataPt)->data[rxDataIter];
00219                                                 combufDataIter++;                                               
00220                                                 rxDataIter++;
00221                                         }
00222                                         NFComBuf->SetDrivesMaxSpeed.updated = 1;
00223                                 }
00224                         }
00225                         else
00226                 #endif
00227                 // ####         Set Min Position
00228                 #ifdef NF_BUFSZ_SetDrivesMinPosition
00229                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMinPosition){                                
00230                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00231                                         combufDataIter = 0;
00232                                         rxDataIter = 0;
00233                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMinPosition;
00234                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMinPosition)) {
00235                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMinPosition.data[combufDataIter]);
00236                                                 dataBytesIter = 0;
00237                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMinPosition){
00238                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMinPosition + dataBytesIter);
00239                                                         dataBytesIter++;
00240                                                 }
00241                                                 //NFComBuf->SetDrivesMinPosition.data[combufDataIter] = ((NF_STRUCT_SetDrivesMinPosition*)dataPt)->data[rxDataIter];
00242                                                 combufDataIter++;                                               
00243                                                 rxDataIter++;
00244                                         }
00245                                         NFComBuf->SetDrivesMinPosition.updated = 1;
00246                                 }
00247                         }
00248                         else
00249                 #endif
00250                 // ####         Set Max Position
00251                 #ifdef NF_BUFSZ_SetDrivesMaxPosition
00252                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMaxPosition){                                
00253                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00254                                         combufDataIter = 0;
00255                                         rxDataIter = 0;
00256                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMaxPosition;
00257                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMaxPosition)) {
00258                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxPosition.data[combufDataIter]);
00259                                                 dataBytesIter = 0;
00260                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxPosition){
00261                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMaxPosition + dataBytesIter);
00262                                                         dataBytesIter++;
00263                                                 }
00264                                                 //NFComBuf->SetDrivesMaxPosition.data[combufDataIter] = ((NF_STRUCT_SetDrivesMaxPosition*)dataPt)->data[rxDataIter];
00265                                                 combufDataIter++;                                               
00266                                                 rxDataIter++;
00267                                         }
00268                                         NFComBuf->SetDrivesMaxPosition.updated = 1;
00269                                 }
00270                         }
00271                         else
00272                 #endif
00273                 // ####         Set Misc
00274                 #ifdef NF_BUFSZ_SetDrivesMisc
00275                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMisc){                               
00276                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00277                                         combufDataIter = 0;
00278                                         rxDataIter = 0;
00279                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMisc;
00280                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMisc)) {
00281                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMisc.data[combufDataIter]);
00282                                                 dataBytesIter = 0;
00283                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMisc){
00284                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMisc + dataBytesIter);
00285                                                         dataBytesIter++;
00286                                                 }
00287                                                 //NFComBuf->SetDrivesMisc.data[combufDataIter] = ((NF_STRUCT_SetDrivesMisc*)dataPt)->data[rxDataIter];
00288                                                 combufDataIter++;                                               
00289                                                 rxDataIter++;
00290                                         }
00291                                         NFComBuf->SetDrivesMisc.updated = 1;
00292                                 }
00293                         }
00294                         else
00295                 #endif
00296                 
00297                 // ########     Regulators
00298                 // ####         Set Current Regulator
00299                 #ifdef NF_BUFSZ_SetCurrentRegulator
00300                         if(rxBuf[rxBufIter] == NF_COMMAND_SetCurrentRegulator){         
00301                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00302                                         combufDataIter = 0;
00303                                         rxDataIter = 0;
00304                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetCurrentRegulator;
00305                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetCurrentRegulator)) {
00306                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetCurrentRegulator.data[combufDataIter]);
00307                                                 dataBytesIter = 0;
00308                                                 while(dataBytesIter < NF_DATABYTES_SetCurrentRegulator){
00309                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetCurrentRegulator + dataBytesIter);
00310                                                         dataBytesIter++;
00311                                                 }
00312                                                 //NFComBuf->SetCurrentRegulator.data[combufDataIter] = ((NF_STRUCT_SetCurrentRegulator*)dataPt)->data[rxDataIter];
00313                                                 combufDataIter++;                                               
00314                                                 rxDataIter++;
00315                                         }
00316                                         NFComBuf->SetCurrentRegulator.updated = 1;
00317                                 }
00318                         }
00319                         else
00320                 #endif
00321                 // ####         Set Speed Regulator
00322                 #ifdef NF_BUFSZ_SetSpeedRegulator
00323                         if(rxBuf[rxBufIter] == NF_COMMAND_SetSpeedRegulator){           
00324                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00325                                         combufDataIter = 0;
00326                                         rxDataIter = 0;
00327                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetSpeedRegulator;
00328                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetSpeedRegulator)) {
00329                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetSpeedRegulator.data[combufDataIter]);
00330                                                 dataBytesIter = 0;
00331                                                 while(dataBytesIter < NF_DATABYTES_SetSpeedRegulator){
00332                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetSpeedRegulator + dataBytesIter);
00333                                                         dataBytesIter++;
00334                                                 }
00335                                                 //NFComBuf->SetSpeedRegulator.data[combufDataIter] = ((NF_STRUCT_SetSpeedRegulator*)dataPt)->data[rxDataIter];
00336                                                 combufDataIter++;                                               
00337                                                 rxDataIter++;
00338                                         }
00339                                         NFComBuf->SetSpeedRegulator.updated = 1;
00340                                 }
00341                         }
00342                         else
00343                 #endif
00344                 // ####         Set Position Regulator
00345                 #ifdef NF_BUFSZ_SetPositionRegulator
00346                         if(rxBuf[rxBufIter] == NF_COMMAND_SetPositionRegulator){                
00347                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00348                                         combufDataIter = 0;
00349                                         rxDataIter = 0;
00350                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetPositionRegulator;
00351                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetPositionRegulator)) {
00352                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetPositionRegulator.data[combufDataIter]);
00353                                                 dataBytesIter = 0;
00354                                                 while(dataBytesIter < NF_DATABYTES_SetPositionRegulator){
00355                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetPositionRegulator + dataBytesIter);
00356                                                         dataBytesIter++;
00357                                                 }
00358                                                 //NFComBuf->SetPositionRegulator.data[combufDataIter] = ((NF_STRUCT_SetPositionRegulator*)dataPt)->data[rxDataIter];
00359                                                 combufDataIter++;                                               
00360                                                 rxDataIter++;
00361                                         }
00362                                         NFComBuf->SetPositionRegulator.updated = 1;
00363                                 }
00364                         }
00365                         else
00366                 #endif
00367                 
00368                 // ########     Servos
00369                 // ####         Set Mode
00370                 #ifdef NF_BUFSZ_SetServosMode
00371                         if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMode){               
00372                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00373                                         combufDataIter = 0;
00374                                         rxDataIter = 0;
00375                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMode;
00376                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMode)) {
00377                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMode.data[combufDataIter]);
00378                                                 dataBytesIter = 0;
00379                                                 while(dataBytesIter < NF_DATABYTES_SetServosMode){
00380                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMode + dataBytesIter);
00381                                                         dataBytesIter++;
00382                                                 }
00383                                                 //NFComBuf->SetServosMode.data[combufDataIter] = ((NF_STRUCT_SetServosMode*)dataPt)->data[rxDataIter];
00384                                                 combufDataIter++;                                               
00385                                                 rxDataIter++;
00386                                         }
00387                                         NFComBuf->SetServosMode.updated = 1;
00388                                 }
00389                         }
00390                         else
00391                 #endif
00392                 // ####         Set Position
00393                 #ifdef NF_BUFSZ_SetServosPosition
00394                         if(rxBuf[rxBufIter] == NF_COMMAND_SetServosPosition){           
00395                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00396                                         combufDataIter = 0;
00397                                         rxDataIter = 0;
00398                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosPosition;
00399                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosPosition)) {
00400                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosPosition.data[combufDataIter]);
00401                                                 dataBytesIter = 0;
00402                                                 while(dataBytesIter < NF_DATABYTES_SetServosPosition){
00403                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosPosition + dataBytesIter);
00404                                                         dataBytesIter++;
00405                                                 }
00406                                                 //NFComBuf->SetServosPosition.data[combufDataIter] = ((NF_STRUCT_SetServosPosition*)dataPt)->data[rxDataIter];
00407                                                 combufDataIter++;                                               
00408                                                 rxDataIter++;
00409                                         }
00410                                         NFComBuf->SetServosPosition.updated = 1;
00411                                 }
00412                         }
00413                         else
00414                 #endif
00415                 // ####         Set Speed
00416                 #ifdef NF_BUFSZ_SetServosSpeed
00417                         if(rxBuf[rxBufIter] == NF_COMMAND_SetServosSpeed){              
00418                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00419                                         combufDataIter = 0;
00420                                         rxDataIter = 0;
00421                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosSpeed;
00422                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosSpeed)) {
00423                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosSpeed.data[combufDataIter]);
00424                                                 dataBytesIter = 0;
00425                                                 while(dataBytesIter < NF_DATABYTES_SetServosSpeed){
00426                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosSpeed + dataBytesIter);
00427                                                         dataBytesIter++;
00428                                                 }
00429                                                 //NFComBuf->SetServosSpeed.data[combufDataIter] = ((NF_STRUCT_SetServosSpeed*)dataPt)->data[rxDataIter];
00430                                                 combufDataIter++;                                               
00431                                                 rxDataIter++;
00432                                         }
00433                                         NFComBuf->SetServosSpeed.updated = 1;
00434                                 }
00435                         }
00436                         else
00437                 #endif
00438                 // ####         Set Max Speed
00439                 #ifdef NF_BUFSZ_SetServosMaxSpeed
00440                         if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMaxSpeed){           
00441                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00442                                         combufDataIter = 0;
00443                                         rxDataIter = 0;
00444                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMaxSpeed;
00445                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMaxSpeed)) {
00446                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMaxSpeed.data[combufDataIter]);
00447                                                 dataBytesIter = 0;
00448                                                 while(dataBytesIter < NF_DATABYTES_SetServosMaxSpeed){
00449                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMaxSpeed + dataBytesIter);
00450                                                         dataBytesIter++;
00451                                                 }
00452                                                 //NFComBuf->SetServosMaxSpeed.data[combufDataIter] = ((NF_STRUCT_SetServosMaxSpeed*)dataPt)->data[rxDataIter];
00453                                                 combufDataIter++;                                               
00454                                                 rxDataIter++;
00455                                         }
00456                                         NFComBuf->SetServosMaxSpeed.updated = 1;
00457                                 }
00458                         }
00459                         else
00460                 #endif
00461                 // ####         Set Min Position
00462                 #ifdef NF_BUFSZ_SetServosMinPosition
00463                         if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMinPosition){                
00464                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00465                                         combufDataIter = 0;
00466                                         rxDataIter = 0;
00467                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMinPosition;
00468                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMinPosition)) {
00469                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMinPosition.data[combufDataIter]);
00470                                                 dataBytesIter = 0;
00471                                                 while(dataBytesIter < NF_DATABYTES_SetServosMinPosition){
00472                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMinPosition + dataBytesIter);
00473                                                         dataBytesIter++;
00474                                                 }
00475                                                 //NFComBuf->SetServosMinPosition.data[combufDataIter] = ((NF_STRUCT_SetServosMinPosition*)dataPt)->data[rxDataIter];
00476                                                 combufDataIter++;                                               
00477                                                 rxDataIter++;
00478                                         }
00479                                         NFComBuf->SetServosMinPosition.updated = 1;
00480                                 }
00481                         }
00482                         else
00483                 #endif
00484                 // ####         Set Max Position
00485                 #ifdef NF_BUFSZ_SetServosMaxPosition
00486                         if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMaxPosition){                
00487                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00488                                         combufDataIter = 0;
00489                                         rxDataIter = 0;
00490                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMaxPosition;
00491                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMaxPosition)) {
00492                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMaxPosition.data[combufDataIter]);
00493                                                 dataBytesIter = 0;
00494                                                 while(dataBytesIter < NF_DATABYTES_SetServosMaxPosition){
00495                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMaxPosition + dataBytesIter);
00496                                                         dataBytesIter++;
00497                                                 }
00498                                                 //NFComBuf->SetServosMaxPosition.data[combufDataIter] = ((NF_STRUCT_SetServosMaxPosition*)dataPt)->data[rxDataIter];
00499                                                 combufDataIter++;                                               
00500                                                 rxDataIter++;
00501                                         }
00502                                         NFComBuf->SetServosMaxPosition.updated = 1;
00503                                 }
00504                         }
00505                         else
00506                 #endif
00507                 // ####         Set Position Adjustment
00508                 #ifdef NF_BUFSZ_SetServosPositionAdj
00509                         if(rxBuf[rxBufIter] == NF_COMMAND_SetServosPositionAdj){                
00510                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00511                                         combufDataIter = 0;
00512                                         rxDataIter = 0;
00513                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosPositionAdj;
00514                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosPositionAdj)) {
00515                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosPositionAdj.data[combufDataIter]);
00516                                                 dataBytesIter = 0;
00517                                                 while(dataBytesIter < NF_DATABYTES_SetServosPositionAdj){
00518                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosPositionAdj + dataBytesIter);
00519                                                         dataBytesIter++;
00520                                                 }
00521                                                 //NFComBuf->SetServosPositionAdj.data[combufDataIter] = ((NF_STRUCT_SetServosPositionAdj*)dataPt)->data[rxDataIter];
00522                                                 combufDataIter++;                                               
00523                                                 rxDataIter++;
00524                                         }
00525                                         NFComBuf->SetServosPositionAdj.updated = 1;
00526                                 }
00527                         }
00528                         else
00529                 #endif
00530                 // ####         Set Misc
00531                 #ifdef NF_BUFSZ_SetServosMisc
00532                         if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMisc){               
00533                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00534                                         combufDataIter = 0;
00535                                         rxDataIter = 0;
00536                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMisc;
00537                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMisc)) {
00538                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMisc.data[combufDataIter]);
00539                                                 dataBytesIter = 0;
00540                                                 while(dataBytesIter < NF_DATABYTES_SetServosMisc){
00541                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMisc + dataBytesIter);
00542                                                         dataBytesIter++;
00543                                                 }
00544                                                 //NFComBuf->SetServosMisc.data[combufDataIter] = ((NF_STRUCT_SetServosMisc*)dataPt)->data[rxDataIter];
00545                                                 combufDataIter++;                                               
00546                                                 rxDataIter++;
00547                                         }
00548                                         NFComBuf->SetServosMisc.updated = 1;
00549                                 }
00550                         }
00551                         else
00552                 #endif
00553                 
00554                 // ########     Digital IO
00555                 // ####         Set Outputs
00556                 #ifdef NF_BUFSZ_SetDigitalOutputs
00557                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDigitalOutputs){           
00558                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00559                                         combufDataIter = 0;
00560                                         rxDataIter = 0;
00561                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDigitalOutputs;
00562                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDigitalOutputs)) {
00563                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDigitalOutputs.data[combufDataIter]);
00564                                                 dataBytesIter = 0;
00565                                                 while(dataBytesIter < NF_DATABYTES_SetDigitalOutputs){
00566                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDigitalOutputs + dataBytesIter);
00567                                                         dataBytesIter++;
00568                                                 }
00569                                                 //NFComBuf->SetDigitalOutputs.data[combufDataIter] = ((NF_STRUCT_SetDigitalOutputs*)dataPt)->data[rxDataIter];
00570                                                 combufDataIter++;                                               
00571                                                 rxDataIter++;
00572                                         }
00573                                         NFComBuf->SetDigitalOutputs.updated = 1;
00574                                 }
00575                         }
00576                         else
00577                 #endif
00578                 
00579                 // ########     Display
00580                 // ####         Set Mode
00581                 #ifdef NF_BUFSZ_SetDisplayMode
00582                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDisplayMode){              
00583                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00584                                         combufDataIter = 0;
00585                                         rxDataIter = 0;
00586                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDisplayMode;
00587                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDisplayMode)) {
00588                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayMode.data[combufDataIter]);
00589                                                 dataBytesIter = 0;
00590                                                 while(dataBytesIter < NF_DATABYTES_SetDisplayMode){
00591                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDisplayMode + dataBytesIter);
00592                                                         dataBytesIter++;
00593                                                 }
00594                                                 //NFComBuf->SetDisplayMode.data[combufDataIter] = ((NF_STRUCT_SetDisplayMode*)dataPt)->data[rxDataIter];
00595                                                 combufDataIter++;                                               
00596                                                 rxDataIter++;
00597                                         }
00598                                         NFComBuf->SetDisplayMode.updated = 1;
00599                                 }
00600                         }
00601                         else
00602                 #endif
00603                 // ####         Set Text
00604                 #ifdef NF_BUFSZ_SetDisplayText
00605                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDisplayText){              
00606                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00607                                         combufDataIter = 0;
00608                                         rxDataIter = 0;
00609                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDisplayText;
00610                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDisplayText)) {
00611                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayText.data[combufDataIter]);
00612                                                 dataBytesIter = 0;
00613                                                 while(dataBytesIter < NF_DATABYTES_SetDisplayText){
00614                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDisplayText + dataBytesIter);
00615                                                         dataBytesIter++;
00616                                                 }
00617                                                 //NFComBuf->SetDisplayText.data[combufDataIter] = ((NF_STRUCT_SetDisplayText*)dataPt)->data[rxDataIter];
00618                                                 combufDataIter++;                                               
00619                                                 rxDataIter++;
00620                                         }
00621                                         NFComBuf->SetDisplayText.updated = 1;
00622                                 }
00623                         }
00624                         else
00625                 #endif
00626                 // ####         Set Backlight
00627                 #ifdef NF_BUFSZ_SetDisplayBacklight
00628                         if(rxBuf[rxBufIter] == NF_COMMAND_SetDisplayBacklight){         
00629                                 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00630                                         combufDataIter = 0;
00631                                         rxDataIter = 0;
00632                                         rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDisplayBacklight;
00633                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDisplayBacklight)) {
00634                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayBacklight.data[combufDataIter]);
00635                                                 dataBytesIter = 0;
00636                                                 while(dataBytesIter < NF_DATABYTES_SetDisplayBacklight){
00637                                                         *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDisplayBacklight + dataBytesIter);
00638                                                         dataBytesIter++;
00639                                                 }
00640                                                 //NFComBuf->SetDisplayBacklight.data[combufDataIter] = ((NF_STRUCT_SetDisplayBacklight*)dataPt)->data[rxDataIter];
00641                                                 combufDataIter++;                                               
00642                                                 rxDataIter++;
00643                                         }
00644                                         NFComBuf->SetDisplayBacklight.updated = 1;
00645                                 }
00646                         }
00647                         else
00648                 #endif
00649                 
00650                 // ############### "READ" Type Commands
00651                 // if rxParamsCnt == 0 && rxAddress == NFComBuf->myAddress
00652                 //              Master wants to acquire data from me (slave)
00653                 // if rxParamsCnt > 0  && rxAddress == NFComBuf->xxx.addr[i]
00654                 //              Slave returns data that I (master) asked for
00655 
00656                 // ########     Drives
00657                 // ####         Read Current
00658                 #ifdef NF_BUFSZ_ReadDrivesCurrent
00659                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadDrivesCurrent){
00660                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDrivesCurrent;
00661                                 // Master wants to acquire data from me (slave)
00662                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00663                                         commandArray[*commandCnt] = NF_COMMAND_ReadDrivesCurrent;
00664                                         (*commandCnt) ++;
00665                                 }
00666                                 // Slave returns data that I (master) asked for
00667                                 else if(rxParamsCnt > 0){
00668                                         combufDataIter = 0;
00669                                         rxDataIter = 0;
00670                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDrivesCurrent)) {
00671                                                 if(NFComBuf->ReadDrivesCurrent.addr[combufDataIter] == rxAddress) {
00672                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesCurrent.data[combufDataIter]);
00673                                                         dataBytesIter = 0;
00674                                                         while(dataBytesIter < NF_DATABYTES_ReadDrivesCurrent){
00675                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDrivesCurrent + dataBytesIter);
00676                                                                 dataBytesIter++;
00677                                                         }
00678                                                         //NFComBuf->ReadDrivesCurrent.data[combufDataIter] = ((NF_STRUCT_ReadDrivesCurrent*)dataPt)->data[rxDataIter];
00679                                                         rxDataIter++;
00680                                                 }
00681                                                 combufDataIter++;
00682                                         }
00683                                 }
00684                         }
00685                         else
00686                 #endif
00687                 // ####         Read Speed
00688                 #ifdef NF_BUFSZ_ReadDrivesSpeed
00689                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadDrivesSpeed){
00690                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDrivesSpeed;
00691                                 // Master wants to acquire data from me (slave)
00692                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00693                                         commandArray[*commandCnt] = NF_COMMAND_ReadDrivesSpeed;
00694                                         (*commandCnt) ++;
00695                                 }
00696                                 // Slave returns data that I (master) asked for
00697                                 else if(rxParamsCnt > 0){
00698                                         combufDataIter = 0;
00699                                         rxDataIter = 0;
00700                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDrivesSpeed)) {
00701                                                 if(NFComBuf->ReadDrivesSpeed.addr[combufDataIter] == rxAddress) {
00702                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesSpeed.data[combufDataIter]);
00703                                                         dataBytesIter = 0;
00704                                                         while(dataBytesIter < NF_DATABYTES_ReadDrivesSpeed){
00705                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDrivesSpeed + dataBytesIter);
00706                                                                 dataBytesIter++;
00707                                                         }
00708                                                         //NFComBuf->ReadDrivesSpeed.data[combufDataIter] = ((NF_STRUCT_ReadDrivesSpeed*)dataPt)->data[rxDataIter];
00709                                                         rxDataIter++;
00710                                                 }
00711                                                 combufDataIter++;
00712                                         }
00713                                 }
00714                         }
00715                         else
00716                 #endif
00717                 // ####         Read Position
00718                 #ifdef NF_BUFSZ_ReadDrivesPosition
00719                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadDrivesPosition){
00720                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDrivesPosition;
00721                                 // Master wants to acquire data from me (slave)
00722                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00723                                         commandArray[*commandCnt] = NF_COMMAND_ReadDrivesPosition;
00724                                         (*commandCnt) ++;
00725                                 }
00726                                 // Slave returns data that I (master) asked for
00727                                 else if(rxParamsCnt > 0){
00728                                         combufDataIter = 0;
00729                                         rxDataIter = 0;
00730                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDrivesPosition)) {
00731                                                 if(NFComBuf->ReadDrivesPosition.addr[combufDataIter] == rxAddress) {
00732                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesPosition.data[combufDataIter]);
00733                                                         dataBytesIter = 0;
00734                                                         while(dataBytesIter < NF_DATABYTES_ReadDrivesPosition){
00735                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDrivesPosition + dataBytesIter);
00736                                                                 dataBytesIter++;
00737                                                         }
00738                                                         //NFComBuf->ReadDrivesPosition.data[combufDataIter] = ((NF_STRUCT_ReadDrivesPosition*)dataPt)->data[rxDataIter];
00739                                                         rxDataIter++;
00740                                                 }
00741                                                 combufDataIter++;
00742                                         }
00743                                 }
00744                         }
00745                         else
00746                 #endif
00747                 // ####         Read Status
00748                 #ifdef NF_BUFSZ_ReadDrivesStatus
00749                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadDrivesStatus){
00750                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDrivesStatus;
00751                                 // Master wants to acquire data from me (slave)
00752                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00753                                         commandArray[*commandCnt] = NF_COMMAND_ReadDrivesStatus;
00754                                         (*commandCnt) ++;
00755                                 }
00756                                 // Slave returns data that I (master) asked for
00757                                 else if(rxParamsCnt > 0){
00758                                         combufDataIter = 0;
00759                                         rxDataIter = 0;
00760                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDrivesStatus)) {
00761                                                 if(NFComBuf->ReadDrivesStatus.addr[combufDataIter] == rxAddress) {
00762                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesStatus.data[combufDataIter]);
00763                                                         dataBytesIter = 0;
00764                                                         while(dataBytesIter < NF_DATABYTES_ReadDrivesStatus){
00765                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDrivesStatus + dataBytesIter);
00766                                                                 dataBytesIter++;
00767                                                         }
00768                                                         //NFComBuf->ReadDrivesStatus.data[combufDataIter] = ((NF_STRUCT_ReadDrivesStatus*)dataPt)->data[rxDataIter];
00769                                                         rxDataIter++;
00770                                                 }
00771                                                 combufDataIter++;
00772                                         }
00773                                 }
00774                         }
00775                         else
00776                 #endif
00777 
00778                 // ########     Servos
00779                 // ####         Read Current
00780                 #ifdef NF_BUFSZ_ReadServosCurrent
00781                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadServosCurrent){
00782                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadServosCurrent;
00783                                 // Master wants to acquire data from me (slave)
00784                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00785                                         commandArray[*commandCnt] = NF_COMMAND_ReadServosCurrent;
00786                                         (*commandCnt) ++;
00787                                 }
00788                                 // Slave returns data that I (master) asked for
00789                                 else if(rxParamsCnt > 0){
00790                                         combufDataIter = 0;
00791                                         rxDataIter = 0;
00792                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadServosCurrent)) {
00793                                                 if(NFComBuf->ReadServosCurrent.addr[combufDataIter] == rxAddress) {
00794                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadServosCurrent.data[combufDataIter]);
00795                                                         dataBytesIter = 0;
00796                                                         while(dataBytesIter < NF_DATABYTES_ReadServosCurrent){
00797                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadServosCurrent + dataBytesIter);
00798                                                                 dataBytesIter++;
00799                                                         }
00800                                                         //NFComBuf->ReadServosCurrent.data[combufDataIter] = ((NF_STRUCT_ReadServosCurrent*)dataPt)->data[rxDataIter];
00801                                                         rxDataIter++;
00802                                                 }
00803                                                 combufDataIter++;
00804                                         }
00805                                 }
00806                         }
00807                         else
00808                 #endif
00809                 // ####         Read Position
00810                 #ifdef NF_BUFSZ_ReadServosPosition
00811                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadServosPosition){
00812                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadServosPosition;
00813                                 // Master wants to acquire data from me (slave)
00814                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00815                                         commandArray[*commandCnt] = NF_COMMAND_ReadServosPosition;
00816                                         (*commandCnt) ++;
00817                                 }
00818                                 // Slave returns data that I (master) asked for
00819                                 else if(rxParamsCnt > 0){
00820                                         combufDataIter = 0;
00821                                         rxDataIter = 0;
00822                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadServosPosition)) {
00823                                                 if(NFComBuf->ReadServosPosition.addr[combufDataIter] == rxAddress) {
00824                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadServosPosition.data[combufDataIter]);
00825                                                         dataBytesIter = 0;
00826                                                         while(dataBytesIter < NF_DATABYTES_ReadServosPosition){
00827                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadServosPosition + dataBytesIter);
00828                                                                 dataBytesIter++;
00829                                                         }
00830                                                         //NFComBuf->ReadServosPosition.data[combufDataIter] = ((NF_STRUCT_ReadServosPosition*)dataPt)->data[rxDataIter];
00831                                                         rxDataIter++;
00832                                                 }
00833                                                 combufDataIter++;
00834                                         }
00835                                 }
00836                         }
00837                         else
00838                 #endif
00839                 // ####         Read Status
00840                 #ifdef NF_BUFSZ_ReadServosStatus
00841                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadServosStatus){
00842                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadServosStatus;
00843                                 // Master wants to acquire data from me (slave)
00844                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00845                                         commandArray[*commandCnt] = NF_COMMAND_ReadServosStatus;
00846                                         (*commandCnt) ++;
00847                                 }
00848                                 // Slave returns data that I (master) asked for
00849                                 else if(rxParamsCnt > 0){
00850                                         combufDataIter = 0;
00851                                         rxDataIter = 0;
00852                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadServosStatus)) {
00853                                                 if(NFComBuf->ReadServosStatus.addr[combufDataIter] == rxAddress) {
00854                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadServosStatus.data[combufDataIter]);
00855                                                         dataBytesIter = 0;
00856                                                         while(dataBytesIter < NF_DATABYTES_ReadServosStatus){
00857                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadServosStatus + dataBytesIter);
00858                                                                 dataBytesIter++;
00859                                                         }
00860                                                         //NFComBuf->ReadServosStatus.data[combufDataIter] = ((NF_STRUCT_ReadServosStatus*)dataPt)->data[rxDataIter];
00861                                                         rxDataIter++;
00862                                                 }
00863                                                 combufDataIter++;
00864                                         }
00865                                 }
00866                         }
00867                         else
00868                 #endif
00869                 
00870                 // ########     Digital IO
00871                 // ####         Read Inputs
00872                 #ifdef NF_BUFSZ_ReadDigitalInputs
00873                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadDigitalInputs){
00874                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDigitalInputs;
00875                                 // Master wants to acquire data from me (slave)
00876                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00877                                         commandArray[*commandCnt] = NF_COMMAND_ReadDigitalInputs;
00878                                         (*commandCnt) ++;
00879                                 }
00880                                 // Slave returns data that I (master) asked for
00881                                 else if(rxParamsCnt > 0){
00882                                         combufDataIter = 0;
00883                                         rxDataIter = 0;
00884                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDigitalInputs)) {
00885                                                 if(NFComBuf->ReadDigitalInputs.addr[combufDataIter] == rxAddress) {
00886                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadDigitalInputs.data[combufDataIter]);
00887                                                         dataBytesIter = 0;
00888                                                         while(dataBytesIter < NF_DATABYTES_ReadDigitalInputs){
00889                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDigitalInputs + dataBytesIter);
00890                                                                 dataBytesIter++;
00891                                                         }
00892                                                         //NFComBuf->ReadDigitalInputs.data[combufDataIter] = ((NF_STRUCT_ReadDigitalInputs*)dataPt)->data[rxDataIter];
00893                                                         rxDataIter++;                                           
00894                                                 }
00895                                                 combufDataIter++;
00896                                         }
00897                                 }
00898                         }
00899                         else
00900                 #endif
00901                 
00902                 // ########     Analog IO
00903                 // ####         Read Inputs
00904                 #ifdef NF_BUFSZ_ReadAnalogInputs
00905                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadAnalogInputs){
00906                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadAnalogInputs;
00907                                 // Master wants to acquire data from me (slave)
00908                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00909                                         commandArray[*commandCnt] = NF_COMMAND_ReadAnalogInputs;
00910                                         (*commandCnt) ++;
00911                                 }
00912                                 // Slave returns data that I (master) asked for
00913                                 else if(rxParamsCnt > 0){
00914                                         combufDataIter = 0;
00915                                         rxDataIter = 0;
00916                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadAnalogInputs)) {
00917                                                 if(NFComBuf->ReadAnalogInputs.addr[combufDataIter] == rxAddress) {
00918                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadAnalogInputs.data[combufDataIter]);
00919                                                         dataBytesIter = 0;
00920                                                         while(dataBytesIter < NF_DATABYTES_ReadAnalogInputs){
00921                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadAnalogInputs + dataBytesIter);
00922                                                                 dataBytesIter++;
00923                                                         }
00924                                                         //NFComBuf->ReadAnalogInputs.data[combufDataIter] = ((NF_STRUCT_ReadAnalogInputs*)dataPt)->data[rxDataIter];
00925                                                         rxDataIter++;                                           
00926                                                 }
00927                                                 combufDataIter++;
00928                                         }
00929                                 }
00930                         }
00931                         else
00932                 #endif
00933                 
00934                 // ########     Sensors
00935                 // ####         Read Distance
00936                 #ifdef NF_BUFSZ_ReadDistance
00937                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadDistance){
00938                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDistance;
00939                                 // Master wants to acquire data from me (slave)
00940                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00941                                         commandArray[*commandCnt] = NF_COMMAND_ReadDistance;
00942                                         (*commandCnt) ++;
00943                                 }
00944                                 // Slave returns data that I (master) asked for
00945                                 else if(rxParamsCnt > 0){
00946                                         combufDataIter = 0;
00947                                         rxDataIter = 0;
00948                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDistance)) {
00949                                                 if(NFComBuf->ReadDistance.addr[combufDataIter] == rxAddress) {
00950                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadDistance.data[combufDataIter]);
00951                                                         dataBytesIter = 0;
00952                                                         while(dataBytesIter < NF_DATABYTES_ReadDistance){
00953                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDistance + dataBytesIter);
00954                                                                 dataBytesIter++;
00955                                                         }
00956                                                         //NFComBuf->ReadDistance.data[combufDataIter] = ((NF_STRUCT_ReadDistance*)dataPt)->data[rxDataIter];
00957                                                         rxDataIter++;                                           
00958                                                 }
00959                                                 combufDataIter++;
00960                                         }
00961                                 }
00962                         }
00963                         else
00964                 #endif
00965                 
00966                 // ########     Device
00967                 // ####         Read Status
00968                 #ifdef NF_BUFSZ_ReadDeviceStatus
00969                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadDeviceStatus){
00970                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDeviceStatus;
00971                                 // Master wants to acquire data from me (slave)
00972                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00973                                         commandArray[*commandCnt] = NF_COMMAND_ReadDeviceStatus;
00974                                         (*commandCnt) ++;
00975                                 }
00976                                 // Slave returns data that I (master) asked for
00977                                 else if(rxParamsCnt > 0){
00978                                         combufDataIter = 0;
00979                                         rxDataIter = 0;
00980                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDeviceStatus)) {
00981                                                 if(NFComBuf->ReadDeviceStatus.addr[combufDataIter] == rxAddress) {
00982                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadDeviceStatus.data[combufDataIter]);
00983                                                         dataBytesIter = 0;
00984                                                         while(dataBytesIter < NF_DATABYTES_ReadDeviceStatus){
00985                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDeviceStatus + dataBytesIter);
00986                                                                 dataBytesIter++;
00987                                                         }
00988                                                         //NFComBuf->ReadDeviceStatus.data[combufDataIter] = ((NF_STRUCT_ReadDeviceStatus*)dataPt)->data[rxDataIter];
00989                                                         rxDataIter++;                                           
00990                                                 }
00991                                                 combufDataIter++;
00992                                         }
00993                                 }
00994                         }
00995                         else
00996                 #endif
00997                 // ####         Read Vitals
00998                 #ifdef NF_BUFSZ_ReadDeviceVitals
00999                         if(rxBuf[rxBufIter] == NF_COMMAND_ReadDeviceVitals){
01000                                 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDeviceVitals;
01001                                 // Master wants to acquire data from me (slave)
01002                                 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
01003                                         commandArray[*commandCnt] = NF_COMMAND_ReadDeviceVitals;
01004                                         (*commandCnt) ++;
01005                                 }
01006                                 // Slave returns data that I (master) asked for
01007                                 else if(rxParamsCnt > 0){
01008                                         combufDataIter = 0;
01009                                         rxDataIter = 0;
01010                                         while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDeviceVitals)) {
01011                                                 if(NFComBuf->ReadDeviceVitals.addr[combufDataIter] == rxAddress) {
01012                                                         u8TempPt = (uint8_t*) &(NFComBuf->ReadDeviceVitals.data[combufDataIter]);
01013                                                         dataBytesIter = 0;
01014                                                         while(dataBytesIter < NF_DATABYTES_ReadDeviceVitals){
01015                                                                 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDeviceVitals + dataBytesIter);
01016                                                                 dataBytesIter++;
01017                                                         }
01018                                                         //NFComBuf->ReadDeviceVitals.data[combufDataIter] = ((NF_STRUCT_ReadDeviceVitals*)dataPt)->data[rxDataIter];
01019                                                         rxDataIter++;                                           
01020                                                 }
01021                                                 combufDataIter++;
01022                                         }
01023                                 }
01024                         }
01025                         else
01026                 #endif
01027                                 NFComBuf->unknownCommandReceived = 1;
01028                         
01029                 
01030                 rxBufIter += rxBuf[rxBufIter+1]+2;
01031         }
01032         
01033         return rxBufIter;
01034 }
01035 
01036 /*
01037 * Funkcja buduje ramke komunikatu na podstawie danych w buforze komunikacji oraz listy zadanych polecen.
01038 * NFComBuf - wskaznik do bufora komunikacyjnego, w ktorej automatycznie beda umieszczane odebrane dane.
01039 * txBuf - wskaznik do tablicy, w ktorej zostanie utworzona wiadomosc.
01040 * commandArray - wskaznik do tablicy polecen, ktore maja byc zawarte w komunikacie.
01041 * commandCnt - ilosc polecen w tablicy commandArray[].
01042 * Funkcja zwraca dlugosc zbudowanego komunikatu, zapisanego w txBuf.
01043 */
01044 uint8_t NF_MakeCommandFrame(NF_STRUCT_ComBuf *NFComBuf,
01045                                                         uint8_t *txBuf,
01046                                                         const uint8_t *commandArray,
01047                                                         uint8_t commandCnt,
01048                                                         uint8_t txAddress){
01049         uint8_t txBufIter, commandIter, txDataIter, combufDataIter, dataBytesIter;      
01050         
01051         uint8_t *dataPt;
01052         uint8_t *u8TempPt;
01053          
01054         txBuf[0] = '#';
01055         txBuf[3] = txAddress;
01056 
01057         txBufIter=4;
01058         commandIter=0;
01059                 
01060         while(commandIter < commandCnt){
01061         
01062                 dataPt = txBuf+txBufIter+2;
01063 
01064                 // ########     Drives
01065                 // ####         Set Mode
01066                 #ifdef NF_BUFSZ_SetDrivesMode
01067                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesMode){
01068                                 combufDataIter = 0;
01069                                 txDataIter = 0;
01070                                 while(combufDataIter < NF_BUFSZ_SetDrivesMode) {
01071                                         if(NFComBuf->SetDrivesMode.addr[combufDataIter] == txAddress) {
01072                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMode.data[combufDataIter]);
01073                                                 dataBytesIter = 0;
01074                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMode){
01075                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMode + dataBytesIter) = *(u8TempPt + dataBytesIter);
01076                                                         dataBytesIter++;
01077                                                 }
01078                                                 //((NF_STRUCT_SetDrivesMode*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesMode.data[combufDataIter];
01079                                                 txDataIter++;                                           
01080                                         }
01081                                         combufDataIter++;
01082                                 }
01083                                 if(txDataIter > 0){
01084                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesMode;
01085                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMode;   
01086                                         txBufIter += txBuf[txBufIter+1]+2;                      
01087                                 }
01088                         }
01089                         else
01090                 #endif
01091                 // ####         Set Speed
01092                 #ifdef NF_BUFSZ_SetDrivesSpeed
01093                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesSpeed){
01094                                 combufDataIter = 0;
01095                                 txDataIter = 0;
01096                                 while(combufDataIter < NF_BUFSZ_SetDrivesSpeed) {
01097                                         if(NFComBuf->SetDrivesSpeed.addr[combufDataIter] == txAddress) {
01098                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesSpeed.data[combufDataIter]);
01099                                                 dataBytesIter = 0;
01100                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesSpeed){
01101                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01102                                                         dataBytesIter++;
01103                                                 }
01104                                                 //((NF_STRUCT_SetDrivesSpeed*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesSpeed.data[combufDataIter];
01105                                                 txDataIter++;                                           
01106                                         }
01107                                         combufDataIter++;
01108                                 }
01109                                 if(txDataIter > 0){
01110                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesSpeed;
01111                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesSpeed;  
01112                                         txBufIter += txBuf[txBufIter+1]+2;                      
01113                                 }
01114                         }
01115                         else
01116                 #endif
01117                 // ####         Set Current
01118                 #ifdef NF_BUFSZ_SetDrivesCurrent
01119                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesCurrent){
01120                                 combufDataIter = 0;
01121                                 txDataIter = 0;
01122                                 while(combufDataIter < NF_BUFSZ_SetDrivesCurrent) {
01123                                         if(NFComBuf->SetDrivesCurrent.addr[combufDataIter] == txAddress) {
01124                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesCurrent.data[combufDataIter]);
01125                                                 dataBytesIter = 0;
01126                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesCurrent){
01127                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesCurrent + dataBytesIter) = *(u8TempPt + dataBytesIter);
01128                                                         dataBytesIter++;
01129                                                 }
01130                                                 //((NF_STRUCT_SetDrivesCurrent*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesCurrent.data[combufDataIter];
01131                                                 txDataIter++;                                           
01132                                         }
01133                                         combufDataIter++;
01134                                 }
01135                                 if(txDataIter > 0){
01136                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesCurrent;
01137                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesCurrent;        
01138                                         txBufIter += txBuf[txBufIter+1]+2;                      
01139                                 }
01140                         }
01141                         else
01142                 #endif
01143                 // ####         Set Position
01144                 #ifdef NF_BUFSZ_SetDrivesPosition
01145                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesPosition){
01146                                 combufDataIter = 0;
01147                                 txDataIter = 0;
01148                                 while(combufDataIter < NF_BUFSZ_SetDrivesPosition) {
01149                                         if(NFComBuf->SetDrivesPosition.addr[combufDataIter] == txAddress) {
01150                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesPosition.data[combufDataIter]);
01151                                                 dataBytesIter = 0;
01152                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesPosition){
01153                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01154                                                         dataBytesIter++;
01155                                                 }
01156                                                 //((NF_STRUCT_SetDrivesPosition*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesPosition.data[combufDataIter];
01157                                                 txDataIter++;                                           
01158                                         }
01159                                         combufDataIter++;
01160                                 }
01161                                 if(txDataIter > 0){
01162                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesPosition;
01163                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesPosition;       
01164                                         txBufIter += txBuf[txBufIter+1]+2;                      
01165                                 }
01166                         }
01167                         else
01168                 #endif  
01169                 // ####         Set PWM
01170                 #ifdef NF_BUFSZ_SetDrivesPWM
01171                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesPWM){
01172                                 combufDataIter = 0;
01173                                 txDataIter = 0;
01174                                 while(combufDataIter < NF_BUFSZ_SetDrivesPWM) {
01175                                         if(NFComBuf->SetDrivesPWM.addr[combufDataIter] == txAddress) {
01176                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesPWM.data[combufDataIter]);
01177                                                 dataBytesIter = 0;
01178                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesPWM){
01179                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesPWM + dataBytesIter) = *(u8TempPt + dataBytesIter);
01180                                                         dataBytesIter++;
01181                                                 }
01182                                                 //((NF_STRUCT_SetDrivesPosition*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesPosition.data[combufDataIter];
01183                                                 txDataIter++;                                           
01184                                         }
01185                                         combufDataIter++;
01186                                 }
01187                                 if(txDataIter > 0){
01188                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesPWM;
01189                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesPWM;    
01190                                         txBufIter += txBuf[txBufIter+1]+2;                      
01191                                 }
01192                         }
01193                         else
01194                 #endif  
01195                 // ####         Set Max Current
01196                 #ifdef NF_BUFSZ_SetDrivesMaxCurrent
01197                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesMaxCurrent){
01198                                 combufDataIter = 0;
01199                                 txDataIter = 0;
01200                                 while(combufDataIter < NF_BUFSZ_SetDrivesMaxCurrent) {
01201                                         if(NFComBuf->SetDrivesMaxCurrent.addr[combufDataIter] == txAddress) {
01202                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxCurrent.data[combufDataIter]);
01203                                                 dataBytesIter = 0;
01204                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxCurrent){
01205                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMaxCurrent + dataBytesIter) = *(u8TempPt + dataBytesIter);
01206                                                         dataBytesIter++;
01207                                                 }
01208                                                 //((NF_STRUCT_SetDrivesPosition*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesPosition.data[combufDataIter];
01209                                                 txDataIter++;                                           
01210                                         }
01211                                         combufDataIter++;
01212                                 }
01213                                 if(txDataIter > 0){
01214                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesMaxCurrent;
01215                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMaxCurrent;     
01216                                         txBufIter += txBuf[txBufIter+1]+2;                      
01217                                 }
01218                         }
01219                         else
01220                 #endif          
01221                 // ####         Set Max Speed
01222                 #ifdef NF_BUFSZ_SetDrivesMaxSpeed
01223                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesMaxSpeed){
01224                                 combufDataIter = 0;
01225                                 txDataIter = 0;
01226                                 while(combufDataIter < NF_BUFSZ_SetDrivesMaxSpeed) {
01227                                         if(NFComBuf->SetDrivesMaxSpeed.addr[combufDataIter] == txAddress) {
01228                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxSpeed.data[combufDataIter]);
01229                                                 dataBytesIter = 0;
01230                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxSpeed){
01231                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMaxSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01232                                                         dataBytesIter++;
01233                                                 }
01234                                                 //((NF_STRUCT_SetDrivesPosition*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesPosition.data[combufDataIter];
01235                                                 txDataIter++;                                           
01236                                         }
01237                                         combufDataIter++;
01238                                 }
01239                                 if(txDataIter > 0){
01240                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesMaxSpeed;
01241                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMaxSpeed;       
01242                                         txBufIter += txBuf[txBufIter+1]+2;                      
01243                                 }
01244                         }
01245                         else
01246                 #endif  
01247                 // ####         Set Min Position
01248                 #ifdef NF_BUFSZ_SetDrivesMinPosition
01249                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesMinPosition){
01250                                 combufDataIter = 0;
01251                                 txDataIter = 0;
01252                                 while(combufDataIter < NF_BUFSZ_SetDrivesMinPosition) {
01253                                         if(NFComBuf->SetDrivesMinPosition.addr[combufDataIter] == txAddress) {
01254                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMinPosition.data[combufDataIter]);
01255                                                 dataBytesIter = 0;
01256                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMinPosition){
01257                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMinPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01258                                                         dataBytesIter++;
01259                                                 }
01260                                                 //((NF_STRUCT_SetDrivesPosition*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesPosition.data[combufDataIter];
01261                                                 txDataIter++;                                           
01262                                         }
01263                                         combufDataIter++;
01264                                 }
01265                                 if(txDataIter > 0){
01266                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesMinPosition;
01267                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMinPosition;    
01268                                         txBufIter += txBuf[txBufIter+1]+2;                      
01269                                 }
01270                         }
01271                         else
01272                 #endif
01273                 // ####         Set MaxPosition
01274                 #ifdef NF_BUFSZ_SetDrivesMaxPosition
01275                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesMaxPosition){
01276                                 combufDataIter = 0;
01277                                 txDataIter = 0;
01278                                 while(combufDataIter < NF_BUFSZ_SetDrivesMaxPosition) {
01279                                         if(NFComBuf->SetDrivesMaxPosition.addr[combufDataIter] == txAddress) {
01280                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxPosition.data[combufDataIter]);
01281                                                 dataBytesIter = 0;
01282                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxPosition){
01283                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMaxPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01284                                                         dataBytesIter++;
01285                                                 }
01286                                                 //((NF_STRUCT_SetDrivesPosition*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesPosition.data[combufDataIter];
01287                                                 txDataIter++;                                           
01288                                         }
01289                                         combufDataIter++;
01290                                 }
01291                                 if(txDataIter > 0){
01292                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesMaxPosition;
01293                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMaxPosition;    
01294                                         txBufIter += txBuf[txBufIter+1]+2;                      
01295                                 }
01296                         }
01297                         else
01298                 #endif
01299                 // ####         Set Misc
01300                 #ifdef NF_BUFSZ_SetDrivesMisc
01301                         if(commandArray[commandIter] == NF_COMMAND_SetDrivesMisc){
01302                                 combufDataIter = 0;
01303                                 txDataIter = 0;
01304                                 while(combufDataIter < NF_BUFSZ_SetDrivesMisc) {
01305                                         if(NFComBuf->SetDrivesMisc.addr[combufDataIter] == txAddress) {
01306                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMisc.data[combufDataIter]);
01307                                                 dataBytesIter = 0;
01308                                                 while(dataBytesIter < NF_DATABYTES_SetDrivesMisc){
01309                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMisc + dataBytesIter) = *(u8TempPt + dataBytesIter);
01310                                                         dataBytesIter++;
01311                                                 }
01312                                                 //((NF_STRUCT_SetDrivesPosition*)dataPt)->data[txDataIter] = NFComBuf->SetDrivesPosition.data[combufDataIter];
01313                                                 txDataIter++;                                           
01314                                         }
01315                                         combufDataIter++;
01316                                 }
01317                                 if(txDataIter > 0){
01318                                         txBuf[txBufIter] = NF_COMMAND_SetDrivesMisc;
01319                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMisc;   
01320                                         txBufIter += txBuf[txBufIter+1]+2;                      
01321                                 }
01322                         }
01323                         else
01324                 #endif
01325 
01326                 // ########     Regulators
01327                 // ####         Set Current Regulator
01328                 #ifdef NF_BUFSZ_SetCurrentRegulator
01329                         if(commandArray[commandIter] == NF_COMMAND_SetCurrentRegulator){
01330                                 combufDataIter = 0;
01331                                 txDataIter = 0;
01332                                 while(combufDataIter < NF_BUFSZ_SetCurrentRegulator) {
01333                                         if(NFComBuf->SetCurrentRegulator.addr[combufDataIter] == txAddress) {
01334                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetCurrentRegulator.data[combufDataIter]);
01335                                                 dataBytesIter = 0;
01336                                                 while(dataBytesIter < NF_DATABYTES_SetCurrentRegulator){
01337                                                         *(dataPt + txDataIter*NF_DATABYTES_SetCurrentRegulator + dataBytesIter) = *(u8TempPt + dataBytesIter);
01338                                                         dataBytesIter++;
01339                                                 }
01340                                                 //((NF_STRUCT_SetCurrentRegulator*)dataPt)->data[txDataIter] = NFComBuf->SetCurrentRegulator.data[combufDataIter];
01341                                                 txDataIter++;                                           
01342                                         }
01343                                         combufDataIter++;
01344                                 }
01345                                 if(txDataIter > 0){
01346                                         txBuf[txBufIter] = NF_COMMAND_SetCurrentRegulator;
01347                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetCurrentRegulator;     
01348                                         txBufIter += txBuf[txBufIter+1]+2;                      
01349                                 }
01350                         }
01351                         else
01352                 #endif
01353                 // ####         Set Speed Regulator
01354                 #ifdef NF_BUFSZ_SetSpeedRegulator
01355                         if(commandArray[commandIter] == NF_COMMAND_SetSpeedRegulator){
01356                                 combufDataIter = 0;
01357                                 txDataIter = 0;
01358                                 while(combufDataIter < NF_BUFSZ_SetSpeedRegulator) {
01359                                         if(NFComBuf->SetSpeedRegulator.addr[combufDataIter] == txAddress) {
01360                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetSpeedRegulator.data[combufDataIter]);
01361                                                 dataBytesIter = 0;
01362                                                 while(dataBytesIter < NF_DATABYTES_SetSpeedRegulator){
01363                                                         *(dataPt + txDataIter*NF_DATABYTES_SetSpeedRegulator + dataBytesIter) = *(u8TempPt + dataBytesIter);
01364                                                         dataBytesIter++;
01365                                                 }
01366                                                 //((NF_STRUCT_SetSpeedRegulator*)dataPt)->data[txDataIter] = NFComBuf->SetSpeedRegulator.data[combufDataIter];
01367                                                 txDataIter++;                                           
01368                                         }
01369                                         combufDataIter++;
01370                                 }
01371                                 if(txDataIter > 0){
01372                                         txBuf[txBufIter] = NF_COMMAND_SetSpeedRegulator;
01373                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetSpeedRegulator;       
01374                                         txBufIter += txBuf[txBufIter+1]+2;                      
01375                                 }
01376                         }
01377                         else
01378                 #endif
01379                 // ####         Set Position Regulator
01380                 #ifdef NF_BUFSZ_SetPositionRegulator
01381                         if(commandArray[commandIter] == NF_COMMAND_SetPositionRegulator){
01382                                 combufDataIter = 0;
01383                                 txDataIter = 0;
01384                                 while(combufDataIter < NF_BUFSZ_SetPositionRegulator) {
01385                                         if(NFComBuf->SetPositionRegulator.addr[combufDataIter] == txAddress) {
01386                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetPositionRegulator.data[combufDataIter]);
01387                                                 dataBytesIter = 0;
01388                                                 while(dataBytesIter < NF_DATABYTES_SetPositionRegulator){
01389                                                         *(dataPt + txDataIter*NF_DATABYTES_SetPositionRegulator + dataBytesIter) = *(u8TempPt + dataBytesIter);
01390                                                         dataBytesIter++;
01391                                                 }
01392                                                 //((NF_STRUCT_SetPositionRegulator*)dataPt)->data[txDataIter] = NFComBuf->SetPositionRegulator.data[combufDataIter];
01393                                                 txDataIter++;                                           
01394                                         }
01395                                         combufDataIter++;
01396                                 }
01397                                 if(txDataIter > 0){
01398                                         txBuf[txBufIter] = NF_COMMAND_SetPositionRegulator;
01399                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetPositionRegulator;    
01400                                         txBufIter += txBuf[txBufIter+1]+2;                      
01401                                 }
01402                         }
01403                         else
01404                 #endif
01405                 
01406                 // ########     Servos
01407                 // ####         Set Mode
01408                 #ifdef NF_BUFSZ_SetServosMode
01409                         if(commandArray[commandIter] == NF_COMMAND_SetServosMode){
01410                                 combufDataIter = 0;
01411                                 txDataIter = 0;
01412                                 while(combufDataIter < NF_BUFSZ_SetServosMode) {
01413                                         if(NFComBuf->SetServosMode.addr[combufDataIter] == txAddress) {
01414                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMode.data[combufDataIter]);
01415                                                 dataBytesIter = 0;
01416                                                 while(dataBytesIter < NF_DATABYTES_SetServosMode){
01417                                                         *(dataPt + txDataIter*NF_DATABYTES_SetServosMode + dataBytesIter) = *(u8TempPt + dataBytesIter);
01418                                                         dataBytesIter++;
01419                                                 }
01420                                                 //((NF_STRUCT_SetServosMode*)dataPt)->data[txDataIter] = NFComBuf->SetServosMode.data[combufDataIter];
01421                                                 txDataIter++;                                           
01422                                         }
01423                                         combufDataIter++;
01424                                 }
01425                                 if(txDataIter > 0){
01426                                         txBuf[txBufIter] = NF_COMMAND_SetServosMode;
01427                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMode;   
01428                                         txBufIter += txBuf[txBufIter+1]+2;                      
01429                                 }
01430                         }
01431                         else
01432                 #endif
01433                 // ####         Set Position
01434                 #ifdef NF_BUFSZ_SetServosPosition
01435                         if(commandArray[commandIter] == NF_COMMAND_SetServosPosition){
01436                                 combufDataIter = 0;
01437                                 txDataIter = 0;
01438                                 while(combufDataIter < NF_BUFSZ_SetServosPosition) {
01439                                         if(NFComBuf->SetServosPosition.addr[combufDataIter] == txAddress) {
01440                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosPosition.data[combufDataIter]);
01441                                                 dataBytesIter = 0;
01442                                                 while(dataBytesIter < NF_DATABYTES_SetServosPosition){
01443                                                         *(dataPt + txDataIter*NF_DATABYTES_SetServosPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01444                                                         dataBytesIter++;
01445                                                 }
01446                                                 //((NF_STRUCT_SetServosPosition*)dataPt)->data[txDataIter] = NFComBuf->SetServosPosition.data[combufDataIter];
01447                                                 txDataIter++;                                           
01448                                         }
01449                                         combufDataIter++;
01450                                 }
01451                                 if(txDataIter > 0){
01452                                         txBuf[txBufIter] = NF_COMMAND_SetServosPosition;
01453                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosPosition;       
01454                                         txBufIter += txBuf[txBufIter+1]+2;                      
01455                                 }
01456                         }
01457                         else
01458                 #endif
01459                 // ####         Set Speed
01460                 #ifdef NF_BUFSZ_SetServosSpeed
01461                         if(commandArray[commandIter] == NF_COMMAND_SetServosSpeed){
01462                                 combufDataIter = 0;
01463                                 txDataIter = 0;
01464                                 while(combufDataIter < NF_BUFSZ_SetServosSpeed) {
01465                                         if(NFComBuf->SetServosSpeed.addr[combufDataIter] == txAddress) {
01466                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosSpeed.data[combufDataIter]);
01467                                                 dataBytesIter = 0;
01468                                                 while(dataBytesIter < NF_DATABYTES_SetServosSpeed){
01469                                                         *(dataPt + txDataIter*NF_DATABYTES_SetServosSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01470                                                         dataBytesIter++;
01471                                                 }
01472                                                 //((NF_STRUCT_SetServosSpeed*)dataPt)->data[txDataIter] = NFComBuf->SetServosSpeed.data[combufDataIter];
01473                                                 txDataIter++;                                           
01474                                         }
01475                                         combufDataIter++;
01476                                 }
01477                                 if(txDataIter > 0){
01478                                         txBuf[txBufIter] = NF_COMMAND_SetServosSpeed;
01479                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosSpeed;  
01480                                         txBufIter += txBuf[txBufIter+1]+2;                      
01481                                 }
01482                         }
01483                         else
01484                 #endif   
01485                 // ####         Set Max Speed
01486                 #ifdef NF_BUFSZ_SetServosMaxSpeed
01487                         if(commandArray[commandIter] == NF_COMMAND_SetServosMaxSpeed){
01488                                 combufDataIter = 0;
01489                                 txDataIter = 0;
01490                                 while(combufDataIter < NF_BUFSZ_SetServosMaxSpeed) {
01491                                         if(NFComBuf->SetServosMaxSpeed.addr[combufDataIter] == txAddress) {
01492                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMaxSpeed.data[combufDataIter]);
01493                                                 dataBytesIter = 0;
01494                                                 while(dataBytesIter < NF_DATABYTES_SetServosMaxSpeed){
01495                                                         *(dataPt + txDataIter*NF_DATABYTES_SetServosMaxSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01496                                                         dataBytesIter++;
01497                                                 }
01498                                                 //((NF_STRUCT_SetServosPosition*)dataPt)->data[txDataIter] = NFComBuf->SetServosPosition.data[combufDataIter];
01499                                                 txDataIter++;                                           
01500                                         }
01501                                         combufDataIter++;
01502                                 }
01503                                 if(txDataIter > 0){
01504                                         txBuf[txBufIter] = NF_COMMAND_SetServosMaxSpeed;
01505                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMaxSpeed;       
01506                                         txBufIter += txBuf[txBufIter+1]+2;                      
01507                                 }
01508                         }
01509                         else
01510                 #endif  
01511                 // ####         Set Min Position
01512                 #ifdef NF_BUFSZ_SetServosMinPosition
01513                         if(commandArray[commandIter] == NF_COMMAND_SetServosMinPosition){
01514                                 combufDataIter = 0;
01515                                 txDataIter = 0;
01516                                 while(combufDataIter < NF_BUFSZ_SetServosMinPosition) {
01517                                         if(NFComBuf->SetServosMinPosition.addr[combufDataIter] == txAddress) {
01518                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMinPosition.data[combufDataIter]);
01519                                                 dataBytesIter = 0;
01520                                                 while(dataBytesIter < NF_DATABYTES_SetServosMinPosition){
01521                                                         *(dataPt + txDataIter*NF_DATABYTES_SetServosMinPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01522                                                         dataBytesIter++;
01523                                                 }
01524                                                 //((NF_STRUCT_SetServosPosition*)dataPt)->data[txDataIter] = NFComBuf->SetServosPosition.data[combufDataIter];
01525                                                 txDataIter++;                                           
01526                                         }
01527                                         combufDataIter++;
01528                                 }
01529                                 if(txDataIter > 0){
01530                                         txBuf[txBufIter] = NF_COMMAND_SetServosMinPosition;
01531                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMinPosition;    
01532                                         txBufIter += txBuf[txBufIter+1]+2;                      
01533                                 }
01534                         }
01535                         else
01536                 #endif
01537                 // ####         Set MaxPosition
01538                 #ifdef NF_BUFSZ_SetServosMaxPosition
01539                         if(commandArray[commandIter] == NF_COMMAND_SetServosMaxPosition){
01540                                 combufDataIter = 0;
01541                                 txDataIter = 0;
01542                                 while(combufDataIter < NF_BUFSZ_SetServosMaxPosition) {
01543                                         if(NFComBuf->SetServosMaxPosition.addr[combufDataIter] == txAddress) {
01544                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMaxPosition.data[combufDataIter]);
01545                                                 dataBytesIter = 0;
01546                                                 while(dataBytesIter < NF_DATABYTES_SetServosMaxPosition){
01547                                                         *(dataPt + txDataIter*NF_DATABYTES_SetServosMaxPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01548                                                         dataBytesIter++;
01549                                                 }
01550                                                 //((NF_STRUCT_SetServosPosition*)dataPt)->data[txDataIter] = NFComBuf->SetServosPosition.data[combufDataIter];
01551                                                 txDataIter++;                                           
01552                                         }
01553                                         combufDataIter++;
01554                                 }
01555                                 if(txDataIter > 0){
01556                                         txBuf[txBufIter] = NF_COMMAND_SetServosMaxPosition;
01557                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMaxPosition;    
01558                                         txBufIter += txBuf[txBufIter+1]+2;                      
01559                                 }
01560                         }
01561                         else
01562                 #endif
01563                 // ####         Set Misc
01564                 #ifdef NF_BUFSZ_SetServosMisc
01565                         if(commandArray[commandIter] == NF_COMMAND_SetServosMisc){
01566                                 combufDataIter = 0;
01567                                 txDataIter = 0;
01568                                 while(combufDataIter < NF_BUFSZ_SetServosMisc) {
01569                                         if(NFComBuf->SetServosMisc.addr[combufDataIter] == txAddress) {
01570                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMisc.data[combufDataIter]);
01571                                                 dataBytesIter = 0;
01572                                                 while(dataBytesIter < NF_DATABYTES_SetServosMisc){
01573                                                         *(dataPt + txDataIter*NF_DATABYTES_SetServosMisc + dataBytesIter) = *(u8TempPt + dataBytesIter);
01574                                                         dataBytesIter++;
01575                                                 }
01576                                                 //((NF_STRUCT_SetServosPosition*)dataPt)->data[txDataIter] = NFComBuf->SetServosPosition.data[combufDataIter];
01577                                                 txDataIter++;                                           
01578                                         }
01579                                         combufDataIter++;
01580                                 }
01581                                 if(txDataIter > 0){
01582                                         txBuf[txBufIter] = NF_COMMAND_SetServosMisc;
01583                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMisc;   
01584                                         txBufIter += txBuf[txBufIter+1]+2;                      
01585                                 }
01586                         }
01587                         else
01588                 #endif 
01589 
01590                 // ########     Digital IO
01591                 // ####         Set Outputs
01592                 #ifdef NF_BUFSZ_SetDigitalOutputs
01593                         if(commandArray[commandIter] == NF_COMMAND_SetDigitalOutputs){
01594                                 combufDataIter = 0;
01595                                 txDataIter = 0;
01596                                 while(combufDataIter < NF_BUFSZ_SetDigitalOutputs) {
01597                                         if(NFComBuf->SetDigitalOutputs.addr[combufDataIter] == txAddress) {
01598                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDigitalOutputs.data[combufDataIter]);
01599                                                 dataBytesIter = 0;
01600                                                 while(dataBytesIter < NF_DATABYTES_SetDigitalOutputs){
01601                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDigitalOutputs + dataBytesIter) = *(u8TempPt + dataBytesIter);
01602                                                         dataBytesIter++;
01603                                                 }
01604                                                 //((NF_STRUCT_SetDigitalOutputs*)dataPt)->data[txDataIter] = NFComBuf->SetDigitalOutputs.data[combufDataIter];
01605                                                 txDataIter++;                                           
01606                                         }
01607                                         combufDataIter++;
01608                                 }
01609                                 if(txDataIter > 0){
01610                                         txBuf[txBufIter] = NF_COMMAND_SetDigitalOutputs;
01611                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDigitalOutputs;       
01612                                         txBufIter += txBuf[txBufIter+1]+2;                      
01613                                 }
01614                         }
01615                         else
01616                 #endif 
01617 
01618                 // ########     Display
01619                 // ####         Set Mode
01620                 #ifdef NF_BUFSZ_SetDisplayMode
01621                         if(commandArray[commandIter] == NF_COMMAND_SetDisplayMode){
01622                                 combufDataIter = 0;
01623                                 txDataIter = 0;
01624                                 while(combufDataIter < NF_BUFSZ_SetDisplayMode) {
01625                                         if(NFComBuf->SetDisplayMode.addr[combufDataIter] == txAddress) {
01626                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayMode.data[combufDataIter]);
01627                                                 dataBytesIter = 0;
01628                                                 while(dataBytesIter < NF_DATABYTES_SetDisplayMode){
01629                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDisplayMode + dataBytesIter) = *(u8TempPt + dataBytesIter);
01630                                                         dataBytesIter++;
01631                                                 }
01632                                                 //((NF_STRUCT_SetDisplayMode*)dataPt)->data[txDataIter] = NFComBuf->SetDisplayMode.data[combufDataIter];
01633                                                 txDataIter++;                                           
01634                                         }
01635                                         combufDataIter++;
01636                                 }
01637                                 if(txDataIter > 0){
01638                                         txBuf[txBufIter] = NF_COMMAND_SetDisplayMode;
01639                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDisplayMode;  
01640                                         txBufIter += txBuf[txBufIter+1]+2;                      
01641                                 }
01642                         }
01643                         else
01644                 #endif 
01645                 // ####         Set Text
01646                 #ifdef NF_BUFSZ_SetDisplayText
01647                         if(commandArray[commandIter] == NF_COMMAND_SetDisplayText){
01648                                 combufDataIter = 0;
01649                                 txDataIter = 0;
01650                                 while(combufDataIter < NF_BUFSZ_SetDisplayText) {
01651                                         if(NFComBuf->SetDisplayText.addr[combufDataIter] == txAddress) {
01652                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayText.data[combufDataIter]);
01653                                                 dataBytesIter = 0;
01654                                                 while(dataBytesIter < NF_DATABYTES_SetDisplayText){
01655                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDisplayText + dataBytesIter) = *(u8TempPt + dataBytesIter);
01656                                                         dataBytesIter++;
01657                                                 }
01658                                                 //((NF_STRUCT_SetDisplayText*)dataPt)->data[txDataIter] = NFComBuf->SetDisplayText.data[combufDataIter];
01659                                                 txDataIter++;                                           
01660                                         }
01661                                         combufDataIter++;
01662                                 }
01663                                 if(txDataIter > 0){
01664                                         txBuf[txBufIter] = NF_COMMAND_SetDisplayText;
01665                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDisplayText;  
01666                                         txBufIter += txBuf[txBufIter+1]+2;                      
01667                                 }
01668                         }
01669                         else
01670                 #endif 
01671                 // ####         Set Backlight
01672                 #ifdef NF_BUFSZ_SetDisplayBacklight
01673                         if(commandArray[commandIter] == NF_COMMAND_SetDisplayBacklight){
01674                                 combufDataIter = 0;
01675                                 txDataIter = 0;
01676                                 while(combufDataIter < NF_BUFSZ_SetDisplayBacklight) {
01677                                         if(NFComBuf->SetDisplayBacklight.addr[combufDataIter] == txAddress) {
01678                                                 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayBacklight.data[combufDataIter]);
01679                                                 dataBytesIter = 0;
01680                                                 while(dataBytesIter < NF_DATABYTES_SetDisplayBacklight){
01681                                                         *(dataPt + txDataIter*NF_DATABYTES_SetDisplayBacklight + dataBytesIter) = *(u8TempPt + dataBytesIter);
01682                                                         dataBytesIter++;
01683                                                 }
01684                                                 //((NF_STRUCT_SetDisplayBacklight*)dataPt)->data[txDataIter] = NFComBuf->SetDisplayBacklight.data[combufDataIter];
01685                                                 txDataIter++;                                           
01686                                         }
01687                                         combufDataIter++;
01688                                 }
01689                                 if(txDataIter > 0){
01690                                         txBuf[txBufIter] = NF_COMMAND_SetDisplayBacklight;
01691                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDisplayBacklight;     
01692                                         txBufIter += txBuf[txBufIter+1]+2;                      
01693                                 }
01694                         }
01695                         else
01696                 #endif 
01697                 
01698                 // ############### "READ" Type Commands
01699                 // if txAddress == NFComBuf->myAddress
01700                 //              I (slave) return data that master asked for
01701                 // if rxAddress == NFComBuf->xxx.addr[i]
01702                 //              I (master) want to acquire data from slave
01703                 
01704                 // ########     Device
01705                 // ####         Read Status
01706                 #ifdef NF_BUFSZ_ReadDeviceStatus
01707                         if(commandArray[commandIter] == NF_COMMAND_ReadDeviceStatus){
01708                                 // I (slave) return data that master asked for
01709                                 if(txAddress == NFComBuf->myAddress){                                   
01710                                         combufDataIter = 0;
01711                                         txDataIter = 0;
01712                                         while(combufDataIter < NF_BUFSZ_ReadDeviceStatus) {
01713                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadDeviceStatus.data[combufDataIter]);
01714                                                 dataBytesIter = 0;
01715                                                 while(dataBytesIter < NF_DATABYTES_ReadDeviceStatus){
01716                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadDeviceStatus + dataBytesIter) = *(u8TempPt + dataBytesIter);
01717                                                         dataBytesIter++;
01718                                                 }
01719                                                 txDataIter++;                                           
01720                                                 combufDataIter++;
01721                                         }
01722                                         txBuf[txBufIter] = NF_COMMAND_ReadDeviceStatus;
01723                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDeviceStatus;        
01724                                         txBufIter += txBuf[txBufIter+1]+2;                      
01725                                 }
01726                                 // I (master) want to acquire data from slave
01727                                 else{
01728                                         txBuf[txBufIter] = NF_COMMAND_ReadDeviceStatus;
01729                                         txBuf[txBufIter+1] = 0; 
01730                                         txBufIter += 2;
01731                                         
01732                                 }
01733                         }
01734                         else
01735                 #endif
01736                 // ####         Read Vitals
01737                 #ifdef NF_BUFSZ_ReadDeviceVitals
01738                         if(commandArray[commandIter] == NF_COMMAND_ReadDeviceVitals){
01739                                 // I (slave) return data that master asked for
01740                                 if(txAddress == NFComBuf->myAddress){                                   
01741                                         combufDataIter = 0;
01742                                         txDataIter = 0;
01743                                         while(combufDataIter < NF_BUFSZ_ReadDeviceVitals) {
01744                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadDeviceVitals.data[combufDataIter]);
01745                                                 dataBytesIter = 0;
01746                                                 while(dataBytesIter < NF_DATABYTES_ReadDeviceVitals){
01747                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadDeviceVitals + dataBytesIter) = *(u8TempPt + dataBytesIter);
01748                                                         dataBytesIter++;
01749                                                 }
01750                                                 txDataIter++;                                           
01751                                                 combufDataIter++;
01752                                         }
01753                                         txBuf[txBufIter] = NF_COMMAND_ReadDeviceVitals;
01754                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDeviceVitals;        
01755                                         txBufIter += txBuf[txBufIter+1]+2;                      
01756                                 }
01757                                 // I (master) want to acquire data from slave
01758                                 else{
01759                                         txBuf[txBufIter] = NF_COMMAND_ReadDeviceVitals;
01760                                         txBuf[txBufIter+1] = 0; 
01761                                         txBufIter += 2;
01762                                         
01763                                 }
01764                         }
01765                         else
01766                 #endif
01767                 
01768                 // ########     Drives
01769                 // ####         Read Position
01770                 #ifdef NF_BUFSZ_ReadDrivesPosition
01771                         if(commandArray[commandIter] == NF_COMMAND_ReadDrivesPosition){
01772                                 // I (slave) return data that master asked for
01773                                 if(txAddress == NFComBuf->myAddress){
01774                                         combufDataIter = 0;
01775                                         txDataIter = 0;
01776                                         while(combufDataIter < NF_BUFSZ_ReadDrivesPosition) {
01777                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesPosition.data[combufDataIter]);
01778                                                 dataBytesIter = 0;
01779                                                 while(dataBytesIter < NF_DATABYTES_ReadDrivesPosition){
01780                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadDrivesPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01781                                                         dataBytesIter++;
01782                                                 }
01783                                                 txDataIter++;
01784                                                 combufDataIter++;
01785                                         }
01786                                         txBuf[txBufIter] = NF_COMMAND_ReadDrivesPosition;
01787                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDrivesPosition;
01788                                         txBufIter += txBuf[txBufIter+1]+2;
01789                                 }
01790                                 // I (master) want to acquire data from slave
01791                                 else{
01792                                         txBuf[txBufIter] = NF_COMMAND_ReadDrivesPosition;
01793                                         txBuf[txBufIter+1] = 0;
01794                                         txBufIter += 2;
01795 
01796                                 }
01797                         }
01798                         else
01799                 #endif
01800                 // ####         Read Current
01801                 #ifdef NF_BUFSZ_ReadDrivesCurrent
01802                         if(commandArray[commandIter] == NF_COMMAND_ReadDrivesCurrent){
01803                                 // I (slave) return data that master asked for
01804                                 if(txAddress == NFComBuf->myAddress){
01805                                         combufDataIter = 0;
01806                                         txDataIter = 0;
01807                                         while(combufDataIter < NF_BUFSZ_ReadDrivesCurrent) {
01808                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesCurrent.data[combufDataIter]);
01809                                                 dataBytesIter = 0;
01810                                                 while(dataBytesIter < NF_DATABYTES_ReadDrivesCurrent){
01811                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadDrivesCurrent + dataBytesIter) = *(u8TempPt + dataBytesIter);
01812                                                         dataBytesIter++;
01813                                                 }
01814                                                 txDataIter++;
01815                                                 combufDataIter++;
01816                                         }
01817                                         txBuf[txBufIter] = NF_COMMAND_ReadDrivesCurrent;
01818                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDrivesCurrent;
01819                                         txBufIter += txBuf[txBufIter+1]+2;
01820                                 }
01821                                 // I (master) want to acquire data from slave
01822                                 else{
01823                                         txBuf[txBufIter] = NF_COMMAND_ReadDrivesCurrent;
01824                                         txBuf[txBufIter+1] = 0;
01825                                         txBufIter += 2;
01826 
01827                                 }
01828                         }
01829                         else
01830                 #endif
01831                 // ####         Read Speed
01832                 #ifdef NF_BUFSZ_ReadDrivesSpeed
01833                         if(commandArray[commandIter] == NF_COMMAND_ReadDrivesSpeed){
01834                                 // I (slave) return data that master asked for
01835                                 if(txAddress == NFComBuf->myAddress){
01836                                         combufDataIter = 0;
01837                                         txDataIter = 0;
01838                                         while(combufDataIter < NF_BUFSZ_ReadDrivesSpeed) {
01839                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesSpeed.data[combufDataIter]);
01840                                                 dataBytesIter = 0;
01841                                                 while(dataBytesIter < NF_DATABYTES_ReadDrivesSpeed){
01842                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadDrivesSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01843                                                         dataBytesIter++;
01844                                                 }
01845                                                 txDataIter++;
01846                                                 combufDataIter++;
01847                                         }
01848                                         txBuf[txBufIter] = NF_COMMAND_ReadDrivesSpeed;
01849                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDrivesSpeed;
01850                                         txBufIter += txBuf[txBufIter+1]+2;
01851                                 }
01852                                 // I (master) want to acquire data from slave
01853                                 else{
01854                                         txBuf[txBufIter] = NF_COMMAND_ReadDrivesSpeed;
01855                                         txBuf[txBufIter+1] = 0;
01856                                         txBufIter += 2;
01857 
01858                                 }
01859                         }
01860                         else
01861                 #endif
01862                 // ####         Read Status
01863                 #ifdef NF_BUFSZ_ReadDrivesStatus
01864                         if(commandArray[commandIter] == NF_COMMAND_ReadDrivesStatus){
01865                                 // I (slave) return data that master asked for
01866                                 if(txAddress == NFComBuf->myAddress){                                   
01867                                         combufDataIter = 0;
01868                                         txDataIter = 0;
01869                                         while(combufDataIter < NF_BUFSZ_ReadDrivesStatus) {
01870                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesStatus.data[combufDataIter]);
01871                                                 dataBytesIter = 0;
01872                                                 while(dataBytesIter < NF_DATABYTES_ReadDrivesStatus){
01873                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadDrivesStatus + dataBytesIter) = *(u8TempPt + dataBytesIter);
01874                                                         dataBytesIter++;
01875                                                 }
01876                                                 txDataIter++;                                           
01877                                                 combufDataIter++;
01878                                         }
01879                                         txBuf[txBufIter] = NF_COMMAND_ReadDrivesStatus;
01880                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDrivesStatus;        
01881                                         txBufIter += txBuf[txBufIter+1]+2;                      
01882                                 }
01883                                 // I (master) want to acquire data from slave
01884                                 else{
01885                                         txBuf[txBufIter] = NF_COMMAND_ReadDrivesStatus;
01886                                         txBuf[txBufIter+1] = 0; 
01887                                         txBufIter += 2;
01888                                         
01889                                 }
01890                         }
01891                         else
01892                 #endif
01893                 
01894                 // ########     Servos
01895                 // ####         Read Position
01896                 #ifdef NF_BUFSZ_ReadServosPosition
01897                         if(commandArray[commandIter] == NF_COMMAND_ReadServosPosition){
01898                                 // I (slave) return data that master asked for
01899                                 if(txAddress == NFComBuf->myAddress){
01900                                         combufDataIter = 0;
01901                                         txDataIter = 0;
01902                                         while(combufDataIter < NF_BUFSZ_ReadServosPosition) {
01903                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosPosition.data[combufDataIter]);
01904                                                 dataBytesIter = 0;
01905                                                 while(dataBytesIter < NF_DATABYTES_ReadServosPosition){
01906                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadServosPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01907                                                         dataBytesIter++;
01908                                                 }
01909                                                 txDataIter++;
01910                                                 combufDataIter++;
01911                                         }
01912                                         txBuf[txBufIter] = NF_COMMAND_ReadServosPosition;
01913                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadServosPosition;
01914                                         txBufIter += txBuf[txBufIter+1]+2;
01915                                 }
01916                                 // I (master) want to acquire data from slave
01917                                 else{
01918                                         txBuf[txBufIter] = NF_COMMAND_ReadServosPosition;
01919                                         txBuf[txBufIter+1] = 0;
01920                                         txBufIter += 2;
01921 
01922                                 }
01923                         }
01924                         else
01925                 #endif
01926                 // ####         Read Current
01927                 #ifdef NF_BUFSZ_ReadServosCurrent
01928                         if(commandArray[commandIter] == NF_COMMAND_ReadServosCurrent){
01929                                 // I (slave) return data that master asked for
01930                                 if(txAddress == NFComBuf->myAddress){
01931                                         combufDataIter = 0;
01932                                         txDataIter = 0;
01933                                         while(combufDataIter < NF_BUFSZ_ReadServosCurrent) {
01934                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosCurrent.data[combufDataIter]);
01935                                                 dataBytesIter = 0;
01936                                                 while(dataBytesIter < NF_DATABYTES_ReadServosCurrent){
01937                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadServosCurrent + dataBytesIter) = *(u8TempPt + dataBytesIter);
01938                                                         dataBytesIter++;
01939                                                 }
01940                                                 txDataIter++;
01941                                                 combufDataIter++;
01942                                         }
01943                                         txBuf[txBufIter] = NF_COMMAND_ReadServosCurrent;
01944                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadServosCurrent;
01945                                         txBufIter += txBuf[txBufIter+1]+2;
01946                                 }
01947                                 // I (master) want to acquire data from slave
01948                                 else{
01949                                         txBuf[txBufIter] = NF_COMMAND_ReadServosCurrent;
01950                                         txBuf[txBufIter+1] = 0;
01951                                         txBufIter += 2;
01952 
01953                                 }
01954                         }
01955                         else
01956                 #endif
01957                 // ####         Read Status
01958                 #ifdef NF_BUFSZ_ReadServosStatus
01959                         if(commandArray[commandIter] == NF_COMMAND_ReadServosStatus){
01960                                 // I (slave) return data that master asked for
01961                                 if(txAddress == NFComBuf->myAddress){                                   
01962                                         combufDataIter = 0;
01963                                         txDataIter = 0;
01964                                         while(combufDataIter < NF_BUFSZ_ReadServosStatus) {
01965                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosStatus.data[combufDataIter]);
01966                                                 dataBytesIter = 0;
01967                                                 while(dataBytesIter < NF_DATABYTES_ReadServosStatus){
01968                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadServosStatus + dataBytesIter) = *(u8TempPt + dataBytesIter);
01969                                                         dataBytesIter++;
01970                                                 }
01971                                                 txDataIter++;                                           
01972                                                 combufDataIter++;
01973                                         }
01974                                         txBuf[txBufIter] = NF_COMMAND_ReadServosStatus;
01975                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadServosStatus;        
01976                                         txBufIter += txBuf[txBufIter+1]+2;                      
01977                                 }
01978                                 // I (master) want to acquire data from slave
01979                                 else{
01980                                         txBuf[txBufIter] = NF_COMMAND_ReadServosStatus;
01981                                         txBuf[txBufIter+1] = 0; 
01982                                         txBufIter += 2;
01983                                         
01984                                 }
01985                         }
01986                         else
01987                 #endif
01988 
01989                 // ########     Analog IO
01990                 // ####         Read Inputs
01991                 #ifdef NF_BUFSZ_ReadAnalogInputs
01992                         if(commandArray[commandIter] == NF_COMMAND_ReadAnalogInputs){
01993                                 // I (slave) return data that master asked for
01994                                 if(txAddress == NFComBuf->myAddress){                                   
01995                                         combufDataIter = 0;
01996                                         txDataIter = 0;
01997                                         while(combufDataIter < NF_BUFSZ_ReadAnalogInputs) {
01998                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadAnalogInputs.data[combufDataIter]);
01999                                                 dataBytesIter = 0;
02000                                                 while(dataBytesIter < NF_DATABYTES_ReadAnalogInputs){
02001                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadAnalogInputs + dataBytesIter) = *(u8TempPt + dataBytesIter);
02002                                                         dataBytesIter++;
02003                                                 }
02004                                                 txDataIter++;                                           
02005                                                 combufDataIter++;
02006                                         }
02007                                         txBuf[txBufIter] = NF_COMMAND_ReadAnalogInputs;
02008                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadAnalogInputs;        
02009                                         txBufIter += txBuf[txBufIter+1]+2;                      
02010                                 }
02011                                 // I (master) want to acquire data from slave
02012                                 else{
02013                                         txBuf[txBufIter] = NF_COMMAND_ReadAnalogInputs;
02014                                         txBuf[txBufIter+1] = 0; 
02015                                         txBufIter += 2;
02016                                         
02017                                 }
02018                         }
02019                         else
02020                 #endif
02021 
02022                 // ########     Sensors
02023                 // ####         Read Distance
02024                 #ifdef NF_BUFSZ_ReadDistance
02025                         if(commandArray[commandIter] == NF_COMMAND_ReadDistance){
02026                                 // I (slave) return data that master asked for
02027                                 if(txAddress == NFComBuf->myAddress){                                   
02028                                         combufDataIter = 0;
02029                                         txDataIter = 0;
02030                                         while(combufDataIter < NF_BUFSZ_ReadDistance) {
02031                                                 u8TempPt = (uint8_t*) &(NFComBuf->ReadDistance.data[combufDataIter]);
02032                                                 dataBytesIter = 0;
02033                                                 while(dataBytesIter < NF_DATABYTES_ReadDistance){
02034                                                         *(dataPt + txDataIter*NF_DATABYTES_ReadDistance + dataBytesIter) = *(u8TempPt + dataBytesIter);
02035                                                         dataBytesIter++;
02036                                                 }
02037                                                 txDataIter++;                                           
02038                                                 combufDataIter++;
02039                                         }
02040                                         txBuf[txBufIter] = NF_COMMAND_ReadDistance;
02041                                         txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDistance;    
02042                                         txBufIter += txBuf[txBufIter+1]+2;                      
02043                                 }
02044                                 // I (master) want to acquire data from slave
02045                                 else{
02046                                         txBuf[txBufIter] = NF_COMMAND_ReadDistance;
02047                                         txBuf[txBufIter+1] = 0; 
02048                                         txBufIter += 2;
02049                                         
02050                                 }
02051                         }
02052                         else
02053                 #endif
02054                 
02055                         {
02056                                 NFComBuf->unknownCommandRequested = 1;  
02057                                 commandIter++;
02058                                 continue;
02059                         }
02060                 commandIter++;
02061         }
02062         txBuf[txBufIter] = NFv2_CrcFast(((const uint8_t*)txBuf) + 3,  txBufIter-3);
02063         txBuf[1] = txBufIter;
02064         txBuf[2] = ~txBufIter;
02065 
02066 
02067         return txBufIter+1;
02068 }
02069 
02070 void NF_ComBufReset(NF_STRUCT_ComBuf *NFComBuf){
02071         uint16_t combufDataIter;
02072         // ########     Device
02073         // ####         Read Status
02074         #ifdef NF_BUFSZ_ReadDeviceStatus
02075                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDeviceStatus; combufDataIter++){
02076                         NFComBuf->ReadDeviceStatus.addr[combufDataIter] = NF_BroadcastAddress;
02077                         NFComBuf->ReadDeviceStatus.data[combufDataIter] = 0;
02078                 }
02079         #endif
02080         // ####         Read Vitals
02081         #ifdef NF_BUFSZ_ReadDeviceVitals
02082                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDeviceVitals; combufDataIter++){
02083                         NFComBuf->ReadDeviceVitals.addr[combufDataIter] = NF_BroadcastAddress;
02084                         NFComBuf->ReadDeviceVitals.data[combufDataIter] = 0;
02085                 }
02086         #endif
02087         
02088         // ########     Drives
02089         // ####         Set Mode
02090         #ifdef NF_BUFSZ_SetDrivesMode
02091                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMode; combufDataIter++){
02092                         NFComBuf->SetDrivesMode.addr[combufDataIter] = NF_BroadcastAddress;
02093                         NFComBuf->SetDrivesMode.data[combufDataIter] = 0;
02094                 }
02095         #endif
02096         // ####         Set Speed
02097         #ifdef NF_BUFSZ_SetDrivesSpeed
02098                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesSpeed; combufDataIter++){
02099                         NFComBuf->SetDrivesSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02100                         NFComBuf->SetDrivesSpeed.data[combufDataIter] = 0;
02101                 }
02102         #endif
02103         // ####         Set Current
02104         #ifdef NF_BUFSZ_SetDrivesCurrent
02105                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesCurrent; combufDataIter++){
02106                         NFComBuf->SetDrivesCurrent.addr[combufDataIter] = NF_BroadcastAddress;
02107                         NFComBuf->SetDrivesCurrent.data[combufDataIter] = 0;
02108                 }
02109         #endif
02110         // ####         Set Position
02111         #ifdef NF_BUFSZ_SetDrivesPosition
02112                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesPosition; combufDataIter++){
02113                         NFComBuf->SetDrivesPosition.addr[combufDataIter] = NF_BroadcastAddress;
02114                         NFComBuf->SetDrivesPosition.data[combufDataIter] = 0;
02115                 }
02116         #endif
02117         // ####         Set PWM
02118         #ifdef NF_BUFSZ_SetDrivesPWM
02119                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesPWM; combufDataIter++){
02120                         NFComBuf->SetDrivesPWM.addr[combufDataIter] = NF_BroadcastAddress;
02121                         NFComBuf->SetDrivesPWM.data[combufDataIter] = 0;
02122                 }
02123         #endif
02124         // ####         Set Max Current
02125         #ifdef NF_BUFSZ_SetDrivesMaxCurrent
02126                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMaxCurrent; combufDataIter++){
02127                         NFComBuf->SetDrivesMaxCurrent.addr[combufDataIter] = NF_BroadcastAddress;
02128                         NFComBuf->SetDrivesMaxCurrent.data[combufDataIter] = 0;
02129                 }
02130         #endif
02131         // ####         Set Max Speed
02132         #ifdef NF_BUFSZ_SetDrivesMaxSpeed
02133                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMaxSpeed; combufDataIter++){
02134                         NFComBuf->SetDrivesMaxSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02135                         NFComBuf->SetDrivesMaxSpeed.data[combufDataIter] = 0;
02136                 }
02137         #endif
02138         // ####         Set Min Position
02139         #ifdef NF_BUFSZ_SetDrivesMinPosition
02140                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMinPosition; combufDataIter++){
02141                         NFComBuf->SetDrivesMinPosition.addr[combufDataIter] = NF_BroadcastAddress;
02142                         NFComBuf->SetDrivesMinPosition.data[combufDataIter] = 0;
02143                 }
02144         #endif
02145         // ####         Set MaxPosition
02146         #ifdef NF_BUFSZ_SetDrivesMaxPosition
02147                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMaxPosition; combufDataIter++){
02148                         NFComBuf->SetDrivesMaxPosition.addr[combufDataIter] = NF_BroadcastAddress;
02149                         NFComBuf->SetDrivesMaxPosition.data[combufDataIter] = 0;
02150                 }
02151         #endif
02152         // ####         Read Current
02153         #ifdef NF_BUFSZ_ReadDrivesCurrent
02154                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDrivesCurrent; combufDataIter++){
02155                         NFComBuf->ReadDrivesCurrent.addr[combufDataIter] = NF_BroadcastAddress;
02156                         NFComBuf->ReadDrivesCurrent.data[combufDataIter] = 0;
02157                 }
02158         #endif
02159         // ####         Read Speed
02160         #ifdef NF_BUFSZ_ReadDrivesSpeed
02161                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDrivesSpeed; combufDataIter++){
02162                         NFComBuf->ReadDrivesSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02163                         NFComBuf->ReadDrivesSpeed.data[combufDataIter] = 0;
02164                 }
02165         #endif
02166         // ####         Read Position
02167         #ifdef NF_BUFSZ_ReadDrivesPosition
02168                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDrivesPosition; combufDataIter++){
02169                         NFComBuf->ReadDrivesPosition.addr[combufDataIter] = NF_BroadcastAddress;
02170                         NFComBuf->ReadDrivesPosition.data[combufDataIter] = 0;
02171                 }
02172         #endif
02173         // ####         Set Misc
02174         #ifdef NF_BUFSZ_SetDrivesMisc
02175                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMisc; combufDataIter++){
02176                         NFComBuf->SetDrivesMisc.addr[combufDataIter] = NF_BroadcastAddress;
02177                         NFComBuf->SetDrivesMisc.data[combufDataIter] = 0;
02178                 }
02179         #endif
02180         // ####         Read Status
02181         #ifdef NF_BUFSZ_ReadDrivesStatus
02182                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDrivesStatus; combufDataIter++){
02183                         NFComBuf->ReadDrivesStatus.addr[combufDataIter] = NF_BroadcastAddress;
02184                         NFComBuf->ReadDrivesStatus.data[combufDataIter] = 0;
02185                 }
02186         #endif
02187         // ########     Servos
02188         // ####         Set Mode
02189         #ifdef NF_BUFSZ_SetServosMode
02190                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMode; combufDataIter++){
02191                         NFComBuf->SetServosMode.addr[combufDataIter] = NF_BroadcastAddress;
02192                         NFComBuf->SetServosMode.data[combufDataIter] = 0;
02193                 }
02194         #endif
02195         // ####         Set Position
02196         #ifdef NF_BUFSZ_SetServosPosition
02197                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosPosition; combufDataIter++){
02198                         NFComBuf->SetServosPosition.addr[combufDataIter] = NF_BroadcastAddress;
02199                         NFComBuf->SetServosPosition.data[combufDataIter] = 0;
02200                 }
02201         #endif
02202         // ####         Set Speed
02203         #ifdef NF_BUFSZ_SetServosSpeed
02204                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosSpeed; combufDataIter++){
02205                         NFComBuf->SetServosSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02206                         NFComBuf->SetServosSpeed.data[combufDataIter] = 0;
02207                 }
02208         #endif
02209         // ####         Set Max Speed
02210         #ifdef NF_BUFSZ_SetServosMaxSpeed
02211                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMaxSpeed; combufDataIter++){
02212                         NFComBuf->SetServosMaxSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02213                         NFComBuf->SetServosMaxSpeed.data[combufDataIter] = 0;
02214                 }
02215         #endif
02216         // ####         Set Min Position
02217         #ifdef NF_BUFSZ_SetServosMinPosition
02218                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMinPosition; combufDataIter++){
02219                         NFComBuf->SetServosMinPosition.addr[combufDataIter] = NF_BroadcastAddress;
02220                         NFComBuf->SetServosMinPosition.data[combufDataIter] = 0;
02221                 }
02222         #endif
02223         // ####         Set MaxPosition
02224         #ifdef NF_BUFSZ_SetServosMaxPosition
02225                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMaxPosition; combufDataIter++){
02226                         NFComBuf->SetServosMaxPosition.addr[combufDataIter] = NF_BroadcastAddress;
02227                         NFComBuf->SetServosMaxPosition.data[combufDataIter] = 0;
02228                 }
02229         #endif
02230         // ####         Read Current
02231         #ifdef NF_BUFSZ_ReadServosCurrent
02232                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadServosCurrent; combufDataIter++){
02233                         NFComBuf->ReadServosCurrent.addr[combufDataIter] = NF_BroadcastAddress;
02234                         NFComBuf->ReadServosCurrent.data[combufDataIter] = 0;
02235                 }
02236         #endif
02237         // ####         Read Position
02238         #ifdef NF_BUFSZ_ReadServosPosition
02239                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadServosPosition; combufDataIter++){
02240                         NFComBuf->ReadServosPosition.addr[combufDataIter] = NF_BroadcastAddress;
02241                         NFComBuf->ReadServosPosition.data[combufDataIter] = 0;
02242                 }
02243         #endif
02244         // ####         Set Misc
02245         #ifdef NF_BUFSZ_SetServosMisc
02246                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMisc; combufDataIter++){
02247                         NFComBuf->SetServosMisc.addr[combufDataIter] = NF_BroadcastAddress;
02248                         NFComBuf->SetServosMisc.data[combufDataIter] = 0;
02249                 }
02250         #endif
02251         // ####         Read Status
02252         #ifdef NF_BUFSZ_ReadServosStatus
02253                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadServosStatus; combufDataIter++){
02254                         NFComBuf->ReadServosStatus.addr[combufDataIter] = NF_BroadcastAddress;
02255                         NFComBuf->ReadServosStatus.data[combufDataIter] = 0;
02256                 }
02257         #endif
02258         
02259         // ########     Digital IO       
02260         // ####         Read Inputs
02261         #ifdef NF_BUFSZ_ReadDigitalInputs
02262                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDigitalInputs; combufDataIter++){
02263                         NFComBuf->ReadDigitalInputs.addr[combufDataIter] = NF_BroadcastAddress;
02264                         NFComBuf->ReadDigitalInputs.data[combufDataIter] = 0;
02265                 }
02266         #endif
02267         // ####         Set Outputs
02268         #ifdef NF_BUFSZ_SetDigitalOutputs
02269                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDigitalOutputs; combufDataIter++){
02270                         NFComBuf->SetDigitalOutputs.addr[combufDataIter] = NF_BroadcastAddress;
02271                         NFComBuf->SetDigitalOutputs.data[combufDataIter] = 0;
02272                 }
02273         #endif
02274         
02275         // ########     Analog IO
02276         // ####         Read Inputs
02277         #ifdef NF_BUFSZ_ReadAnalogInputs
02278                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadAnalogInputs; combufDataIter++){
02279                         NFComBuf->ReadAnalogInputs.addr[combufDataIter] = NF_BroadcastAddress;
02280                         NFComBuf->ReadAnalogInputs.data[combufDataIter] = 0;
02281                 }
02282         #endif
02283         
02284         // ########     Sensors
02285         // ####         Read Distance
02286         #ifdef NF_BUFSZ_ReadDistance
02287                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDistance; combufDataIter++){
02288                         NFComBuf->ReadDistance.addr[combufDataIter] = NF_BroadcastAddress;
02289                         NFComBuf->ReadDistance.data[combufDataIter] = 0;
02290                 }
02291         #endif
02292         
02293         // ########     Display
02294         // ####         Set Mode
02295         #ifdef NF_BUFSZ_SetDisplayMode
02296                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDisplayMode; combufDataIter++){
02297                         NFComBuf->SetDisplayMode.addr[combufDataIter] = NF_BroadcastAddress;
02298                         NFComBuf->SetDisplayMode.data[combufDataIter] = 0;
02299                 }
02300         #endif
02301         // ####         Set Text
02302         #ifdef NF_BUFSZ_SetDisplayText
02303                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDisplayText; combufDataIter++){
02304                         NFComBuf->SetDisplayText.addr[combufDataIter] = NF_BroadcastAddress;
02305                         NFComBuf->SetDisplayText.data[combufDataIter] = 0;
02306                 }
02307         #endif
02308         // ####         Set Backlight
02309         #ifdef NF_BUFSZ_SetDisplayBacklight
02310                 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDisplayBacklight; combufDataIter++){
02311                         NFComBuf->SetDisplayBacklight.addr[combufDataIter] = NF_BroadcastAddress;
02312                         NFComBuf->SetDisplayBacklight.data[combufDataIter] = 0;
02313                 }
02314         #endif
02315 }
02316 
02317 void NFv2_CrcInit(void)
02318 {
02319     uint8_t  remainder;
02320         uint8_t b; //bit
02321         uint16_t dividend;
02322 
02323     /*
02324      * Compute the remainder of each possible dividend.
02325      */
02326     for (dividend = 0; dividend < 256; ++dividend)
02327     {
02328         /*
02329          * Start with the dividend followed by zeros.
02330          */
02331         remainder = dividend << (WIDTH - 8);
02332 
02333         /*
02334          * Perform modulo-2 division, a bit at a time.
02335          */
02336         for (b = 8; b > 0; --b)
02337         {
02338             /*
02339              * Try to divide the current data bit.
02340              */
02341             if (remainder & TOPBIT)
02342             {
02343                 remainder = (remainder << 1) ^ POLYNOMIAL;
02344             }
02345             else
02346             {
02347                 remainder = (remainder << 1);
02348             }
02349         }
02350 
02351         /*
02352          * Store the result into the table.
02353          */
02354         crcTable[dividend] = remainder;
02355     }
02356 
02357 }   /* crcInit() */
02358 
02359 uint8_t NFv2_CrcFast(const uint8_t message[], uint8_t nBytes)
02360 {
02361     uint8_t mydata;
02362     uint8_t byte;
02363     uint8_t remainder = 0;
02364 
02365 
02366     /*
02367      * Divide the message by the polynomial, a byte at a time.
02368      */
02369     for (byte = 0; byte < nBytes; ++byte)
02370     {
02371         mydata = message[byte] ^ (remainder >> (WIDTH - 8));
02372         remainder = crcTable[mydata] ^ (remainder << 8);
02373     }
02374 
02375     /*
02376      * The final remainder is the CRC.
02377      */
02378     return (remainder);
02379 
02380 }   /* crcFast() */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


elektron_base
Author(s): Konrad Banachowicz
autogenerated on Thu Nov 14 2013 11:57:04