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
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
00112 FeedbackSensor *fs = new FeedbackSensor();
00113
00114
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
00137 DDRB = 0x00;
00138 TCCR0 = 0x07;
00139 TCNT0 = 0x00;
00140 TCNT1L = 0x00;
00141 TCNT1H = 0x00;
00142 TCCR1A = 0x00;
00143 TCCR1B = 0x07;
00144
00145
00146
00147 UCSRA = (1<<U2X);
00148 UBRRL = XTAL / (BAUD*8L) - 1;
00149 UBRRH = 0;
00150 UCSRB = (1<<TXEN) | (1<<RXEN) | (1<<RXCIE);
00151
00152
00153
00154
00155
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
00163 GICR |= (1<<INT0) | (1<<INT1);
00164 GIFR |= (1<<INTF0) | (1<<INTF1);
00165
00166 sei();
00167 RS485_RXD;
00168 }
00169
00170
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;
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++)
00189 bCheckSum += txbuff[bCount];
00190
00191 txbuff[bCount] = ~bCheckSum;
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);
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
00215 if( pkt->crc != 0xff ){
00216 TxPacket(E_CHECKSUM,0);
00217 return;
00218 }
00219
00220
00221 if( pkt->cmd == INST_PING ){
00222 TxPacket(NO_ERROR,0);
00223 return;
00224 }
00225
00226
00227 if( pkt->cmd == INST_READ && pkt->param[0] == P_DIST ){
00228
00229
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
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
00245 if( pkt->cmd == INST_RESET ){
00246 TxPacket(NO_ERROR,0);
00247 RESET_WHEEL;
00248 return;
00249 }
00250 }
00251