void send(char a, int b, int c, int d)
{
m_usb_tx_char(a);
m_usb_tx_uint(b);
m_usb_tx_uint(c);
m_usb_tx_uint(d);
}
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 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
}
}
int main(void)
{
unsigned int value;
m_usb_init();
while(!m_usb_isconnected()); // wait for a connection
while(1) {
if(m_usb_rx_available()) {
value = m_usb_rx_char();
m_usb_tx_uint(value);
}
}
}
// ***********************************
// Main Loop
// ***********************************
int main(void){
setup();
while(1){
wdt_reset(); // Feed the dog!
// ***********************************
// Debug Output
// ***********************************
if(DEBUG){
m_usb_tx_string("CH3 Filt: ");
m_usb_tx_uint(ch3_val);
m_usb_tx_string("\t");
m_usb_tx_string("CH4 Filt: ");
m_usb_tx_uint(ch4_val);
m_usb_tx_string("\t");
m_usb_tx_string("CH5 Filt: ");
m_usb_tx_uint(ch5_val);
m_usb_tx_string("\t");
m_usb_tx_string("Dir Duty: ");
m_usb_tx_uint(ch4_duty);
m_usb_tx_string("\t");
m_usb_tx_string("Th Duty: ");
m_usb_tx_uint(ch3_duty);
m_usb_tx_string("\n");
}
// ***********************************
// Motor PWM
// ***********************************
// (de)Activate motors based on CH5 kill switch
if (ch5_val < CH5_LO_THRESH) { // If kill switch is activated, zero spin
m_usb_tx_string("Kill switch activated! \t");
OCR1B = OFF_OFFSET; // Mapped onto pin B6
OCR1C = OFF_OFFSET; // Mapped onto pin B7
}
else{
// PWM output to motors
ch4_duty = ((ch4_val - CH4_LO) * MAX_DIR) / ch4_range; // Direction [0-MAX_DIR]
ch3_duty = ((ch3_val - CH3_LO) * MAX_TH ) / ch3_range; // Throttle [0-MAX_TH]
// OCR1A counts to 30000
// Set OCR1B & C to move from 1500 to 3000
// Left motor is slightly slow.
// Slightly adjusted for this
left_motor = ch3_duty + 0.65*(ch4_duty - 50) - 8;
right_motor = ch3_duty + 0.65*(50 - ch4_duty);
// Set PWM to new duty cycle, proportional to direction
OCR1B = ON_OFFSET + left_motor; // Mapped onto pin B6
OCR1C = ON_OFFSET + right_motor; // Mapped onto pin B7
}
} // end while
} // end main