-
Notifications
You must be signed in to change notification settings - Fork 0
function.c
In this function we both receive new data from wireless board and send debug and monitoring data to it.This function is called in ISR(PORTD_INT0_vect) in main.c which is an external interrupt handler. It first gets the status of nRF24l01. nRF24l01 in robots operate in PRX mode. PRX stands for primary receiver. In this mode the most important bit in status register is _RX_DR.
When nRF24l01 receives a new packet of data it sets the _RX_DR and making an interrupt request by pulling down the IRQ line which is connected to pin 2 of PORTD. Then ISR(PORTD_INT0_vect) executes which calls wireless_connection ( ).
In wireless_connection ( ) we check _RX_DR bit and if it is set, get new packet -> NRF24L01_Read_RX_Buf(spi_rx_buf, _Buffer_Size);
Warning : Reading status bit
_RX_DR
clears it which is necessary for next interrupt request from nRF24l01.
TODO : Last if statement is not needed(?).
inline void wireless_connection ( void )
{
uint8_t status_L = NRF24L01_WriteReg(W_REGISTER | STATUSe, _TX_DS|_MAX_RT|_RX_DR);
if((status_L & _RX_DR) == _RX_DR)
{
ioport_set_value(LED_WHITE, high);
WIRLESS_TIMEOUT_TIMER = 0;
wdt_reset();
//! read payload through SPI,
NRF24L01_Read_RX_Buf(spi_rx_buf, _Buffer_Size);
free_wheel.wireless_timeout = false ;
if(spi_rx_buf[0] == RobotID )
{
ioport_set_value(LED_RED, high);
Robot.RID = spi_rx_buf[0];
Robot.Vx_sp.byte[high] = spi_rx_buf[1];
Robot.Vx_sp.byte[low] = spi_rx_buf[2];
Robot.Vy_sp.byte[high] = spi_rx_buf[3];
Robot.Vy_sp.byte[low] = spi_rx_buf[4];
Robot.Wr_sp.byte[high] = spi_rx_buf[5];
Robot.Wr_sp.byte[low] = spi_rx_buf[6];
Robot.Vx.byte[high] = spi_rx_buf[7];
Robot.Vx.byte[low] = spi_rx_buf[8];
Robot.Vy.byte[high] = spi_rx_buf[9];
Robot.Vy.byte[low] = spi_rx_buf[10];
Robot.Wr.byte[high] = spi_rx_buf[11];
Robot.Wr.byte[low] = spi_rx_buf[12];
Robot.alpha.byte[high] = spi_rx_buf[13];
Robot.alpha.byte[low] = spi_rx_buf[14];
Robot.KICK = spi_rx_buf[15];
Robot.CHIP = spi_rx_buf[16];
/*Robot.SPIN*/Robot.orc_length = spi_rx_buf[17];//! test !!!!!!!!!!
NRF24L01_Write_TX_Buf(spi_tx_buf, _Buffer_Size);
signal_strength++;
}
}
if ((status_L&_MAX_RT) == _MAX_RT)
{
NRF24L01_Flush_TX();
}
}In this function we transmit all monitoring data to nRF24l01 to be sent to Wireless board.
TODO : Using DMA
inline void data_transmission (void)
{
HL show[16];
show[0].full = cycle_time_us ;
show[1].full = free_wheel.wireless_timeout;
show[2].full = free_wheel.motor_fault;
show[3].full = free_wheel.low_battery;
show[4].full = Robot.ct;
show[5].full = Robot.nsp;
show[6].full = Robot.nrp;
show[7].full = Robot.ss;
show[8].full = bbs.lko;
show[9].full = Robot.wrc;
show[10].full = Robot.MCU_temperature.full ;
show[11].full = (int)(Robot.I0.full*1000) ;
show[12].full = (int)(Robot.I1.full*1000) ;
show[13].full = (int)(Robot.I2.full*1000) ;
show[14].full = (int)(Robot.I3.full*1000) ;
//! Debug data
spi_tx_buf[0] = show[0].byte[high];//
spi_tx_buf[1] = show[0].byte[low]; //
spi_tx_buf[2] = show[1].byte[high];//
spi_tx_buf[3] = show[1].byte[low]; //
spi_tx_buf[4] = show[2].byte[high];//
spi_tx_buf[5] = show[2].byte[low]; //
spi_tx_buf[6] = show[3].byte[high];//
spi_tx_buf[7] = show[3].byte[low]; //
//! Monitoring data
spi_tx_buf[8] = show[4].byte[high];
spi_tx_buf[9] = show[4].byte[low];
spi_tx_buf[10] = show[5].byte[high];
spi_tx_buf[11] = show[5].byte[low];
spi_tx_buf[12] = show[6].byte[high];
spi_tx_buf[13] = show[6].byte[low];
spi_tx_buf[14] = show[7].byte[high];
spi_tx_buf[15] = show[7].byte[low];
spi_tx_buf[16] = show[8].byte[high];
spi_tx_buf[17] = show[8].byte[low];
spi_tx_buf[18] = show[9].byte[high];
spi_tx_buf[19] = show[9].byte[low];
spi_tx_buf[20] = show[10].byte[high];
spi_tx_buf[21] = show[10].byte[low];
spi_tx_buf[22] = show[11].byte[high];
spi_tx_buf[23] = show[11].byte[low];
spi_tx_buf[24] = show[12].byte[high];
spi_tx_buf[25] = show[12].byte[low];
spi_tx_buf[26] = show[13].byte[high];
spi_tx_buf[27] = show[13].byte[low];
spi_tx_buf[28] = show[14].byte[high];
spi_tx_buf[29] = show[14].byte[low];
spi_tx_buf[30] = Robot.batx1000.byte[high];
spi_tx_buf[31] = Robot.batx1000.byte[low];
}data_packing packs five words (2 bytes length) into a packet. These words are duty cycles which controller generates for each motor. Lack of enough connection line between SPARTAN3 and ATxmega64 (there is 15 lines) made us to use a 7 bit communication protocol in both direction. Last line is for synchronizing SPARTAN3 and ATxmega64 (clock line). In this protocol we cut last bit from each byte and make a new 7-bit data from cut bits. There is five payload words, and two checksum words, generated from payload, so the will be 5+2 words. So there is 14 bytes and it yields 14[byte] * 8[bit] / 7[bit] = 16 of 7-bit data type. In addition of payloads and checksum, there is two 7-bit preambles (start sign) at the beginning of packet.
7-bits that should be sent are placed in even rooms of an array. 7-bits that should be received will be placed in odd rooms of another array.
inline void data_packing ( void )
{
HL MAKsumA ;
HL MAKsumB ;
//Robot.W0_sp.full = 1000;//test !!!!!!!!!!!!!!!!!!!!!!!!!
//Robot.W1_sp.full = 1000;//test !!!!!!!!!!!!!!!!!!!!!!!!!
//Robot.W2_sp.full = 1000;//test !!!!!!!!!!!!!!!!!!!!!!!!!
//Robot.W3_sp.full = 1000;//test !!!!!!!!!!!!!!!!!!!!!!!!!
MAKsumA.full = Robot.W0_sp.byte[high] + Robot.W1_sp.byte[high] + Robot.W2_sp.byte[high] + Robot.W3_sp.byte[high] + Robot.SB_sp
+ Robot.W0_sp.byte[low ] + Robot.W1_sp.byte[low ] + Robot.W2_sp.byte[low ] + Robot.W3_sp.byte[low ] + Robot.orc_length ;
MAKsumB.full = Robot.W0_sp.byte[high]*10 + Robot.W1_sp.byte[high]*9 + Robot.W2_sp.byte[high]*8 + Robot.W3_sp.byte[high]*7 + Robot.SB_sp*6
+ Robot.W0_sp.byte[low ]*5 + Robot.W1_sp.byte[low ]*4 + Robot.W2_sp.byte[low ]*3 + Robot.W3_sp.byte[low ]*2 + Robot.orc_length ;
//in even cases micro puts data on F0 to F6 and clear data_clk pin (F7) to 0 ,so micro puts '0'+'data' on port F
//so there is no need for "CLK_PORT.OUTCLR = CLK_PIN ;"
send_packet[0] = 0b01010101 ; //first start sign
send_packet[2] = 0b01010101 ; //second start sign
send_packet[4] = ( ((Robot.W0_sp.byte[high] & 0x80) >> 7) |
((Robot.W1_sp.byte[high] & 0x80) >> 6) |
((Robot.W2_sp.byte[high] & 0x80) >> 5) |
((Robot.W3_sp.byte[high] & 0x80) >> 4) |
((Robot.SB_sp & 0x80) >> 3) |
((MAKsumA.byte[high] & 0x80) >> 2) |
((MAKsumB.byte[high] & 0x80) >> 1) ) & 0b01111111;
send_packet[6] = ( ((Robot.W0_sp.byte[low] & 0x80) >> 7) |
((Robot.W1_sp.byte[low] & 0x80) >> 6) |
((Robot.W2_sp.byte[low] & 0x80) >> 5) |
((Robot.W3_sp.byte[low] & 0x80) >> 4) |
((Robot.orc_length & 0x80) >> 3) |
((MAKsumA.byte[low] & 0x80) >> 2) |
((MAKsumB.byte[low] & 0x80) >> 1) ) & 0b01111111;
send_packet[8] = Robot.W0_sp.byte[high] & 0b01111111 ;
send_packet[10] = Robot.W1_sp.byte[high] & 0b01111111 ;
send_packet[12] = Robot.W2_sp.byte[high] & 0b01111111 ;
send_packet[14] = Robot.W3_sp.byte[high] & 0b01111111 ;
send_packet[16] = Robot.SB_sp & 0b01111111 ;
send_packet[18] = MAKsumA.byte[high] & 0b01111111 ;
send_packet[20] = MAKsumB.byte[high] & 0b01111111 ;
send_packet[22] = Robot.W0_sp.byte[low] & 0b01111111 ;
send_packet[24] = Robot.W1_sp.byte[low] & 0b01111111 ;
send_packet[26] = Robot.W2_sp.byte[low] & 0b01111111 ;
send_packet[28] = Robot.W3_sp.byte[low] & 0b01111111 ;
send_packet[30] = Robot.orc_length & 0b01111111 ;
send_packet[32] = MAKsumA.byte[low] & 0b01111111 ;
send_packet[34] = MAKsumB.byte[low] & 0b01111111 ;
}This function sends the packet created in inline void data_packing ( void ) and receives a packet from SPARTAN3 (FPGA). Sending in even cases and receicing in odd cases. PIN7 of PORTF ( FPGA_CLK ) is used for clock.
inline void fpga_connection ( void )
{
if (packet_counter % 2 == 0)//sending
{
PORTF_OUT = send_packet[packet_counter] ;
}
else //receiving
{
ioport_set_value(FPGA_CLK, high);//CLK_PORT.OUTSET = CLK_PIN ;
receive_packet[packet_counter] = PORTX_IN ;
}
if (packet_counter == 35)
{
number_of_sent_packet ++ ;
data = unpacking_data ;
}
}This function is the complementary of data_packing () . It generates checksum from received packet and tests whether the packet is correct or not.
inline void data_unpacking (void)
{
//unpacking data from FPGA
//High bytes
temp_data[0].byte[high] = ( receive_packet[9] & 0b01111111 ) | ( ( receive_packet[5] & 0b00000001 ) << 7 ) ;
temp_data[1].byte[high] = ( receive_packet[11] & 0b01111111 ) | ( ( receive_packet[5] & 0b00000010 ) << 6 ) ;
temp_data[2].byte[high] = ( receive_packet[13] & 0b01111111 ) | ( ( receive_packet[5] & 0b00000100 ) << 5 ) ;
temp_data[3].byte[high] = ( receive_packet[15] & 0b01111111 ) | ( ( receive_packet[5] & 0b00001000 ) << 4 ) ;
temp_data[4].byte[high] = ( receive_packet[17] & 0b01111111 ) | ( ( receive_packet[5] & 0b00010000 ) << 3 ) ;
temp_data[5].byte[high] = ( receive_packet[19] & 0b01111111 ) | ( ( receive_packet[5] & 0b00100000 ) << 2 ) ;
temp_data[6].byte[high] = ( receive_packet[21] & 0b01111111 ) | ( ( receive_packet[5] & 0b01000000 ) << 1 ) ;
/* temp_data[7] = ( receive_packet[21] & 0b01111111 ) ;*/
//Low bytes
temp_data[0].byte[low] = ( receive_packet[23] & 0b01111111 ) | ( ( receive_packet[7] & 0b00000001 ) << 7 ) ;
temp_data[1].byte[low] = ( receive_packet[25] & 0b01111111 ) | ( ( receive_packet[7] & 0b00000010 ) << 6 ) ;
temp_data[2].byte[low] = ( receive_packet[27] & 0b01111111 ) | ( ( receive_packet[7] & 0b00000100 ) << 5 ) ;
temp_data[3].byte[low] = ( receive_packet[29] & 0b01111111 ) | ( ( receive_packet[7] & 0b00001000 ) << 4 ) ;
temp_data[4].byte[low] = ( receive_packet[31] & 0b01111111 ) | ( ( receive_packet[7] & 0b00010000 ) << 3 ) ;
temp_data[5].byte[low] = ( receive_packet[33] & 0b01111111 ) | ( ( receive_packet[7] & 0b00100000 ) << 2 ) ;
temp_data[6].byte[low] = ( receive_packet[35] & 0b01111111 ) | ( ( receive_packet[7] & 0b01000000 ) << 1 ) ;
/* temp_data[15] = ( receive_packet[39] & 0b01111111 ) ;*/
//generating check_sum
uint16_t MAKsumA ;
uint16_t MAKsumB;
MAKsumA = temp_data[0].byte[high] + temp_data[1].byte[high] + temp_data[2].byte[high] + temp_data[3].byte[high] + temp_data[4].byte[high] +
temp_data[0].byte[low ] + temp_data[1].byte[low ] + temp_data[2].byte[low ] + temp_data[3].byte[low ] + temp_data[4].byte[low ] ;
MAKsumB = temp_data[0].byte[high]*10 + temp_data[1].byte[high]*9 + temp_data[2].byte[high]*8 + temp_data[3].byte[high]*7 + temp_data[4].byte[high]*6 +
temp_data[0].byte[low ]*5 + temp_data[1].byte[low ]*4 + temp_data[2].byte[low ]*3 + temp_data[3].byte[low ]*2 + temp_data[4].byte[low ] ;
//saving checked data
if( ( MAKsumA == temp_data[5].full ) && ( MAKsumB == temp_data[6].full))
{
Robot.W0.full = temp_data[0].full ;
Robot.W1.full = temp_data[1].full ;
Robot.W2.full = temp_data[2].full ;
Robot.W3.full = temp_data[3].full ;
Robot.SB.full = temp_data[4].full ;
if(MAKsumA == 0 && MAKsumB == 0)
{
if (!(temp_data[0].full == 0 && temp_data[1].full == 0 && temp_data[2].full == 0 && temp_data[3].full == 0 && temp_data[4].full == 0))
{
number_of_received_packet ++ ;
}
}
else
{
number_of_received_packet ++ ;
}
}
}It turns off all motors if :
-
Battery voltage is low
-
There is a over current alarm
-
Wireless is disconnected
-
Current sensors are not calibrated
-
There is an order from wireless board to turn off motors
🔵 1-2-3-4-0-0-0-0 is a sign . FPGA recognizes it and turns off all motors
inline void free_wheel_function ( void )
{
if (free_wheel.low_battery || free_wheel.motor_fault || free_wheel.wireless_timeout || !current_offset_check || (Robot.Vx_sp.full == 258 && Robot.Vy_sp.full == 772))
{
Robot.W0_sp.byte[high] = 1;
Robot.W0_sp.byte[low ] = 2;
Robot.W1_sp.byte[high] = 3;
Robot.W1_sp.byte[low ] = 4;
Robot.W2_sp.byte[high] = 0;
Robot.W2_sp.byte[low ] = 0;
Robot.W3_sp.byte[high] = 0;
Robot.W3_sp.byte[low ] = 0;
}
}These two functions together works like a stopwatch.
//starting counter
inline void Timer_on(void)
{
TCE0_CNT = 0 ;
TCE1_CNT = 0 ;
}
inline void Timer_show (void)
{
cycle_time_us = TCE1_CNT * 1e+3 + TCE0_CNT * 2 ;
cycle_time_s = TCE1_CNT/1000.0 + TCE0_CNT/500000.0 ;
}