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
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
00020 #define lowbyte(w) ((byte) w)
00021 #define highbyte(w) ((byte) (w >> 8))
00022
00023
00024 #define highbyte_s(w) (w&0x80000000?(byte)(w >> 8)|(0x8000):(byte)(w >> 8))
00025
00026
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
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
00050 #define P_DIST 1
00051
00052 typedef unsigned char byte;
00053
00054
00055
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
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
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
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
00215
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
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250 void initAtmega(void)
00251 {
00252
00253 DDRB = 0x00;
00254 TCCR0 = 0x07;
00255 TCNT0 = 0x00;
00256 TCNT1L = 0x00;
00257 TCNT1H = 0x00;
00258 TCCR1A = 0x00;
00259 TCCR1B = 0x07;
00260
00261
00262
00263 UCSRA = (1<<U2X);
00264 UBRRL = XTAL / (BAUD*8L) - 1;
00265 UBRRH = 0;
00266 UCSRB = (1<<TXEN) | (1<<RXEN) | (1<<RXCIE);
00267
00268
00269
00270
00271
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
00279 GICR |= (1<<INT0) | (1<<INT1);
00280 GIFR |= (1<<INTF0) | (1<<INTF1);
00281
00282 sei();
00283 RS485_RXD;
00284 }
00285
00286 SIGNAL(SIG_INTERRUPT0)
00287 {
00288 register unsigned char pd = PIND;
00289 register unsigned char pb = PINB;
00290
00291
00292
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
00308
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
00321
00322 SIGNAL (SIG_UART_RECV)
00323 {
00324 push(interruptbuff, RXD_DATA);
00325 }
00326
00327
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;
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++)
00346 bCheckSum += txbuff[bCount];
00347
00348 txbuff[bCount] = ~bCheckSum;
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);
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
00372 if( pkt->crc != 0xff ){
00373 TxPacket(E_CHECKSUM,0);
00374 return;
00375 }
00376
00377
00378 if( pkt->cmd == INST_PING ){
00379 TxPacket(NO_ERROR,0);
00380 return;
00381 }
00382
00383
00384 if( pkt->cmd == INST_READ && pkt->param[0] == P_DIST ){
00385
00386
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
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
00402 if( pkt->cmd == INST_RESET ){
00403 TxPacket(NO_ERROR,0);
00404 RESET_WHEEL;
00405 return;
00406 }
00407 }
00408
00409