#include "global.h"
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/io.h>
Go to the source code of this file.
Data Structures | |
struct | BioloidPacket |
struct | Smartbuffer |
struct | Statemachine |
Defines | |
#define | BAUD 1000000 |
#define | BROADCAST_ID 0xFE |
#define | cbi(REG, BITNUM) REG &= ~(_BV(BITNUM)) |
#define | CHECK_TXD_FINISH bit_is_set(UCSRA,TXC) |
#define | E_ANGLE_LIMIT 2 |
#define | E_CHECKSUM 16 |
#define | E_INPUT_VOLTAGE 1 |
#define | E_INSTRUCTION 64 |
#define | E_OVERHEATING 4 |
#define | E_OVERLOAD 32 |
#define | E_RANGE 8 |
#define | ENABLE_BIT_DEFINITIONS |
#define | highbyte(w) ((byte) (w >> 8)) |
#define | highbyte_s(w) (w&0x80000000?(byte)(w >> 8)|(0x8000):(byte)(w >> 8)) |
#define | init(sb) (sb.readPtr = sb.writePtr = 0) |
#define | INST_ACTION 0x05 |
#define | INST_DIGITAL_RESET 0x07 |
#define | INST_PING 0x01 |
#define | INST_READ 0x02 |
#define | INST_REG_WRITE 0x04 |
#define | INST_RESET 0x06 |
#define | INST_SYNC_REG_WRITE 0x84 |
#define | INST_SYNC_WRITE 0x83 |
#define | INST_SYSTEM_READ 0x0C |
#define | INST_SYSTEM_WRITE 0x0D |
#define | INST_WRITE 0x03 |
#define | isEmpty(sb) (sb.writePtr == sb.readPtr) |
#define | len(sb) ( sb.readPtr > sb.writePtr ? (sb.writePtr - sb.readPtr + 256) : (sb.writePtr - sb.readPtr) ) |
#define | lowbyte(w) ((byte) w) |
#define | MAX_PACKELEN MAX_PARAM+4 |
#define | MAX_PARAM 8 |
#define | NO_ERROR 0 |
#define | P_DIST 1 |
#define | pop(sb) (sb.buffer[ sb.readPtr++ ]) |
#define | push(sb, value) (sb.buffer[ sb.writePtr++ ] = value) |
#define | READDIST_X TCNT0 |
#define | READDIST_Y TCNT1 |
#define | RESET_MOUSE TCNT0 = TCNT1 = 0 |
#define | RESET_SENSOR RESET_WHEEL |
#define | RESET_TXD_FINISH cbi(UCSRA,TXC) |
#define | RESET_WHEEL wheel1 = wheel2 = 0 |
#define | RS485_RXD sbi(PORTD, PD6), cbi(PORTD, PD5), sbi(UCSRB,RXCIE) |
#define | RS485_TXD sbi(PORTD, PD5), cbi(PORTD, PD6), cbi(UCSRB,RXCIE) |
#define | RXD_DATA UDR |
#define | sbi(REG, BITNUM) REG |= _BV(BITNUM) |
#define | SELF_ID 200 |
#define | SET_TXD_FINISH sbi(UCSRA,TXC) |
#define | STATE_1ST_FF_RCVD 1 |
#define | STATE_2ND_FF_RCVD 2 |
#define | STATE_COMMAND_RCVD 5 |
#define | STATE_ID_RCVD 3 |
#define | STATE_IDLE 0 |
#define | STATE_LENGTH_RCVD 4 |
#define | STATE_PKT_COMPLETE 6 |
#define | TXD_DATA UDR |
#define | TXD_READY bit_is_set(UCSRA,UDRE) |
#define | WORD_MAX_SIZE 32767 |
#define | WORD_MIN_SIZE -32767 |
#define | XTAL 16000000 |
Typedefs | |
typedef unsigned char | byte |
Functions | |
void | callback (BioloidPacket *pkt) |
void | initAtmega (void) |
SIGNAL (SIG_UART_RECV) | |
SIGNAL (SIG_INTERRUPT1) | |
SIGNAL (SIG_INTERRUPT0) | |
void | stm_init (Statemachine *sm, byte id, void(*callback)(BioloidPacket *pkt)) |
void | stm_process (Statemachine *sm, byte b) |
void | TxD (byte bTxdData) |
byte | TxPacket (byte bError, byte bParameterLength) |
Variables | |
signed char | dist |
static Smartbuffer | interruptbuff |
static byte | param [8] |
signed int | wheel1 |
signed int | wheel2 |
#define BAUD 1000000 |
Definition at line 218 of file encoder-ax12-mode.c.
Referenced by initAtmega().
#define BROADCAST_ID 0xFE |
Definition at line 8 of file encoder-ax12-mode.c.
Referenced by stm_process().
#define cbi | ( | REG, | |||
BITNUM | ) | REG &= ~(_BV(BITNUM)) |
Definition at line 13 of file encoder-ax12-mode.c.
#define CHECK_TXD_FINISH bit_is_set(UCSRA,TXC) |
Definition at line 202 of file encoder-ax12-mode.c.
Referenced by TxPacket().
#define E_ANGLE_LIMIT 2 |
Definition at line 29 of file encoder-ax12-mode.c.
#define E_CHECKSUM 16 |
Definition at line 32 of file encoder-ax12-mode.c.
Referenced by callback().
#define E_INPUT_VOLTAGE 1 |
Definition at line 28 of file encoder-ax12-mode.c.
#define E_INSTRUCTION 64 |
Definition at line 34 of file encoder-ax12-mode.c.
#define E_OVERHEATING 4 |
Definition at line 30 of file encoder-ax12-mode.c.
#define E_OVERLOAD 32 |
Definition at line 33 of file encoder-ax12-mode.c.
#define E_RANGE 8 |
Definition at line 31 of file encoder-ax12-mode.c.
#define ENABLE_BIT_DEFINITIONS |
Definition at line 12 of file encoder-ax12-mode.c.
#define highbyte | ( | w | ) | ((byte) (w >> 8)) |
Definition at line 21 of file encoder-ax12-mode.c.
#define highbyte_s | ( | w | ) | (w&0x80000000?(byte)(w >> 8)|(0x8000):(byte)(w >> 8)) |
Definition at line 24 of file encoder-ax12-mode.c.
Referenced by callback().
#define init | ( | sb | ) | (sb.readPtr = sb.writePtr = 0) |
Definition at line 179 of file encoder-ax12-mode.c.
Referenced by sensorMain().
#define INST_ACTION 0x05 |
Definition at line 41 of file encoder-ax12-mode.c.
#define INST_DIGITAL_RESET 0x07 |
Definition at line 43 of file encoder-ax12-mode.c.
#define INST_PING 0x01 |
Definition at line 37 of file encoder-ax12-mode.c.
Referenced by callback().
#define INST_READ 0x02 |
Definition at line 38 of file encoder-ax12-mode.c.
Referenced by callback().
#define INST_REG_WRITE 0x04 |
Definition at line 40 of file encoder-ax12-mode.c.
#define INST_RESET 0x06 |
Definition at line 42 of file encoder-ax12-mode.c.
Referenced by callback().
#define INST_SYNC_REG_WRITE 0x84 |
Definition at line 47 of file encoder-ax12-mode.c.
#define INST_SYNC_WRITE 0x83 |
Definition at line 46 of file encoder-ax12-mode.c.
#define INST_SYSTEM_READ 0x0C |
Definition at line 44 of file encoder-ax12-mode.c.
#define INST_SYSTEM_WRITE 0x0D |
Definition at line 45 of file encoder-ax12-mode.c.
#define INST_WRITE 0x03 |
Definition at line 39 of file encoder-ax12-mode.c.
#define isEmpty | ( | sb | ) | (sb.writePtr == sb.readPtr) |
Definition at line 180 of file encoder-ax12-mode.c.
Referenced by sensorMain().
#define len | ( | sb | ) | ( sb.readPtr > sb.writePtr ? (sb.writePtr - sb.readPtr + 256) : (sb.writePtr - sb.readPtr) ) |
Definition at line 183 of file encoder-ax12-mode.c.
#define lowbyte | ( | w | ) | ((byte) w) |
Definition at line 20 of file encoder-ax12-mode.c.
#define MAX_PACKELEN MAX_PARAM+4 |
Definition at line 7 of file encoder-ax12-mode.c.
Referenced by stm_process(), and TxPacket().
#define MAX_PARAM 8 |
Definition at line 6 of file encoder-ax12-mode.c.
#define NO_ERROR 0 |
Definition at line 27 of file encoder-ax12-mode.c.
Referenced by callback().
#define P_DIST 1 |
Definition at line 50 of file encoder-ax12-mode.c.
Referenced by callback().
#define pop | ( | sb | ) | (sb.buffer[ sb.readPtr++ ]) |
Definition at line 182 of file encoder-ax12-mode.c.
Referenced by sensorMain().
#define push | ( | sb, | |||
value | ) | (sb.buffer[ sb.writePtr++ ] = value) |
Definition at line 181 of file encoder-ax12-mode.c.
Referenced by SIGNAL().
#define READDIST_X TCNT0 |
Definition at line 208 of file encoder-ax12-mode.c.
#define READDIST_Y TCNT1 |
Definition at line 209 of file encoder-ax12-mode.c.
#define RESET_MOUSE TCNT0 = TCNT1 = 0 |
Definition at line 210 of file encoder-ax12-mode.c.
#define RESET_SENSOR RESET_WHEEL |
Definition at line 212 of file encoder-ax12-mode.c.
Referenced by sensorMain().
#define RESET_TXD_FINISH cbi(UCSRA,TXC) |
Definition at line 201 of file encoder-ax12-mode.c.
Definition at line 211 of file encoder-ax12-mode.c.
Referenced by callback().
#define RS485_RXD sbi(PORTD, PD6), cbi(PORTD, PD5), sbi(UCSRB,RXCIE) |
Definition at line 198 of file encoder-ax12-mode.c.
Referenced by initAtmega(), sensorMain(), and TxPacket().
#define RS485_TXD sbi(PORTD, PD5), cbi(PORTD, PD6), cbi(UCSRB,RXCIE) |
Definition at line 197 of file encoder-ax12-mode.c.
Referenced by TxPacket().
#define RXD_DATA UDR |
Definition at line 206 of file encoder-ax12-mode.c.
Referenced by SIGNAL().
#define sbi | ( | REG, | |||
BITNUM | ) | REG |= _BV(BITNUM) |
Definition at line 14 of file encoder-ax12-mode.c.
#define SELF_ID 200 |
Definition at line 9 of file encoder-ax12-mode.c.
Referenced by sensorMain(), and TxPacket().
#define SET_TXD_FINISH sbi(UCSRA,TXC) |
Definition at line 200 of file encoder-ax12-mode.c.
Referenced by TxPacket().
#define STATE_1ST_FF_RCVD 1 |
Definition at line 73 of file encoder-ax12-mode.c.
Referenced by stm_process().
#define STATE_2ND_FF_RCVD 2 |
Definition at line 74 of file encoder-ax12-mode.c.
Referenced by stm_process().
#define STATE_COMMAND_RCVD 5 |
Definition at line 77 of file encoder-ax12-mode.c.
Referenced by stm_process().
#define STATE_ID_RCVD 3 |
Definition at line 75 of file encoder-ax12-mode.c.
Referenced by stm_process().
#define STATE_IDLE 0 |
Definition at line 72 of file encoder-ax12-mode.c.
Referenced by stm_init(), and stm_process().
#define STATE_LENGTH_RCVD 4 |
Definition at line 76 of file encoder-ax12-mode.c.
Referenced by stm_process().
#define STATE_PKT_COMPLETE 6 |
Definition at line 78 of file encoder-ax12-mode.c.
Referenced by stm_process().
#define TXD_DATA UDR |
Definition at line 205 of file encoder-ax12-mode.c.
Referenced by TxD().
#define TXD_READY bit_is_set(UCSRA,UDRE) |
Definition at line 204 of file encoder-ax12-mode.c.
Referenced by TxD().
#define WORD_MAX_SIZE 32767 |
Definition at line 16 of file encoder-ax12-mode.c.
Referenced by callback().
#define WORD_MIN_SIZE -32767 |
Definition at line 17 of file encoder-ax12-mode.c.
Referenced by callback().
#define XTAL 16000000 |
Definition at line 217 of file encoder-ax12-mode.c.
Referenced by initAtmega().
typedef unsigned char byte |
Definition at line 52 of file encoder-ax12-mode.c.
void callback | ( | BioloidPacket * | pkt | ) |
Definition at line 369 of file encoder-ax12-mode.c.
References BioloidPacket::cmd, BioloidPacket::crc, E_CHECKSUM, highbyte_s, INST_PING, INST_READ, INST_RESET, lowbyte, NO_ERROR, P_DIST, BioloidPacket::param, RESET_WHEEL, TxPacket(), WORD_MAX_SIZE, and WORD_MIN_SIZE.
Referenced by sensorMain(), and stm_init().
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 }
void initAtmega | ( | void | ) |
Definition at line 250 of file encoder-ax12-mode.c.
References BAUD, RS485_RXD, and XTAL.
Referenced by sensorMain().
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 }
SIGNAL | ( | SIG_UART_RECV | ) |
Definition at line 322 of file encoder-ax12-mode.c.
References push, and RXD_DATA.
00323 { 00324 push(interruptbuff, RXD_DATA); 00325 }
SIGNAL | ( | SIG_INTERRUPT1 | ) |
Definition at line 302 of file encoder-ax12-mode.c.
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 }
SIGNAL | ( | SIG_INTERRUPT0 | ) |
Definition at line 286 of file encoder-ax12-mode.c.
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 }
void stm_init | ( | Statemachine * | sm, | |
byte | id, | |||
void(*)(BioloidPacket *pkt) | callback | |||
) |
Definition at line 88 of file encoder-ax12-mode.c.
References callback(), Statemachine::callback, Statemachine::id, Statemachine::state, and STATE_IDLE.
Referenced by sensorMain().
00089 { 00090 sm->state = STATE_IDLE; 00091 sm->id = id; 00092 sm->callback = callback; 00093 }
void stm_process | ( | Statemachine * | sm, | |
byte | b | |||
) |
Definition at line 95 of file encoder-ax12-mode.c.
References BROADCAST_ID, Statemachine::callback, BioloidPacket::cmd, BioloidPacket::crc, BioloidPacket::id, Statemachine::id, BioloidPacket::len, MAX_PACKELEN, BioloidPacket::param, BioloidPacket::paramPtr, Statemachine::pkt, Statemachine::state, STATE_1ST_FF_RCVD, STATE_2ND_FF_RCVD, STATE_COMMAND_RCVD, STATE_ID_RCVD, STATE_IDLE, STATE_LENGTH_RCVD, and STATE_PKT_COMPLETE.
Referenced by sensorMain().
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 }
void TxD | ( | byte | bTxdData | ) |
Definition at line 362 of file encoder-ax12-mode.c.
References TXD_DATA, and TXD_READY.
Referenced by TxPacket().
Definition at line 328 of file encoder-ax12-mode.c.
References CHECK_TXD_FINISH, MAX_PACKELEN, RS485_RXD, RS485_TXD, SELF_ID, SET_TXD_FINISH, and TxD().
Referenced by callback().
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 }
signed char dist |
Definition at line 230 of file encoder-ax12-mode.c.
Smartbuffer interruptbuff [static] |
Definition at line 220 of file encoder-ax12-mode.c.
Definition at line 221 of file encoder-ax12-mode.c.
signed int wheel1 |
globals for ISR
Definition at line 222 of file encoder-ax12-mode.c.
Referenced by FeedbackSensor::enableSensor(), FeedbackSensor::FeedbackSensor(), FeedbackSensor::getFeedback(), and SIGNAL().
signed int wheel2 |
Definition at line 223 of file encoder-ax12-mode.c.
Referenced by FeedbackSensor::enableSensor(), FeedbackSensor::FeedbackSensor(), and FeedbackSensor::getFeedback().