#include <avr/io.h>
#include <avr/interrupt.h>
#include "SensorOnlyMode.h"
Go to the source code of this file.
Defines | |
#define | highbyte(w) ((byte) (w >> 8)) |
#define | lowbyte(w) ((byte) w) |
Functions | |
void | callback (Packet *pkt) |
void | initAtmega (void) |
int | sensorMain (void) |
void | stm_init (Statemachine *sm, byte id, void(*callback)(Packet *pkt)) |
void | stm_process (Statemachine *sm, byte b) |
void | TxD (byte bTxdData) |
byte | TxPacket (byte bError, byte bParameterLength) |
Variables | |
signed char | dist |
FeedbackSensor * | fs = new FeedbackSensor() |
volatile Smartbuffer | interruptbuff |
static byte | param [MAX_PARAM] |
static signed int | wheel1 |
static signed int | wheel2 |
#define highbyte | ( | w | ) | ((byte) (w >> 8)) |
Definition at line 9 of file SensorOnlyMode.c.
#define lowbyte | ( | w | ) | ((byte) w) |
Definition at line 8 of file SensorOnlyMode.c.
void callback | ( | Packet * | pkt | ) |
Definition at line 212 of file SensorOnlyMode.c.
References Packet::cmd, Packet::crc, E_CHECKSUM, highbyte_s, INST_PING, INST_READ, INST_RESET, lowbyte, NO_ERROR, P_DIST, Packet::param, RESET_WHEEL, TxPacket(), WORD_MAX_SIZE, and WORD_MIN_SIZE.
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 }
void initAtmega | ( | void | ) |
Definition at line 134 of file SensorOnlyMode.c.
References BAUD, RS485_RXD, and XTAL.
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 }
int sensorMain | ( | void | ) |
Definition at line 115 of file SensorOnlyMode.c.
References callback(), init, initAtmega(), isEmpty, pop, RESET_SENSOR, RS485_RXD, SELF_ID, stm_init(), and stm_process().
Referenced by main().
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 }
void stm_init | ( | Statemachine * | sm, | |
byte | id, | |||
void(*)(Packet *pkt) | callback | |||
) |
Definition at line 12 of file SensorOnlyMode.c.
References callback(), Statemachine::callback, Statemachine::id, Statemachine::state, and STATE_IDLE.
00013 { 00014 sm->state = STATE_IDLE; 00015 sm->id = id; 00016 sm->callback = callback; 00017 }
void stm_process | ( | Statemachine * | sm, | |
byte | b | |||
) |
Definition at line 19 of file SensorOnlyMode.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.
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 }
void TxD | ( | byte | bTxdData | ) |
Definition at line 171 of file SensorOnlyMode.c.
References CHECK_TXD_FINISH, MAX_PACKELEN, RS485_RXD, RS485_TXD, SELF_ID, SET_TXD_FINISH, and TxD().
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 }
signed char dist |
Definition at line 109 of file SensorOnlyMode.c.
FeedbackSensor* fs = new FeedbackSensor() |
Definition at line 112 of file SensorOnlyMode.c.
volatile Smartbuffer interruptbuff |
Definition at line 100 of file SensorOnlyMode.c.
Definition at line 101 of file SensorOnlyMode.c.
signed int wheel1 [static] |
Definition at line 102 of file SensorOnlyMode.c.
signed int wheel2 [static] |
Definition at line 102 of file SensorOnlyMode.c.