int MATLAB_test(int count, ...) {
char rx_buffer;
//see wikipedia article on variadic functions******
va_list ap;
int array[count];
va_start(ap, count);
for (int j= 0; j < count; j++) {
array[j] = va_arg(ap, int);
}
va_end(ap);
//*************
//char rx_buffer;
while(!m_usb_rx_available()); //wait for an indication from the computer
rx_buffer = m_usb_rx_char(); //grab the computer packet
m_usb_rx_flush(); //clear buffer
if(rx_buffer == 1) { //computer wants ir buffer
//write ir buffer as concatenated hex: i.e. f0f1f4f5
for (int i = 0; i < count; i++) {
m_usb_tx_uint(array[i]);
m_usb_tx_char('\t');
}
m_usb_tx_char('\n'); //MATLAB serial command reads 1 line at a time
}
}
void matlab_output( int* position,float* angle, unsigned int* wii_data){
if( m_usb_rx_available() ){
m_usb_tx_int( ( int)position[0] );
m_usb_tx_char('\n');
m_usb_tx_int( ( int)position[1] );
m_usb_tx_char('\n');
m_usb_tx_int( ( int )(1000*angle[0]) );
m_usb_tx_char('\n');
m_usb_tx_uint(wii_data[0]);
m_usb_tx_char('\n');
m_usb_tx_uint(wii_data[1]);
m_usb_tx_char('\n');
m_usb_tx_uint(wii_data[3]);
m_usb_tx_char('\n');
m_usb_tx_uint(wii_data[4]);
m_usb_tx_char('\n');
m_usb_tx_uint(wii_data[6]);
m_usb_tx_char('\n');
m_usb_tx_uint(wii_data[7]);
m_usb_tx_char('\n');
m_usb_tx_uint(wii_data[9]);
m_usb_tx_char('\n');
m_usb_tx_uint(wii_data[10]);
m_usb_tx_char('\n');
m_usb_rx_flush();
}
}
int main(void) {
m_clockdivide(0);
m_bus_init();
setup_timer_3();
m_rf_open(chan,TX_add,p_length);
setup_pins();
if (debug_ADC){ setupUSB();}
long VLeftSum=0;
long HLeftSum=0;
long VRightSum=0;
long HRightSum=0;
// int VLeftSum=0;
// int HLeftSum=0;
// int VRightSum=0;
// int HRightSum=0;
int cnt_RB = 0;
int cnt_LB = 0;
/////////////////////////////////////////////////////////////////////////////////
//
// give me a half second to go from switching on the m2 to holding the config buttons.
//
//////////////////////////////////////////////////////////////////////////////////
m_green(1);
int timer_3_cnt=0;
int Calibration = 50;
for (timer_3_cnt=0 ; timer_3_cnt< Calibration ; ){
if(check(TIFR3,OCF3A)){ ///// timer 3 runs at 100Hz
set(TIFR3,OCF3A );
timer_3_cnt++;
}
}
m_red(ON);
for (timer_3_cnt=0 ; timer_3_cnt<Calibration ; ){
if(check(TIFR3,OCF3A)){ ///// timer 3 runs at 100Hz
set(TIFR3,OCF3A );
timer_3_cnt++;
update_ADC(0,0,0,0);
check_buttons();
if (send_buffer[1]==1){ cnt_RB++ ;}
if (send_buffer[0]==1){ cnt_LB++ ;}
VLeftSum += *(int*)(&send_buffer[2]);
HLeftSum += *(int*)(&send_buffer[6]);
VRightSum += *(int*)(&send_buffer[4]);
HRightSum += *(int*)(&send_buffer[8]);
// if (debug_ADC){while(!m_usb_rx_available()); m_usb_rx_flush();}
if (debug_ADC){debug_ADC_sums( timer_3_cnt, VRightSum, VLeftSum, HLeftSum, HRightSum );}
}
}
// if (debug_ADC){while(!m_usb_rx_available()); m_usb_rx_flush();}
int VLeftOffset = (int)(VLeftSum/Calibration);
int HLeftOffset = (int)(HLeftSum/Calibration);
int VRightOffset= (int)(VRightSum/Calibration);
int HRightOffset= (int)(HRightSum/Calibration);
int VLeftOffset = 512;//(int)(VLeftSum/Calibration);
int HLeftOffset = 512;//(int)(HLeftSum/Calibration);
int VRightOffset= 512;//(int)(VRightSum/Calibration);
int HRightOffset= 512;//(int)(HRightSum/Calibration);
bool L_bump = ( cnt_LB > Calibration/2 );
bool R_bump = ( cnt_RB > Calibration/2 );
if (debug_ADC){while(!m_usb_rx_available()); m_usb_rx_flush();}
if (debug_ADC){debug_ADC_vals( Calibration, VRightSum, VLeftSum, HLeftSum, HRightSum );}
m_green(0);
m_red(OFF);
set_drive_mode( L_bump , R_bump );
while (1){
check_buttons();
update_ADC( VLeftOffset,HLeftOffset,VRightOffset,HRightOffset );
TX_comm();
if (debug_ADC){deal_with_new();}
}
}
