SensorOnlyMode.c

Go to the documentation of this file.
00001 
00002 
00003 #include <avr/io.h>
00004 #include <avr/interrupt.h>
00005 #include "SensorOnlyMode.h"
00006 
00007 // convert a word into 2 bytes
00008 #define lowbyte(w)  ((byte) w)
00009 #define highbyte(w) ((byte) (w >> 8))
00010 
00011 
00012 void stm_init(Statemachine *sm, byte id, void (*callback)(Packet *pkt))
00013 {
00014   sm->state = STATE_IDLE;
00015   sm->id = id;
00016   sm->callback = callback;
00017 }
00018 
00019 void stm_process(Statemachine *sm,  byte b )
00020 {
00021   switch(sm->state)
00022   {
00023     case STATE_IDLE:
00024     {
00025       if( b == 0xFF )
00026         sm->state = STATE_1ST_FF_RCVD;
00027       break;
00028     }
00029       
00030     case STATE_1ST_FF_RCVD:
00031     {
00032       if( b == 0xFF )
00033         sm->state = STATE_2ND_FF_RCVD;
00034       else
00035         sm->state = STATE_IDLE;
00036       break;      
00037     }
00038       
00039     case STATE_2ND_FF_RCVD:
00040     {
00041       if( b != sm->id && b != BROADCAST_ID){
00042         sm->state = STATE_IDLE;
00043         break;
00044       }
00045       
00046       sm->state = STATE_ID_RCVD;
00047       sm->pkt.id  = b;
00048       sm->pkt.crc = b;
00049       break;
00050     }
00051       
00052     case STATE_ID_RCVD:
00053     {
00054       if( b > MAX_PACKELEN ){
00055         sm->state = STATE_IDLE;
00056         break;
00057       }
00058       
00059       sm->pkt.len = b;
00060       sm->pkt.crc += b;
00061       sm->state = STATE_LENGTH_RCVD;
00062       break;
00063     }
00064       
00065     case STATE_LENGTH_RCVD:
00066     {
00067       sm->pkt.paramPtr = 0;
00068       sm->pkt.cmd = b;
00069       sm->pkt.crc += b;
00070       sm->state = STATE_COMMAND_RCVD;
00071       break;
00072     }
00073       
00074     case STATE_COMMAND_RCVD:
00075     {
00076       if( sm->pkt.paramPtr+2 >= sm->pkt.len ){
00077         sm->state = STATE_PKT_COMPLETE;
00078       }
00079       else {
00080         sm->pkt.param[ sm->pkt.paramPtr++ ] = b;
00081         sm->pkt.crc += b;
00082         break;
00083       }
00084     }
00085       
00086     case STATE_PKT_COMPLETE:
00087     {
00088       sm->state = STATE_IDLE;    
00089       sm->pkt.crc += b;      
00090       sm->callback( &sm->pkt );
00091       break;
00092     }
00093     default:
00094       sm->state = STATE_IDLE;
00095   }
00096 }
00097 
00098 
00099 
00100 volatile Smartbuffer interruptbuff;
00101 static byte param[ MAX_PARAM ];
00102 static signed int wheel1, wheel2;
00103 
00104 byte TxPacket(byte bError, byte bParameterLength);
00105 void TxD(byte bTxdData);
00106 void callback( Packet *pkt );
00107 void initAtmega(void);
00108 
00109 signed char dist;
00110 
00111 // Reward Sensor object
00112 FeedbackSensor *fs = new FeedbackSensor();
00113 
00114 //int sensorMain (void) 
00115 int sensorMain (void) 
00116 {
00117 
00118   Statemachine stm;
00119   stm_init(&stm, SELF_ID, &callback);
00120   RESET_SENSOR;
00121   init(interruptbuff);
00122   initAtmega();
00123   
00124   RS485_RXD;
00125   for(;;){
00126      if( !isEmpty(interruptbuff) ){
00127      stm_process(&stm, pop(interruptbuff) );
00128     }  
00129   }
00130 }
00131 
00132 
00133 
00134 void initAtmega(void)
00135 {
00136   // init
00137      DDRB   = 0x00; // setup PortB 
00138      TCCR0  = 0x07; // inic count0
00139      TCNT0  = 0x00;
00140      TCNT1L = 0x00; // inic count1
00141      TCNT1H = 0x00;
00142      TCCR1A = 0x00;
00143      TCCR1B = 0x07;                
00144   
00145   
00146      // UART
00147      UCSRA = (1<<U2X);
00148      UBRRL = XTAL / (BAUD*8L) - 1;               // speed
00149      UBRRH = 0;                                  // load the upper bits
00150      UCSRB = (1<<TXEN) | (1<<RXEN) | (1<<RXCIE);  // enable transmit and receive.enable TX and RX   
00151   
00152   
00153      // wheel
00154      // Data A is on PB0
00155      // Data B is on PD2 (INT0)
00156      DDRD   = 0x00;
00157      DDRD  |= (1<<PD1) | (1<<PD4) | (1<<PD5) | (1<<PD6) | (1<<PD7);
00158      DDRB  &= ~(1<<PB0);
00159      DDRB  &= ~(1<<PB1);
00160      MCUCR |= (1<<ISC10) | (1<<ISC00);
00161      
00162      // interupt enable for PD2, PD3
00163      GICR |= (1<<INT0) | (1<<INT1);
00164      GIFR |= (1<<INTF0) | (1<<INTF1);
00165      
00166      sei();  //Enable Interrupt   
00167      RS485_RXD;
00168 }
00169 
00170 /* This functions transmit a packet */
00171 byte TxPacket(byte bError, byte bParameterLength) 
00172 {
00173   byte bCount,bCheckSum,bPacketLength;
00174   byte txbuff[ MAX_PACKELEN ];
00175   
00176   txbuff[0] = 0xff;
00177   txbuff[1] = 0xff;
00178   txbuff[2] = SELF_ID;
00179   txbuff[3] = bParameterLength+2; // Length(Paramter,Instruction,Checksum)
00180   txbuff[4] = bError;
00181   
00182   for(bCount = 0; bCount < bParameterLength; bCount++)
00183     txbuff[bCount+5] = param[bCount];
00184   
00185   bCheckSum = 0;
00186   bPacketLength = bParameterLength+4+2;
00187   
00188   for(bCount = 2; bCount < bPacketLength-1; bCount++) // except 0xff,checksum
00189     bCheckSum += txbuff[bCount];
00190   
00191   txbuff[bCount] = ~bCheckSum; // Writing Checksum with Bit Inversion
00192   
00193   RS485_TXD;
00194   for(bCount = 0; bCount < bPacketLength; bCount++)
00195   {
00196     SET_TXD_FINISH;
00197     TxD(txbuff[bCount]);
00198   }
00199   while(!CHECK_TXD_FINISH); // Wait until TXD Shift register empty
00200   RS485_RXD;
00201   
00202   return(bPacketLength);
00203 }
00204 
00205 void TxD(byte bTxdData)
00206 {
00207   while(!TXD_READY);
00208   TXD_DATA = bTxdData;  
00209 }
00210 
00211 
00212 void callback( Packet *pkt )
00213 {  
00214   // checksum test
00215   if( pkt->crc != 0xff ){
00216     TxPacket(E_CHECKSUM,0);
00217     return;
00218   }   
00219   
00220   // ping
00221   if( pkt->cmd == INST_PING ){
00222     TxPacket(NO_ERROR,0);
00223     return;
00224   }  
00225   
00226   // read
00227   if( pkt->cmd == INST_READ && pkt->param[0] == P_DIST ){
00228     
00229     // check the limits of the size
00230     if( wheel1 > WORD_MAX_SIZE ) wheel1 = WORD_MAX_SIZE;
00231     if( wheel1 < WORD_MIN_SIZE ) wheel1 = WORD_MIN_SIZE;
00232     if( wheel2 > WORD_MAX_SIZE ) wheel2 = WORD_MAX_SIZE;
00233     if( wheel2 < WORD_MIN_SIZE ) wheel2 = WORD_MIN_SIZE;
00234     
00235     // send a signed word for both wheels
00236     param[0] = lowbyte(wheel1);
00237     param[1] = highbyte_s(wheel1);
00238     param[2] = lowbyte(wheel2);
00239     param[3] = highbyte_s(wheel2);
00240     TxPacket(NO_ERROR,4);
00241     return;
00242   }
00243   
00244   // reset
00245   if( pkt->cmd == INST_RESET ){
00246     TxPacket(NO_ERROR,0);
00247     RESET_WHEEL;
00248     return;
00249   }                    
00250 }
00251 

Generated on Wed Nov 25 11:27:41 2009 for Crawling Robot Microcontroller Software by  doxygen 1.5.5