encoder-ax12-mode.c

Go to the documentation of this file.
00001 #include "global.h"
00002 #include <avr/interrupt.h>
00003 #include <avr/signal.h>
00004 #include <avr/io.h>
00005 
00006 #define MAX_PARAM       8
00007 #define MAX_PACKELEN    MAX_PARAM+4
00008 #define BROADCAST_ID    0xFE
00009 #define SELF_ID         200
00010 
00011 //--- ATMEGA ---
00012 #define ENABLE_BIT_DEFINITIONS 
00013 #define cbi(REG,BITNUM) REG &= ~(_BV(BITNUM)) // clear bit
00014 #define sbi(REG,BITNUM) REG |= _BV(BITNUM)    // set bit
00015 
00016 #define WORD_MAX_SIZE  32767
00017 #define WORD_MIN_SIZE -32767
00018 
00019 // convert a word into 2 bytes
00020 #define lowbyte(w)  ((byte) w)
00021 #define highbyte(w) ((byte) (w >> 8))
00022 
00023 // convert a word into 2 bytes with sign
00024 #define highbyte_s(w) (w&0x80000000?(byte)(w >> 8)|(0x8000):(byte)(w >> 8))
00025 
00026 //--- ERRORCODES ---
00027 #define NO_ERROR        0
00028 #define E_INPUT_VOLTAGE 1
00029 #define E_ANGLE_LIMIT   2
00030 #define E_OVERHEATING   4
00031 #define E_RANGE         8
00032 #define E_CHECKSUM      16
00033 #define E_OVERLOAD      32
00034 #define E_INSTRUCTION   64
00035 
00036 //---INSTRUCTIONS ---
00037 #define INST_PING           0x01
00038 #define INST_READ           0x02
00039 #define INST_WRITE          0x03
00040 #define INST_REG_WRITE      0x04
00041 #define INST_ACTION         0x05
00042 #define INST_RESET          0x06
00043 #define INST_DIGITAL_RESET  0x07
00044 #define INST_SYSTEM_READ    0x0C
00045 #define INST_SYSTEM_WRITE   0x0D
00046 #define INST_SYNC_WRITE     0x83
00047 #define INST_SYNC_REG_WRITE 0x84
00048 
00049 //--FLASH--
00050 #define P_DIST 1
00051 
00052 typedef unsigned char byte;
00053 
00054 /***********************************************************************
00055 * PACKET
00056 ***********************************************************************/
00057 
00058 typedef struct
00059 {
00060      byte id;
00061      byte len;
00062      byte cmd;
00063      byte paramPtr;
00064      byte param[ MAX_PARAM ];
00065      byte crc; 
00066 } BioloidPacket;
00067 
00068 /***********************************************************************
00069 * STATEMACHINE
00070 ***********************************************************************/
00071 
00072 #define STATE_IDLE          0   
00073 #define STATE_1ST_FF_RCVD   1  
00074 #define STATE_2ND_FF_RCVD   2   
00075 #define STATE_ID_RCVD       3  
00076 #define STATE_LENGTH_RCVD   4   
00077 #define STATE_COMMAND_RCVD  5   
00078 #define STATE_PKT_COMPLETE  6   
00079 
00080 typedef struct 
00081 {
00082      byte id;
00083      byte state;
00084      BioloidPacket pkt;
00085      void (*callback)(BioloidPacket *pkt);
00086 } Statemachine;
00087 
00088 void stm_init(Statemachine *sm, byte id, void (*callback)(BioloidPacket *pkt))
00089 {
00090      sm->state = STATE_IDLE;
00091      sm->id = id;
00092      sm->callback = callback;
00093 }
00094 
00095 void stm_process(Statemachine *sm,  byte b )
00096 {
00097      switch(sm->state)
00098      {
00099           case STATE_IDLE:
00100           {
00101                if( b == 0xFF )
00102                     sm->state = STATE_1ST_FF_RCVD;
00103                break;
00104           }
00105       
00106           case STATE_1ST_FF_RCVD:
00107           {
00108                if( b == 0xFF )
00109                     sm->state = STATE_2ND_FF_RCVD;
00110                else
00111                     sm->state = STATE_IDLE;
00112                break;      
00113           }
00114       
00115           case STATE_2ND_FF_RCVD:
00116           {
00117                if( b != sm->id && b != BROADCAST_ID){
00118                     sm->state = STATE_IDLE;
00119                     break;
00120                }
00121       
00122                sm->state = STATE_ID_RCVD;
00123                sm->pkt.id  = b;
00124                sm->pkt.crc = b;
00125                break;
00126           }
00127       
00128           case STATE_ID_RCVD:
00129           {
00130                if( b > MAX_PACKELEN ){
00131                     sm->state = STATE_IDLE;
00132                     break;
00133                }
00134       
00135                sm->pkt.len = b;
00136                sm->pkt.crc += b;
00137                sm->state = STATE_LENGTH_RCVD;
00138                break;
00139           }
00140       
00141           case STATE_LENGTH_RCVD:
00142           {
00143                sm->pkt.paramPtr = 0;
00144                sm->pkt.cmd = b;
00145                sm->pkt.crc += b;
00146                sm->state = STATE_COMMAND_RCVD;
00147                break;
00148           }
00149       
00150           case STATE_COMMAND_RCVD:
00151           {
00152                if( sm->pkt.paramPtr+2 >= sm->pkt.len ){
00153                     sm->state = STATE_PKT_COMPLETE;
00154                }
00155                else {
00156                     sm->pkt.param[ sm->pkt.paramPtr++ ] = b;
00157                     sm->pkt.crc += b;
00158                     break;
00159                }
00160           }
00161       
00162           case STATE_PKT_COMPLETE:
00163           {
00164                sm->state = STATE_IDLE;    
00165                sm->pkt.crc += b;      
00166                sm->callback( &sm->pkt );
00167                break;
00168           }
00169           default:
00170                sm->state = STATE_IDLE;
00171      }
00172 }
00173 
00174 
00175 /***********************************************************************
00176 * SMARTBUFFER
00177 ***********************************************************************/
00178 
00179 #define init(sb)        (sb.readPtr = sb.writePtr = 0)
00180 #define isEmpty(sb)     (sb.writePtr == sb.readPtr)
00181 #define push(sb, value) (sb.buffer[ sb.writePtr++ ] = value)
00182 #define pop(sb)         (sb.buffer[ sb.readPtr++ ])
00183 #define len(sb) ( sb.readPtr > sb.writePtr ? (sb.writePtr - sb.readPtr + 256) : (sb.writePtr - sb.readPtr) )
00184 
00185 typedef struct {
00186      volatile byte  readPtr;
00187      volatile byte  writePtr;
00188      byte     buffer[ 256 ];
00189 } Smartbuffer;
00190 
00191 
00192 
00193 /***********************************************************************
00194 * MAIN
00195 ***********************************************************************/
00196 
00197 #define RS485_TXD sbi(PORTD, PD5), cbi(PORTD, PD6), cbi(UCSRB,RXCIE)
00198 #define RS485_RXD sbi(PORTD, PD6), cbi(PORTD, PD5), sbi(UCSRB,RXCIE)
00199 
00200 #define SET_TXD_FINISH   sbi(UCSRA,TXC)
00201 #define RESET_TXD_FINISH cbi(UCSRA,TXC)
00202 #define CHECK_TXD_FINISH bit_is_set(UCSRA,TXC)
00203 
00204 #define TXD_READY             bit_is_set(UCSRA,UDRE)
00205 #define TXD_DATA              UDR
00206 #define RXD_DATA              UDR
00207 
00208 #define READDIST_X  TCNT0
00209 #define READDIST_Y  TCNT1
00210 #define RESET_MOUSE TCNT0 = TCNT1 = 0
00211 #define RESET_WHEEL wheel1 = wheel2 = 0
00212 #define RESET_SENSOR RESET_WHEEL
00213 
00214 //#define XTAL 11059000
00215 //#define BAUD 9600
00216 
00217 #define XTAL 16000000
00218 #define BAUD 1000000
00219 
00220 static Smartbuffer interruptbuff;
00221 static byte param[ MAX_PARAM ];
00222 signed int wheel1;
00223 signed int wheel2;
00224 
00225 byte TxPacket(byte bError, byte bParameterLength);
00226 void TxD(byte bTxdData);
00227 void callback( BioloidPacket *pkt );
00228 void initAtmega(void);
00229 
00230 signed char dist;
00231 
00232 // int main (void) 
00233 // {
00234 //   Statemachine stm;
00235 //   stm_init(&stm, SELF_ID, &callback);
00236 //   RESET_SENSOR;
00237 //   init(interruptbuff);
00238 //   initAtmega();
00239 //   
00240 //   RS485_RXD;
00241 //   for(;;){
00242 //        if( !isEmpty(interruptbuff) ){
00243 //             stm_process(&stm, pop(interruptbuff) );
00244 //        }  
00245 //   }
00246 // }
00247 
00248 
00249 
00250 void initAtmega(void)
00251 {
00252   // init
00253      DDRB   = 0x00; // setup PortB 
00254      TCCR0  = 0x07; // inic count0
00255      TCNT0  = 0x00;
00256      TCNT1L = 0x00; // inic count1
00257      TCNT1H = 0x00;
00258      TCCR1A = 0x00;
00259      TCCR1B = 0x07;                
00260   
00261   
00262      // UART
00263      UCSRA = (1<<U2X);
00264      UBRRL = XTAL / (BAUD*8L) - 1;               // speed
00265      UBRRH = 0;                                  // load the upper bits
00266      UCSRB = (1<<TXEN) | (1<<RXEN) | (1<<RXCIE);  // enable transmit and receive.enable TX and RX   
00267   
00268   
00269   // wheel
00270   // Data A is on PB0
00271   // Data B is on PD2 (INT0)
00272      DDRD   = 0x00;
00273      DDRD  |= (1<<PD1) | (1<<PD4) | (1<<PD5) | (1<<PD6) | (1<<PD7);
00274      DDRB  &= ~(1<<PB0);
00275      DDRB  &= ~(1<<PB1);
00276      MCUCR |= (1<<ISC10) | (1<<ISC00);
00277   
00278   // interupt enable for PD2, PD3
00279      GICR |= (1<<INT0) | (1<<INT1);
00280      GIFR |= (1<<INTF0) | (1<<INTF1);
00281   
00282      sei();  //Enable Interrupt   
00283      RS485_RXD;
00284 }
00285 
00286 SIGNAL(SIG_INTERRUPT0)
00287 { 
00288      register unsigned char pd = PIND;
00289      register unsigned char pb = PINB;
00290   
00291   // if the signals are different is was
00292   // a forwad movement - else a backward movement
00293      if(((pd & (1<<PIND2))>=1  &&  (pb & (1<<PINB0))==0) ||
00294         ((pd & (1<<PIND2))==0  &&  (pb & (1<<PINB0))>=1) ){
00295           wheel1++;
00296        } 
00297        else{   
00298             wheel1--;
00299        }
00300 }
00301 
00302 SIGNAL(SIG_INTERRUPT1)
00303 { 
00304      register unsigned char pd = PIND;
00305      register unsigned char pb = PINB;
00306   
00307   // if the signals are different is was
00308   // a forwad movement - else a backward movement
00309      if(
00310         ((pd & (1<<PIND3))>=1  &&  (pb & (1<<PINB1))==0) ||
00311             ((pd & (1<<PIND3))==0  &&  (pb & (1<<PINB1))>=1) 
00312        ){
00313           wheel2++;
00314        } 
00315        else{   
00316             wheel2--;
00317        }
00318 }
00319 
00320 /* Interrupt Signal Handler */
00321 //ISR(USART_RXC_vect)
00322 SIGNAL (SIG_UART_RECV)
00323 {
00324      push(interruptbuff, RXD_DATA);
00325 }    
00326 
00327 /* This functions transmit a packet */
00328 byte TxPacket(byte bError, byte bParameterLength) 
00329 {
00330      byte bCount,bCheckSum,bPacketLength;
00331      byte txbuff[ MAX_PACKELEN ];
00332   
00333      txbuff[0] = 0xff;
00334      txbuff[1] = 0xff;
00335      txbuff[2] = SELF_ID;
00336      txbuff[3] = bParameterLength+2; // Length(Paramter,Instruction,Checksum)
00337      txbuff[4] = bError;
00338   
00339      for(bCount = 0; bCount < bParameterLength; bCount++)
00340           txbuff[bCount+5] = param[bCount];
00341   
00342      bCheckSum = 0;
00343      bPacketLength = bParameterLength+4+2;
00344   
00345      for(bCount = 2; bCount < bPacketLength-1; bCount++) // except 0xff,checksum
00346           bCheckSum += txbuff[bCount];
00347   
00348      txbuff[bCount] = ~bCheckSum; // Writing Checksum with Bit Inversion
00349   
00350      RS485_TXD;
00351      for(bCount = 0; bCount < bPacketLength; bCount++)
00352      {
00353           SET_TXD_FINISH;
00354           TxD(txbuff[bCount]);
00355      }
00356      while(!CHECK_TXD_FINISH); // Wait until TXD Shift register empty
00357      RS485_RXD;
00358   
00359      return(bPacketLength);
00360 }
00361 
00362 void TxD(byte bTxdData)
00363 {
00364      while(!TXD_READY);
00365      TXD_DATA = bTxdData;  
00366 }
00367 
00368 
00369 void callback( BioloidPacket *pkt )
00370 {  
00371   // checksum test
00372      if( pkt->crc != 0xff ){
00373           TxPacket(E_CHECKSUM,0);
00374           return;
00375      }   
00376   
00377   // ping
00378      if( pkt->cmd == INST_PING ){
00379           TxPacket(NO_ERROR,0);
00380           return;
00381      }  
00382   
00383   // read
00384      if( pkt->cmd == INST_READ && pkt->param[0] == P_DIST ){
00385     
00386     // check the limits of the size
00387           if( wheel1 > WORD_MAX_SIZE ) wheel1 = WORD_MAX_SIZE;
00388           if( wheel1 < WORD_MIN_SIZE ) wheel1 = WORD_MIN_SIZE;
00389           if( wheel2 > WORD_MAX_SIZE ) wheel2 = WORD_MAX_SIZE;
00390           if( wheel2 < WORD_MIN_SIZE ) wheel2 = WORD_MIN_SIZE;
00391     
00392     // send a signed word for both wheels
00393           param[0] = lowbyte(wheel1);
00394           param[1] = highbyte_s(wheel1);
00395           param[2] = lowbyte(wheel2);
00396           param[3] = highbyte_s(wheel2);
00397           TxPacket(NO_ERROR,4);
00398           return;
00399      }
00400   
00401   // reset
00402      if( pkt->cmd == INST_RESET ){
00403           TxPacket(NO_ERROR,0);
00404           RESET_WHEEL;
00405           return;
00406      }
00407 }
00408 
00409 
Generated on Fri Oct 8 17:10:07 2010 for Crawling Robot Microcontroller Software by  doxygen 1.6.3