//takes a 16 bit value instead of an 8 bit value for maximum resolution
void pwmWriteHR(uint8_t pin, uint16_t val)
{
pinMode(pin, OUTPUT);
uint32_t tmp = val;
if (val == 0)
digitalWrite(pin, LOW);
else if (val == 65535)
digitalWrite(pin, HIGH);
else
{
TimerData td = timer_to_pwm_data[digitalPinToTimer(pin)];
if(td.ChannelRegLoc) //null checking
{
if(td.Is16Bit)
{
sbi(_SFR_MEM8(td.PinConnectRegLoc), td.PinConnectBits);
_SFR_MEM16(td.ChannelRegLoc) = (tmp * _SFR_MEM16(td.TimerTopRegLoc)) / 65535;
}
else
{
sbi(_SFR_MEM8(td.PinConnectRegLoc), td.PinConnectBits);
_SFR_MEM8(td.ChannelRegLoc) = (tmp * _SFR_MEM8(td.TimerTopRegLoc)) / 65535;
}
}
}
}
static int pwm_set_polarity(struct pwm *pwm, uint32_t val)
{
if (val)
_SFR_MEM8(pwm->ctl_reg) |= pwm->pol_msk;
else
_SFR_MEM8(pwm->ctl_reg) &= ~pwm->pol_msk;
return 0;
}
static void writeMany(const SPI_ABSTRACT_DEVICE* device,const void* dta, size_t size){
SPI_SW* spi = (SPI_SW*)(device->bus);
uint8_t delay = spi->_bus_.clock / 3; // delay loop is approx 3 cycles
uint8_t* src = (uint8_t*)dta;
// Get MOSI pin data
const IOPin* io = spi->MOSI;
const IOPort* mosiPortDef = (const IOPort*)pgm_read_word(&io->port);
PORT mosiPort = pgm_read_word(&mosiPortDef->port);
PIN mosiMask = pgm_read_byte(&io->pin);
// Get SCLK data
io = spi->SCLK;
const IOPort* sclkPortDef = (const IOPort*)pgm_read_word(&io->port);
PORT sclkPort = pgm_read_word(&sclkPortDef->port);
PIN sclkMask = pgm_read_byte(&io->pin);
uint8_t cpha = spi->_bus_.mode & 1;
if(spi->_bus_.order == SPI_DATA_ORDER_MSB){
// MSB is sent first
while(size--){
uint8_t rtn = *src++;
for(int i=0; i<8; i++){ /* for each bit */
if(cpha) _SFR_MEM8(sclkPort) ^= sclkMask;/* toggle clock (high) */
if(rtn & 0x80){
_SFR_MEM8(mosiPort) |= mosiMask; /* set high */
}else{
_SFR_MEM8(mosiPort) &= ~mosiMask; /* set low */
}
rtn<<=1;
if(!cpha) _SFR_MEM8(sclkPort) ^= sclkMask;/* toggle clock (high) */
if(delay) _delay_loop_1(delay); /* delay */
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (low) */
} /* next bit */
}
}else{
// LSB is sent first
while(size--){
uint8_t rtn = *src++;
for(int i=0; i<8; i++){ /* for each bit */
if(cpha) _SFR_MEM8(sclkPort) ^= sclkMask;/* toggle clock (high) */
if(rtn & 1){
_SFR_MEM8(mosiPort) |= mosiMask; /* set high */
}else{
_SFR_MEM8(mosiPort) &= ~mosiMask; /* set low */
}
rtn>>=1;
if(!cpha) _SFR_MEM8(sclkPort) ^= sclkMask;/* toggle clock (high) */
if(delay) _delay_loop_1(delay); /* delay */
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (low) */
} /* next bit */
}
}
__spiWriteMOSI(spi,device->fillerByte); /* Set the output pin */
/* using first bit of filler */
}
int main(void)
{
// Global init
sei();
uart_init();
fdevopen(uart1_dev_send, uart1_dev_recv);
// Error configuration
error_register_emerg(log_event);
error_register_error(log_event);
error_register_warning(log_event);
error_register_notice(log_event);
error_register_debug(log_event);
log_level = ERROR_SEVERITY_DEBUG;
// Clear screen
printf("%c[2J",0x1B);
printf("%c[0;0H",0x1B);
// Test
fpga_init();
wait_ms(100);
_SFR_MEM8(0x1800) = 1;
wait_ms(100);
_SFR_MEM8(0x1800) = 0;
NOTICE(0, "ADNS9500 init");
adns9500_init();
NOTICE(0, "ADNS9500 boot");
adns9500_boot();
NOTICE(0,"ADNS9500 > AUTO");
adns9500_set_mode(ADNS9500_BHVR_MODE_AUTOMATIC);
adns9500_encoders_t e;
while(1)
{
adns9500_encoders_get_value(&e);
printf("%ld %ld %ld %ld %ld %ld | %2.2X %2.2X %2.2X | %2.2X\n",
e.vectors[0], e.vectors[1], e.vectors[2],
e.vectors[3], e.vectors[4], e.vectors[5],
e.squals[0], e.squals[1], e.squals[2],
e.fault);
wait_ms(100);
}
NOTICE(0, "DONE");
while(1) nop();
return 0;
}
static void pwm_dis(struct pwm *pwm)
{
int id = pwm_id(pwm);
check_deinit_timer(pwm);
_SFR_MEM8(pwm->ctl_reg) &= ~pwm->en_msk;
/* Direction set back to 'in' only if it was 'in' before enable */
if (!(gpio_orig_dir & (1 << id)))
_SFR_MEM8(pwm->dir_reg) &= ~pwm->dir_msk;
pwm_stat &= ~(1 << id);
}
void
hal_subregister_write(uint16_t address, uint8_t mask, uint8_t position,
uint8_t value)
{
cli();
uint8_t register_value = _SFR_MEM8(address);
register_value &= ~mask;
value <<= position;
value &= mask;
value |= register_value;
_SFR_MEM8(address) = value;
sei();
}
int gpio_init(gpio_t pin, gpio_dir_t dir, gpio_pp_t pullup)
{
int res;
if (dir == GPIO_DIR_OUT) {
_SFR_MEM8(_ddr_addr(pin)) |= (1 << _pin_num(pin));
res = bit_is_set(_SFR_MEM8(_ddr_addr(pin)), _pin_num(pin));
}
else {
_SFR_MEM8(_ddr_addr(pin)) &= ~(1 << _pin_num(pin));
res = bit_is_clear(_SFR_MEM8(_ddr_addr(pin)), _pin_num(pin));
}
return (res == 0) ? -1 : 0;
}
void
hal_subregister_write(uint16_t address, uint8_t mask, uint8_t position,
uint8_t value)
{
HAL_ENTER_CRITICAL_REGION();
uint8_t register_value = _SFR_MEM8(address);
register_value &= ~mask;
value <<= position;
value &= mask;
value |= register_value;
_SFR_MEM8(address) = value;
HAL_LEAVE_CRITICAL_REGION();
}
/** \brief This function will download a frame to the radio transceiver's frame
* buffer.
*
* \param write_buffer Pointer to data that is to be written to frame buffer.
* \param length Length of data. The maximum length is 127 bytes.
*/
void
hal_frame_write(uint8_t *write_buffer, uint8_t length)
{
#if defined(__AVR_ATmega128RFA1__)
uint8_t *tx_buffer;
tx_buffer=(uint8_t *)0x180; //start of fifo in i/o space
/* Write frame length, including the two byte checksum */
/* The top bit of the length field shall be set to 0 for IEEE 802.15.4 compliant frames */
/* It should already be clear, so bypassing the masking is sanity check of the uip stack */
// length &= 0x7f;
_SFR_MEM8(tx_buffer++) = length;
/* Download to the Frame Buffer.
* When the FCS is autogenerated there is no need to transfer the last two bytes
* since they will be overwritten.
*/
#if !RF230_CONF_CHECKSUM
length -= 2;
#endif
do _SFR_MEM8(tx_buffer++)= *write_buffer++; while (--length);
#else /* defined(__AVR_ATmega128RFA1__) */
/* Optionally truncate length to maximum frame length.
* Not doing this is a fast way to know when the application needs fixing!
*/
// length &= 0x7f;
HAL_SPI_TRANSFER_OPEN();
/* Send Frame Transmit (long mode) command and frame length */
HAL_SPI_TRANSFER(0x60);
HAL_SPI_TRANSFER(length);
/* Download to the Frame Buffer.
* When the FCS is autogenerated there is no need to transfer the last two bytes
* since they will be overwritten.
*/
#if !RF230_CONF_CHECKSUM
length -= 2;
#endif
do HAL_SPI_TRANSFER(*write_buffer++); while (--length);
HAL_SPI_TRANSFER_CLOSE();
#endif /* defined(__AVR_ATmega128RFA1__) */
}
void pwmWrite(uint8_t pin, uint8_t val)
{
pinMode(pin, OUTPUT);
//casting "val" to be larger so that the final value (which is the partially
//the result of multiplying two potentially high value int16s) will not truncate
uint32_t tmp = val;
if (val == 0)
digitalWrite(pin, LOW);
else if (val == 255)
digitalWrite(pin, HIGH);
else
{
uint16_t regLoc16 = 0;
uint16_t regLoc8 = 0;
uint16_t top;
switch(digitalPinToTimer(pin))
{
case TIMER0B:
sbi(TCCR0A, COM0B1);
regLoc8 = OCR0B_MEM;
top = Timer0_GetTop();
break;
case TIMER1A:
sbi(TCCR1A, COM1A1);
regLoc16 = OCR1A_MEM;
top = Timer1_GetTop();
break;
case TIMER1B:
sbi(TCCR1A, COM1B1);
regLoc16 = OCR1B_MEM;
top = Timer1_GetTop();
break;
case TIMER2B:
sbi(TCCR2A, COM2B1);
regLoc8 = OCR2B_MEM;
top = Timer2_GetTop();
break;
case NOT_ON_TIMER:
default:
if (val < 128)
digitalWrite(pin, LOW);
else
digitalWrite(pin, HIGH);
return;
}
if(regLoc16)
_SFR_MEM16(regLoc16) = (tmp*top)/255;
else
_SFR_MEM8(regLoc8) = (tmp*top)/255;
}
}
int gpio_init(gpio_t pin, gpio_mode_t mode)
{
switch (mode) {
case GPIO_OUT:
_SFR_MEM8(_ddr_addr(pin)) |= (1 << _pin_num(pin));
break;
case GPIO_IN:
_SFR_MEM8(_ddr_addr(pin)) &= ~(1 << _pin_num(pin));
_SFR_MEM8(_port_addr(pin)) &= ~(1 << _pin_num(pin));
break;
case GPIO_IN_PU:
_SFR_MEM8(_port_addr(pin)) |= (1 << _pin_num(pin));
break;
default:
return -1;
}
return 0;
}
/******************************************************
Read the value of an I/O pin and
return TRUE if it is high or FALSE if low
******************************************************/
boolean pin_is_high(const IOPin* io){
if(io){
const IOPort* portDef = (const IOPort*)pgm_read_word(&io->port);
PORT pin = pgm_read_word(&portDef->pin);
PIN mask = pgm_read_byte(&io->pin);
return (_SFR_MEM8(pin) & mask) ? TRUE : FALSE;
}
return FALSE;
}
static int pwm_en(struct pwm *pwm)
{
int id = pwm_id(pwm);
check_init_timer(pwm);
_SFR_MEM8(pwm->ctl_reg) |= pwm->en_msk;
if (_SFR_MEM8(pwm->dir_reg) & pwm->dir_msk) {
/* Output direction already enabled */
gpio_orig_dir |= (1 << id);
}
else {
/* Enable output direction */
gpio_orig_dir &= ~(1 << id);
_SFR_MEM8(pwm->dir_reg) |= pwm->dir_msk;
}
pwm_stat |= (1 << id);
return 0;
}
//takes a 16 bit value instead of an 8 bit value for maximum resolution
void pwmWriteHR(uint8_t pin, uint16_t val)
{
pinMode(pin, OUTPUT);
uint32_t tmp = val;
if (val == 0)
digitalWrite(pin, LOW);
else if (val == 65535)
digitalWrite(pin, HIGH);
else
{
uint16_t regLoc16 = 0;
uint16_t regLoc8 = 0;
uint16_t top;
switch(digitalPinToTimer(pin))
{
case TIMER0B:
sbi(TCCR0A, COM0B1);
regLoc8 = OCR0B_MEM;
top = Timer0_GetTop();
break;
case TIMER1A:
sbi(TCCR1A, COM1A1);
regLoc16 = OCR1A_MEM;
top = Timer1_GetTop();
break;
case TIMER1B:
sbi(TCCR1A, COM1B1);
regLoc16 = OCR1B_MEM;
top = Timer1_GetTop();
break;
case TIMER2B:
sbi(TCCR2A, COM2B1);
regLoc8 = OCR2B_MEM;
top = Timer2_GetTop();
break;
case NOT_ON_TIMER:
default:
if (val < 128)
digitalWrite(pin, LOW);
else
digitalWrite(pin, HIGH);
return;
}
if(regLoc16)
_SFR_MEM16(regLoc16) = (tmp*top)/65535;
else
_SFR_MEM8(regLoc8) = (tmp*top)/65535;
}
}
float GetPinResolution(uint8_t pin)
{
TimerData td = timer_to_pwm_data[digitalPinToTimer(pin)];
double baseTenRes = 0;
if(td.ChannelRegLoc)
{
//getting a base 10 resolution
td.Is16Bit? (baseTenRes = _SFR_MEM16(td.TimerTopRegLoc)) : (baseTenRes = _SFR_MEM8(td.TimerTopRegLoc));
//change the base and return
return toBaseTwo(baseTenRes);
}
else
{
return 0;
}
}
static void readMany(const SPI_ABSTRACT_DEVICE* device,void* dta, size_t size){
SPI_SW* spi = (SPI_SW*)(device->bus);
uint8_t delay = spi->_bus_.clock / 3; // delay loop is approx 3 cycles
uint8_t* dst = (uint8_t*)dta;
__spiWriteMOSI(spi,device->fillerByte); /* Set the output pin */
/* using first bit of filler */
// Get MISO pin data
const IOPin* io = spi->MISO;
const IOPort* misoPortDef = (const IOPort*)pgm_read_word(&io->port);
PORT misoPin = pgm_read_word(&misoPortDef->pin);
PIN misoMask = pgm_read_byte(&io->pin);
// Get SCLK data
io = spi->SCLK;
const IOPort* sclkPortDef = (const IOPort*)pgm_read_word(&io->port);
PORT sclkPort = pgm_read_word(&sclkPortDef->port);
PIN sclkMask = pgm_read_byte(&io->pin);
uint8_t cpha = spi->_bus_.mode & 1;
if(spi->_bus_.order == SPI_DATA_ORDER_MSB){
// MSB is sent first
while(size--){
uint8_t rtn = 0;
for(int i=0; i<8; i++){ /* for each bit */
rtn<<=1;
if(!cpha){ // Mode 0 and 2
pin_high(spi->MOSI); /* make MOSI high */
if(_SFR_MEM8(misoPin) & misoMask){ /* read input for modes 0 & 2 */
rtn |= 1;
}
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (high) */
if(delay) _delay_loop_1(delay); /* delay */
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (low) */
}else{ // Mode 1 or 3
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (high) */
pin_high(spi->MOSI); /* make MOSI high */
if(delay) _delay_loop_1(delay); /* delay */
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (low) */
if(_SFR_MEM8(misoPin) & misoMask){ /* read input for modes 1 & 3 */
rtn |= 1;
}
}
} /* next bit */
*dst++ = rtn; /* store byte */
}
}else{
// LSB is sent first
while(size--){
uint8_t rtn = 0;
for(int i=0; i<8; i++){ /* for each bit */
rtn>>=1;
if(!cpha){ // Mode 0 or 2
pin_high(spi->MOSI); /* make MOSI high */
if(_SFR_MEM8(misoPin) & misoMask){ /* read input for modes 0 & 2 */
rtn |= 0x80;
}
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (high) */
if(delay) _delay_loop_1(delay); /* delay */
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (low) */
}else{ // Mode 1 or 3
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (high) */
pin_high(spi->MOSI); /* make MOSI high */
if(delay) _delay_loop_1(delay); /* delay */
_SFR_MEM8(sclkPort) ^= sclkMask; /* toggle clock (low) */
if(_SFR_MEM8(misoPin) & misoMask){ /* read input for modes 1 & 3 */
rtn |= 0x80;
}
}
} /* next bit */
*dst++ = rtn; /* store byte */
}
}
}
/** \brief This function will upload a frame from the radio transceiver's frame
* buffer.
*
* If the frame currently available in the radio transceiver's frame buffer
* is out of the defined bounds. Then the frame length, lqi value and crc
* be set to zero. This is done to indicate an error.
* This version is optimized for use with contiki RF230BB driver.
* The callback routine and CRC are left out for speed in reading the rx buffer.
* Any delays here can lead to overwrites by the next packet!
*
* \param rx_frame Pointer to the data structure where the frame is stored.
* \param rx_callback Pointer to callback function for receiving one byte at a time.
*/
void
//hal_frame_read(hal_rx_frame_t *rx_frame, rx_callback_t rx_callback)
hal_frame_read(hal_rx_frame_t *rx_frame)
{
#if defined(__AVR_ATmega128RFA1__)
uint8_t frame_length,*rx_data,*rx_buffer;
rx_data = (rx_frame->data);
frame_length = TST_RX_LENGTH; //frame length, not including lqi?
rx_frame->length = frame_length;
rx_buffer=(uint8_t *)0x180; //start of fifo in i/o space
do{
*rx_data++ = _SFR_MEM8(rx_buffer++);
} while (--frame_length > 0);
/*Read LQI value for this frame.*/
rx_frame->lqi = *rx_buffer;
if (1) {
#else /* defined(__AVR_ATmega128RFA1__) */
uint8_t *rx_data;
/* check that we have either valid frame pointer or callback pointer */
// if (!rx_frame && !rx_callback)
// return;
HAL_SPI_TRANSFER_OPEN();
/*Send frame read (long mode) command.*/
HAL_SPI_TRANSFER(0x20);
/*Read frame length. This includes the checksum. */
uint8_t frame_length = HAL_SPI_TRANSFER(0);
/*Check for correct frame length.*/
// if ((frame_length >= HAL_MIN_FRAME_LENGTH) && (frame_length <= HAL_MAX_FRAME_LENGTH)){
if (1) {
// uint16_t crc = 0;
// if (rx_frame){
rx_data = (rx_frame->data);
rx_frame->length = frame_length;
// } else {
// rx_callback(frame_length);
// }
/*Upload frame buffer to data pointer */
HAL_SPI_TRANSFER_WRITE(0);
HAL_SPI_TRANSFER_WAIT();
do{
*rx_data++ = HAL_SPI_TRANSFER_READ();
HAL_SPI_TRANSFER_WRITE(0);
// if (rx_frame){
// *rx_data++ = tempData;
// } else {
// rx_callback(tempData);
// }
/* RF230 does crc in hardware, doing the checksum here ensures the rx buffer has not been overwritten by the next packet */
/* Since doing the checksum makes such overwrites more probable, we skip it and hope for the best. */
/* A full buffer should be read in 320us at 2x spi clocking, so with a low interrupt latency overwrites should not occur */
// crc = _crc_ccitt_update(crc, tempData);
HAL_SPI_TRANSFER_WAIT();
} while (--frame_length > 0);
/*Read LQI value for this frame.*/
// if (rx_frame){
rx_frame->lqi = HAL_SPI_TRANSFER_READ();
// } else {
// rx_callback(HAL_SPI_TRANSFER_READ());
// }
#endif /* defined(__AVR_ATmega128RFA1__) */
/*Check calculated crc, and set crc field in hal_rx_frame_t accordingly.*/
// if (rx_frame){
rx_frame->crc = 1;
// } else {
// rx_callback(crc != 0);
// }
} else {
// if (rx_frame){
rx_frame->length = 0;
rx_frame->lqi = 0;
rx_frame->crc = false;
// }
}
HAL_SPI_TRANSFER_CLOSE();
}
/*----------------------------------------------------------------------------*/
/** \brief This function will download a frame to the radio transceiver's frame
* buffer.
*
* \param write_buffer Pointer to data that is to be written to frame buffer.
* \param length Length of data. The maximum length is 127 bytes.
*/
void
hal_frame_write(uint8_t *write_buffer, uint8_t length)
{
#if defined(__AVR_ATmega128RFA1__)
uint8_t *tx_buffer;
tx_buffer=(uint8_t *)0x180; //start of fifo in i/o space
/* Write frame length, including the two byte checksum */
/* The top bit of the length field shall be set to 0 for IEEE 802.15.4 compliant frames */
/* It should already be clear, so bypassing the masking is sanity check of the uip stack */
// length &= 0x7f;
_SFR_MEM8(tx_buffer++) = length;
/* Download to the Frame Buffer.
* When the FCS is autogenerated there is no need to transfer the last two bytes
* since they will be overwritten.
*/
#if !RF230_CONF_CHECKSUM
length -= 2;
#endif
do _SFR_MEM8(tx_buffer++)= *write_buffer++; while (--length);
#else /* defined(__AVR_ATmega128RFA1__) */
/* Optionally truncate length to maximum frame length.
* Not doing this is a fast way to know when the application needs fixing!
*/
// length &= 0x7f;
HAL_SPI_TRANSFER_OPEN();
/* Send Frame Transmit (long mode) command and frame length */
HAL_SPI_TRANSFER(0x60);
HAL_SPI_TRANSFER(length);
/* Download to the Frame Buffer.
* When the FCS is autogenerated there is no need to transfer the last two bytes
* since they will be overwritten.
*/
#if !RF230_CONF_CHECKSUM
length -= 2;
#endif
do HAL_SPI_TRANSFER(*write_buffer++); while (--length);
HAL_SPI_TRANSFER_CLOSE();
#endif /* defined(__AVR_ATmega128RFA1__) */
}
/** \brief Transfer a frame from the radio transceiver to a RAM buffer
*
* This version is optimized for use with contiki RF230BB driver.
* The callback routine and CRC are left out for speed in reading the rx buffer.
* Any delays here can lead to overwrites by the next packet!
*
* If the frame length is out of the defined bounds, the length, lqi and crc
* are set to zero.
*
* \param rx_frame Pointer to the data structure where the frame is stored.
*/
void
hal_frame_read(hal_rx_frame_t *rx_frame)
{
#if defined(__AVR_ATmega128RFA1__)
uint8_t frame_length,*rx_data,*rx_buffer;
/* Get length from the TXT_RX_LENGTH register, not including LQI
* Bypassing the length check can result in overrun if buffer is < 256 bytes.
*/
frame_length = TST_RX_LENGTH;
if ((frame_length < HAL_MIN_FRAME_LENGTH) || (frame_length > HAL_MAX_FRAME_LENGTH)) {
/* Length test failed */
rx_frame->length = 0;
rx_frame->lqi = 0;
rx_frame->crc = false;
return;
}
rx_frame->length = frame_length;
/* Start of buffer in I/O space, pointer to RAM buffer */
rx_buffer=(uint8_t *)0x180;
rx_data = (rx_frame->data);
do{
*rx_data++ = _SFR_MEM8(rx_buffer++);
} while (--frame_length > 0);
/*Read LQI value for this frame.*/
rx_frame->lqi = *rx_buffer;
/* If crc was calculated set crc field in hal_rx_frame_t accordingly.
* Else show the crc has passed the hardware check.
*/
rx_frame->crc = true;
#else /* defined(__AVR_ATmega128RFA1__) */
uint8_t frame_length, *rx_data;
/*Send frame read (long mode) command.*/
HAL_SPI_TRANSFER_OPEN();
HAL_SPI_TRANSFER(0x20);
/*Read frame length. This includes the checksum. */
frame_length = HAL_SPI_TRANSFER(0);
/*Check for correct frame length. Bypassing this test can result in a buffer overrun! */
if ((frame_length < HAL_MIN_FRAME_LENGTH) || (frame_length > HAL_MAX_FRAME_LENGTH)) {
/* Length test failed */
rx_frame->length = 0;
rx_frame->lqi = 0;
rx_frame->crc = false;
}
else {
rx_data = (rx_frame->data);
rx_frame->length = frame_length;
/*Transfer frame buffer to RAM buffer */
HAL_SPI_TRANSFER_WRITE(0);
HAL_SPI_TRANSFER_WAIT();
do{
*rx_data++ = HAL_SPI_TRANSFER_READ();
HAL_SPI_TRANSFER_WRITE(0);
/* CRC was checked in hardware, but redoing the checksum here ensures the rx buffer
* is not being overwritten by the next packet. Since that lengthy computation makes
* such overwrites more likely, we skip it and hope for the best.
* Without the check a full buffer is read in 320us at 2x spi clocking.
* The 802.15.4 standard requires 640us after a greater than 18 byte frame.
* With a low interrupt latency overwrites should never occur.
*/
// crc = _crc_ccitt_update(crc, tempData);
HAL_SPI_TRANSFER_WAIT();
} while (--frame_length > 0);
/*Read LQI value for this frame.*/
rx_frame->lqi = HAL_SPI_TRANSFER_READ();
/* If crc was calculated set crc field in hal_rx_frame_t accordingly.
* Else show the crc has passed the hardware check.
*/
rx_frame->crc = true;
}
HAL_SPI_TRANSFER_CLOSE();
#endif /* defined(__AVR_ATmega128RFA1__) */
}
/* Hack for internal radio registers. hal_register_read and hal_register_write are
handled through defines, but the preprocesser can't parse a macro containing
another #define with multiple arguments, e.g. using
#define hal_subregister_read( address, mask, position ) (address&mask)>>position
#define SR_TRX_STATUS TRX_STATUS, 0x1f, 0
the following only sees 1 argument to the macro
return hal_subregister_read(SR_TRX_STATUS);
Possible fix is through two defines:
#define x_hal_subregister_read(x) hal_subregister_read(x);
#define hal_subregister_read( address, mask, position ) (address&mask)>>position
but the subregister defines in atmega128rfa1_registermap.h are currently set up without
the _SFR_MEM8 attribute, for use by hal_subregister_write.
*/
uint8_t
hal_subregister_read(uint16_t address, uint8_t mask, uint8_t position)
{
return (_SFR_MEM8(address)&mask)>>position;
}
/** \brief Transfer a frame from the radio transceiver to a RAM buffer
*
* This version is optimized for use with contiki RF230BB driver.
* The callback routine and CRC are left out for speed in reading the rx buffer.
* Any delays here can lead to overwrites by the next packet!
*
* If the frame length is out of the defined bounds, the length, lqi and crc
* are set to zero.
*
* \param rx_frame Pointer to the data structure where the frame is stored.
*/
void
hal_frame_read(hal_rx_frame_t *rx_frame)
{
#if defined(__AVR_ATmega128RFA1__)
uint8_t frame_length,*rx_data,*rx_buffer;
/* Get length from the TXT_RX_LENGTH register, not including LQI
* Bypassing the length check can result in overrun if buffer is < 256 bytes.
*/
frame_length = TST_RX_LENGTH;
if ( 0 || ((frame_length >= HAL_MIN_FRAME_LENGTH) && (frame_length <= HAL_MAX_FRAME_LENGTH))) {
rx_frame->length = frame_length;
/* Start of buffer in I/O space, pointer to RAM buffer */
rx_buffer=(uint8_t *)0x180;
rx_data = (rx_frame->data);
do{
*rx_data++ = _SFR_MEM8(rx_buffer++);
} while (--frame_length > 0);
/*Read LQI value for this frame.*/
rx_frame->lqi = *rx_buffer;
#else /* defined(__AVR_ATmega128RFA1__) */
uint8_t *rx_data;
/*Send frame read (long mode) command.*/
HAL_SPI_TRANSFER_OPEN();
HAL_SPI_TRANSFER(0x20);
/*Read frame length. This includes the checksum. */
uint8_t frame_length = HAL_SPI_TRANSFER(0);
/*Check for correct frame length. Bypassing this test can result in a buffer overrun! */
if ( 0 || ((frame_length >= HAL_MIN_FRAME_LENGTH) && (frame_length <= HAL_MAX_FRAME_LENGTH))) {
rx_data = (rx_frame->data);
rx_frame->length = frame_length;
/*Transfer frame buffer to RAM buffer */
HAL_SPI_TRANSFER_WRITE(0);
HAL_SPI_TRANSFER_WAIT();
do{
*rx_data++ = HAL_SPI_TRANSFER_READ();
HAL_SPI_TRANSFER_WRITE(0);
/* CRC was checked in hardware, but redoing the checksum here ensures the rx buffer
* is not being overwritten by the next packet. Since that lengthy computation makes
* such overwrites more likely, we skip it and hope for the best.
* Without the check a full buffer is read in 320us at 2x spi clocking.
* The 802.15.4 standard requires 640us after a greater than 18 byte frame.
* With a low interrupt latency overwrites should never occur.
*/
// crc = _crc_ccitt_update(crc, tempData);
HAL_SPI_TRANSFER_WAIT();
} while (--frame_length > 0);
/*Read LQI value for this frame.*/
rx_frame->lqi = HAL_SPI_TRANSFER_READ();
#endif /* defined(__AVR_ATmega128RFA1__) */
/* If crc was calculated set crc field in hal_rx_frame_t accordingly.
* Else show the crc has passed the hardware check.
*/
rx_frame->crc = true;
} else {
/* Length test failed */
rx_frame->length = 0;
rx_frame->lqi = 0;
rx_frame->crc = false;
}
HAL_SPI_TRANSFER_CLOSE();
}
/*----------------------------------------------------------------------------*/
/** \brief This function will download a frame to the radio transceiver's frame
* buffer.
*
* \param write_buffer Pointer to data that is to be written to frame buffer.
* \param length Length of data. The maximum length is 127 bytes.
*/
void
hal_frame_write(uint8_t *write_buffer, uint8_t length)
{
#if defined(__AVR_ATmega128RFA1__)
uint8_t *tx_buffer;
tx_buffer=(uint8_t *)0x180; //start of fifo in i/o space
/* Write frame length, including the two byte checksum */
/* The top bit of the length field shall be set to 0 for IEEE 802.15.4 compliant frames */
/* It should already be clear, so bypassing the masking is sanity check of the uip stack */
// length &= 0x7f;
_SFR_MEM8(tx_buffer++) = length;
/* Download to the Frame Buffer.
* When the FCS is autogenerated there is no need to transfer the last two bytes
* since they will be overwritten.
*/
#if !RF230_CONF_CHECKSUM
length -= 2;
#endif
do _SFR_MEM8(tx_buffer++)= *write_buffer++; while (--length);
#else /* defined(__AVR_ATmega128RFA1__) */
/* Optionally truncate length to maximum frame length.
* Not doing this is a fast way to know when the application needs fixing!
*/
// length &= 0x7f;
HAL_SPI_TRANSFER_OPEN();
/* Send Frame Transmit (long mode) command and frame length */
HAL_SPI_TRANSFER(0x60);
HAL_SPI_TRANSFER(length);
/* Download to the Frame Buffer.
* When the FCS is autogenerated there is no need to transfer the last two bytes
* since they will be overwritten.
*/
#if !RF230_CONF_CHECKSUM
length -= 2;
#endif
do HAL_SPI_TRANSFER(*write_buffer++); while (--length);
HAL_SPI_TRANSFER_CLOSE();
#endif /* defined(__AVR_ATmega128RFA1__) */
}
static int pwm_get_polarity(const struct pwm *pwm)
{
return _SFR_MEM8(pwm->ctl_reg) & pwm->pol_msk ? 1 : 0;
}
void gpio_set(gpio_t pin)
{
_SFR_MEM8(_port_addr(pin)) |= (1 << _pin_num(pin));
}
int gpio_read(gpio_t pin)
{
return (_SFR_MEM8(_pin_addr(pin)) & (1 << _pin_num(pin)));
}
void __portMaskClear(const PORT_MASK* pm){
register PORT port = pgm_read_word(&pm->port);
register PIN mask = pgm_read_byte(&pm->mask);
_SFR_MEM8(port) &= ~mask;
}
void gpio_clear(gpio_t pin)
{
_SFR_MEM8(_port_addr(pin)) &= ~(1 << _pin_num(pin));
}
int main(void)
{
// ADNS configuration
adns6010_configuration_t adns_config;
//--------------------------------------------------------------------------
// Booting
// Turn interruptions ON
sei();
// Initialize UART
uart_init();
fdevopen(uart1_dev_send, uart1_dev_recv);
//--------------------------------------------------------
// Error configuration
error_register_emerg(log_event);
error_register_error(log_event);
error_register_warning(log_event);
error_register_notice(log_event);
error_register_debug(log_event);
log_level = ERROR_SEVERITY_NOTICE;
//log_level = ERROR_SEVERITY_DEBUG;
// Clear screen
printf("%c[2J",0x1B);
printf("%c[0;0H",0x1B);
// Some advertisment :p
NOTICE(0,"Robotter 2009 - Galipeur - UNIOC-NG ADNS6010 CALIBRATION");
NOTICE(0,"Compiled "__DATE__" at "__TIME__".");
//--------------------------------------------------------
// Initialize scheduler
scheduler_init();
//--------------------------------------------------------
// Initialize time
time_init(160);
//--------------------------------------------------------
// Initialize FPGA
fpga_init();
// turn off led
_SFR_MEM8(0x1800) = 1;
//--------------------------------------------------------
// ADNS6010
//--------------------------------------------------------
NOTICE(0,"Initializing ADNS6010s");
adns6010_init();
#ifndef ADNS_OVERRIDE
NOTICE(0,"Checking ADNS6010s firmware");
adns6010_checkFirmware();
// ADNS CONFIGURATION
adns_config.res = ADNS6010_RES_2000;
adns_config.shutter = ADNS6010_SHUTTER_OFF;
adns_config.power = 0x11;
NOTICE(0,"Checking ADNS6010s SPI communication");
adns6010_checkSPI();
NOTICE(0,"Booting ADNS6010s");
adns6010_boot(&adns_config);
NOTICE(0,"Checking ADNS6010s");
adns6010_checks();
NOTICE(0,"ADNS6010s are GO");
#endif
//--------------------------------------------------------
NOTICE(0,"Initializing ADCs");
adc_init();
// For ploting purposes
NOTICE(0,"<PLOTMARK>");
// Set ADNS6010 system to automatic
adns6010_setMode(ADNS6010_BHVR_MODE_AUTOMATIC);
// Safe key event
scheduler_add_periodical_event_priority(&safe_key_pressed, NULL, 100, 50);
//----------------------------------------------------------------------
NOTICE(0,"Any key to go");
char cal_name = 'x';
uint8_t c;
while(1)
{
c = cli_getkey();
if(c != -1)
break;
}
//----------------------------------------------------------------------
//----------------------------------------------------------------------
int i,k;
NOTICE(0,"Go");
// Initialize control systems
cs_initialize();
cs_vx=0;
cs_vy=0;
cs_omega=0;
// remove break
motor_cs_break(0);
event_cs =
scheduler_add_periodical_event_priority(&cs_update, NULL, 25, 100);
NOTICE(0,"Press 'r' for auto line calibration / 'l' for ADNS line calibration / 'm' for motor encoders line calibration / 'a' for angle calibration / 'n' non-auto angle calibration / 'k' motor non-auto angle calibration");
c = cli_getkey();
//-----------------------------------------------------------------------------
// MOTOR ENCODERS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='m')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
NOTICE(0,"ME ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
motor_encoders_get_value(&motor_zero);
wait_ms(200);
NOTICE(0,"ME zero values = (%ld,%ld,%ld)",
motor_zero.vectors[0],motor_zero.vectors[1],motor_zero.vectors[2]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
cs_vx=0;
cs_vy=0;
cs_omega=0;
// perform calibration, positive direction
motor_line_calibration(1);
NOTICE(0,"ME ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
motor_encoders_get_value(&motor_zero);
wait_ms(200);
NOTICE(0,"ME zero values = (%ld,%ld,%ld)",
motor_zero.vectors[0],motor_zero.vectors[1],motor_zero.vectors[2]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
// perform calibration, negative direction
motor_line_calibration(-1);
// output calibration data
NOTICE(0,"CALIBRATION DONE, printint scilab matrix :");
printf("%c=[\n",cal_name);
for(i=0;i<12;i++)
for(k=0;k<3;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==2)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// ADNS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='r')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
for(i=0;i<20;i++)
{
setmotors_course(robot_angle, 1);
wait_ms(1000);
setmotors_course(robot_angle, -1);
wait_ms(1000);
adns6010_encoders_get_value(&adns_zero);
}
}
//-----------------------------------------------------------------------------
// ADNS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='l')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
NOTICE(0,"ADNS ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
adns6010_encoders_get_value(&adns_zero);
wait_ms(200);
NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)",
adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2],
adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
cs_vx=0;
cs_vy=0;
cs_omega=0;
// perform calibration, positive direction
line_calibration(1);
NOTICE(0,"ADNS ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
adns6010_encoders_get_value(&adns_zero);
wait_ms(200);
NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)",
adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2],
adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
// perform calibration, negative direction
line_calibration(-1);
// output calibration data
NOTICE(0,"CALIBRATION DONE, printint scilab matrix :");
printf("%c=[\n",cal_name);
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='a' )
{
NOTICE(0,"Angle calibration is fully automatic, robot will do approx. 3 turns");
NOTICE(0,"Press a key to start, calibration will start ~5s after.");
while(!cli_getkey());
NOTICE(0,"Starting in 5s...");
wait_ms(5000);
angle_calibration(1);
angle_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
int i,k;
for(i=0;i<12;i++)
{
printf("u_r%d = [0 0 %7.1f];\n",i,calibration_data_compass[i]);
}
printf("// ");
for(i=0;i<12;i++)
if(i==11)
printf("u_r11];\n");
else
printf("u_r%d;",i);
}
//-----------------------------------------------------------------------------
// MOTOR NON-AUTO ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='k' )
{
NOTICE(0,"key to go");
while(!cli_getkey());
motor_angle_na_calibration(1);
motor_angle_na_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<3;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==2)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// NON-AUTO ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='n' )
{
NOTICE(0,"key to go");
while(!cli_getkey());
angle_na_calibration(1);
angle_na_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
EMERG(0,"Program ended");
while(1);
return 0;
}
