-
Notifications
You must be signed in to change notification settings - Fork 0
function.c
Amin edited this page Jul 22, 2016
·
8 revisions
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_DRclears 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 ;
}It trigs ADC through event Chanel. Then it reads all ADC values
inline void read_all_adc(void)
{
//! To manually trigger adc through event channel (CH0)
EVSYS.DATA = 0x01;
EVSYS.STROBE = 0x01;
adc_temperature = adc_get_result(&ADCA, ADC_CH0) - adc_offset ;
adc_bat = (adc_get_result(&ADCA, ADC_CH1) - adc_offset) / adc_gain ;
adc_m2 = (adc_get_result(&ADCA, ADC_CH2) - adc_offset) / adc_gain ;
adc_m3 = (adc_get_result(&ADCA, ADC_CH3) - adc_offset) / adc_gain ;
adc_m0 = (adc_get_result(&ADCB, ADC_CH0) - adc_offset) / adc_gain ;
adc_m1 = (adc_get_result(&ADCB, ADC_CH1) - adc_offset) / adc_gain ;
adc_bandgap = (adc_get_result(&ADCB, ADC_CH2) - adc_offset) / adc_gain ;
adc_clear_interrupt_flag(&ADCA, ADC_CH0 | ADC_CH1 | ADC_CH2 | ADC_CH3);
adc_clear_interrupt_flag(&ADCB, ADC_CH0 | ADC_CH1 | ADC_CH2);
Robot.MCU_temperature.full = Robot.MCU_temperature.full + (adc_temperature /2 - Robot.MCU_temperature.full) * 0.2 ;
}This function calculate battery voltage and checks it not to drop below 10 V.
inline void battery_voltage_update(void)
{
//! Low pass filter for eliminating noise and impact of current drawing of motors and boost circuit
Robot.bat_v.full = (adc_bat*1.22/.22 - Robot.bat_v.full)*0.001 + Robot.bat_v.full ;// voltage dividing : 1M & 220K
Robot.batx1000.full =Robot.bat_v.full*1000;
if (Robot.bat_v.full < 10.5)
{
ioport_set_value(BUZZER, high);
if (Robot.bat_v.full < 10) free_wheel.low_battery = true ;
}
else
{
ioport_set_value(BUZZER, low);
free_wheel.low_battery = false ;
}
}It's a timer interrupt handler. Every 250 ms collects some information about wireless connection and parallel connection to FPGA.
inline void every_250ms(void)
{
seconds++;
//! Monitoring
Robot.nsp = number_of_sent_packet;
Robot.nrp = number_of_received_packet;
Robot.ss = signal_strength;
number_of_sent_packet = 0;
number_of_received_packet = 0;
signal_strength = 0;
}Outer if statement checks whether it should charge Capacitors or not, if not just a buzzer beeps.
inline void boost_buck_manager(void)
{
if (!bbs.failure && !bbs.dont_charge)
{
//! calculating charging time
if (ioport_get_pin_level(CHARGE_LIMIT))
{
if (bbs.charge_flag)
{
bbs.charge_counter++;
if (bbs.charge_counter>100)
{
bbs.charge_counter = 0 ;
bbs.charge_flag = false ;
Robot.ct = BOOST_BUCK_TIMER ;
BOOST_BUCK_TIMER = 0 ;
}
}
}
if (!bbs.chip_flag && !bbs.kick_flag && !ioport_get_pin_level(CHARGE_LIMIT))
{
CHARGE_PERIOD(300);
CHARGE_DUTY_CYCLE(280);
CHARGE_START;
// TODO it may create a delay before kick or chip
//bbs.charge_flag = true;
if (BOOST_BUCK_TIMER > MAX_CHARGING_TIME && bbs.charge_flag == true)
{
bbs.failure = true ;
}
}
else
{
CHARGE_STOP;
//! Kick
if (((Robot.KICK >0 && Robot.KICK <= 100) || ioport_get_pin_level(BIG_BUTTON)) && !bbs.kick_flag && !bbs.chip_flag && !bbs.charge_flag)
{
KICK_PERIOD(100);
if (ioport_get_pin_level(BIG_BUTTON))
{
KICK_DUTY_CYCLE(100);
bbs.lko = button_kick;
}
else
{
KICK_DUTY_CYCLE(Robot.KICK);
bbs.lko = kick ;
}
KICK_START;
BOOST_BUCK_TIMER = 0;
bbs.kick_flag = true;
}
if (bbs.kick_flag && (BOOST_BUCK_TIMER > KICK_TIME_LIMIT))
{
KICK_STOP;
BOOST_BUCK_TIMER = 0;
bbs.kick_flag = false;
bbs.charge_flag = true;
if (bbs.lko == button_kick)
{
bbs.dont_charge = true;
}
}
//! Chip
if (Robot.CHIP && !bbs.kick_flag && !bbs.chip_flag && !bbs.charge_flag)
{
CHIP_PERIOD(123);
CHIP_DUTY_CYCLE(123);
CHIP_START;
BOOST_BUCK_TIMER = 0;
bbs.chip_flag = true;
}
if (bbs.chip_flag && (BOOST_BUCK_TIMER > CHIP_TIME_LIMIT))
{
CHIP_STOP;
BOOST_BUCK_TIMER = 0;
bbs.chip_flag = false;
bbs.charge_flag = true;
}
}
}
else
{
CHARGE_STOP;
KICK_STOP;
CHIP_STOP;
bbs.charge_flag = false;
if (BOOST_BUCK_TIMER > 1000)
{
ioport_toggle_pin(BUZZER);
BOOST_BUCK_TIMER = 0;
}
}
}Otherwise next if statement executes. It measures charging time.
if (ioport_get_pin_level(CHARGE_LIMIT))
{
if (bbs.charge_flag)
{
bbs.charge_counter++;
if (bbs.charge_counter>100)
{
bbs.charge_counter = 0 ;
bbs.charge_flag = false ;
Robot.ct = BOOST_BUCK_TIMER ;
BOOST_BUCK_TIMER = 0 ;
}
}
}
```
Then there is a **if-else** statement which in **if** it manages charging and checkouts probable fault. And in **else** statement it manages kicking and chipping.
## inline void motors_current_check(void)
This function calculates motors current from read voltages of current sensors. It also checks over-current in motors.
```c
inline void motors_current_check(void)
{
if (current_offset_check)
{
//currents
// voltage dividing : 560k & 470k => (560 + 470) / 560 = 103 / 56
// Current sensor : 185 mV/A output sensitivity
// 103 / 56 / 0.185 = 9.942084942
Robot.I0.full = ((adc_m0 - adc_m0_offset)* 9.942084942 - Robot.I0.full)*0.1 + Robot.I0.full;
Robot.I1.full = ((adc_m1 - adc_m1_offset)* 9.942084942 - Robot.I1.full)*0.1 + Robot.I1.full;
Robot.I2.full = ((adc_m2 - adc_m2_offset)* 9.942084942 - Robot.I2.full)*0.1 + Robot.I2.full;
Robot.I3.full = ((adc_m3 - adc_m3_offset)* 9.942084942 - Robot.I3.full)*0.1 + Robot.I3.full;
// TODO amazing and strange result in comparison with current-sensor's result
// i_m=(V-V_emf)/R=(V-?_m/k_n )/R : another way of calculating current
// i_model_M0 = (float) (u[0][0] - Robot.W0.full*N/ kn) / res ;
// i_model_M1 = (float) (u[1][0] - Robot.W1.full*N/ kn) / res ;
// i_model_M2 = (float) (u[2][0] - Robot.W2.full*N/ kn) / res ;
// i_model_M3 = (float) (u[3][0] - Robot.W3.full*N/ kn) / res ;
if ( fabs(Robot.I0.full)>3.5) Robot.W0_warning += fabs(Robot.I0.full) * 5;
else if(Robot.W0_warning) Robot.W0_warning --;
if ( fabs(Robot.I1.full)>3.5) Robot.W1_warning += fabs(Robot.I1.full) * 5;
else if(Robot.W1_warning) Robot.W1_warning --;
if ( fabs(Robot.I2.full)>3.5) Robot.W2_warning += fabs(Robot.I2.full) * 5;
else if(Robot.W2_warning) Robot.W2_warning --;
if ( fabs(Robot.I3.full)>3.5) Robot.W3_warning += fabs(Robot.I3.full) * 5;
else if(Robot.W3_warning) Robot.W3_warning --;
if(Robot.W0_warning > 20000 || Robot.W1_warning > 20000 || Robot.W2_warning > 20000 || Robot.W3_warning > 20000)
{
free_wheel.motor_fault = true;
}
}
}
```