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 debug_ADC_sums(int timer_3_cnt, long VRightSum,long VLeftSum, long HLeftSum, long HRightSum ){
m_usb_tx_int(timer_3_cnt);
m_usb_tx_string(" me \t");
m_usb_tx_long(VRightSum);
m_usb_tx_char('\t');
m_usb_tx_long(HRightSum);
m_usb_tx_char('\t');
m_usb_tx_long(VLeftSum);
m_usb_tx_char('\t');
m_usb_tx_long(HLeftSum);
m_usb_tx_string("\t\n\r");
}
void doUSB(void)
{
m_usb_tx_string("\n");
// if(m_usb_isconnected())
// {
// m_red(ON);
//m_usb_tx_char(97);
// m_usb_tx_int(debugVar);
// m_usb_tx_char(32);
// int i;for (i=0;i<12;i++)
// {
// m_usb_tx_uint(blobMemAddr[i]);
// m_usb_tx_char(32);
// }
if (currentTeam == RED) {
m_usb_tx_string("RED ");
} else if (currentTeam == BLUE) {
m_usb_tx_string("BLUE ");
} else {
m_usb_tx_string("NO TEAM! ");
}
// m_usb_tx_char(40);
// m_usb_tx_int(robotX);
// m_usb_tx_char(44);
// m_usb_tx_int(robotY);
// m_usb_tx_char(41);
m_usb_tx_char(32);
m_usb_tx_char(60);
m_usb_tx_int(angleOfRobot); // (X,Y) <ANGLE
m_usb_tx_char(32);
m_usb_tx_string("DUTY: ");
m_usb_tx_int(MOTOR_TIMER_OCR_R);
m_usb_tx_int(MOTOR_TIMER_OCR_L);
m_usb_tx_int(motorDutyL);
m_usb_tx_char(32);
m_usb_tx_int(motorDutyR);
m_usb_tx_string("ANGLE_TO_ENEMY_GOAL:");
m_usb_tx_int(angleToEnemyGoal);
m_usb_tx_string("\n");
// m_usb_tx_uint(robotY);
// m_usb_tx_int( angleOfRobot);
// }
//****************************
m_usb_tx_string(" ROBOT ANGLE: ");m_usb_tx_int(angleOfRobot);m_usb_tx_char(32);
m_usb_tx_string(" THETA1: ");m_usb_tx_int(angleToRobot);m_usb_tx_char(32);
m_usb_tx_string(" THETA2: ");m_usb_tx_int(theta2);m_usb_tx_char(32);
m_usb_tx_string("ROBOT COORDS: (");m_usb_tx_int(robotX);m_usb_tx_char(32);
m_usb_tx_int(robotY);m_usb_tx_string(")");m_usb_tx_char(32);
}
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);
}
int main(void)
{
m_clockdivide(0);
unsigned char accel_scale = 0;
unsigned char gyro_scale = 0;
int data[9] = {0,0,0,0,0,0,0,0,0};
m_usb_init();
m_imu_init(accel_scale, gyro_scale);
while(1)
{
m_wait(10);
m_imu_raw(data);
m_usb_tx_int(data[0]);
m_usb_tx_char('\t');
m_usb_tx_int(data[1]);
m_usb_tx_char('\t');
m_usb_tx_int(data[2]);
m_usb_tx_char('\t');
m_usb_tx_int(data[3]);
m_usb_tx_char('\t');
m_usb_tx_int(data[4]);
m_usb_tx_char('\t');
m_usb_tx_int(data[5]);
m_usb_tx_char('\n');
m_usb_tx_char('\r');
}
}
void debug_ADC_vals( int Calibration, long VRightSum,long VLeftSum, long HLeftSum, long HRightSum ){
int VLeftOffset = (int)(VLeftSum/Calibration);
int HLeftOffset = (int)(HLeftSum/Calibration);
int VRightOffset= (int)(VRightSum/Calibration);
int HRightOffset= (int)(HRightSum/Calibration);
m_usb_tx_string("VLeftOffset\t");
m_usb_tx_int(VLeftOffset);
m_usb_tx_char('\t');
m_usb_tx_string("HLeftOffset\t");
m_usb_tx_int(HLeftOffset);
m_usb_tx_char('\t');
m_usb_tx_string("VRightOffset\t");
m_usb_tx_int(VRightOffset);
m_usb_tx_char('\t');
m_usb_tx_string("LRightOffset\t");
m_usb_tx_int(HRightOffset);
m_usb_tx_char('\t');
}
int main(void){
m_clockdivide(0);
int data[9] = {0,0,0,0,0,0,0,0,0};
m_green(ON);
m_bus_init();
m_usb_init();
while(!m_usb_isconnected());
m_green(OFF);
m_imu_init(accel_scale, gyro_scale);
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
while(1) {
while ( !newPacket );
int i,a;
while (i=0; i<PACKET_LENGTH , i+=2 ){
a = *(int*)&packet[i*2+2];
m_usb_tx_int(a);
m_usb_tx_char('\t');
}
m_usb_tx_char('\n');
m_usb_tx_char('\r');
}
}
int main(void)
{
robockey_init();
while(1)
{
comm(command);
mode = check_mode();
if(wii_flag && command == PLAY)
{
location_flag = localization(blobs, calibration, location);
wii_flag = 0;
}
switch(mode)
{
case 0: // normal mode(GOAL A), need command PLAY TO START
if(switch_flag)
{
goal[0] = -125;
goal[1] = 0;
goal[2] = 0;
if(command == HALFTIME)
{
goal[0] = 125;
switch_flag = 0;
}
}
if(command == PLAY)
{
//if(!found)
//{
//found = find(adc1, adc2, &count);
//set(TIMSK0,OCIE0A); // Enable timer0 interrupt
//}
if(find(adc1, adc2, &count))
{
m_green(TOGGLE);
m_red(OFF);
move(location, goal);
}
}
break;
case 1: // normal mode(GOAL B), need command PLAY TO START
if(switch_flag)
{
goal[0] = 125;
goal[1] = 0;
goal[2] = 0;
if(command == HALFTIME)
{
goal[0] = -125;
switch_flag = 0;
}
}
if(command == PLAY)
{
if(find(adc1, adc2, &count))
{
m_green(TOGGLE);
m_red(OFF);
move(location, goal);
}
}
break;
case 2: // calibrate the center of the rink
m_green(ON);
m_red(ON);
if(wii_flag)
{
float calibrationDefault[2] = {0, 0};
float calibrationResult[3] = {0, 0, 0};
if(localization(blobs, calibrationDefault, calibrationResult))
{
//m_green(ON);
//m_red(ON);
calibration[0] = calibrationResult[0];
calibration[1] = calibrationResult[1];
}
}
break;
case 3: // test mode(GOAL A), always play while the power is on.
m_green(TOGGLE);
m_red(OFF);
if(find(adc1, adc2, &count))
{
if(wii_flag)
{
//if(!found)
//{
//found = find(adc1, adc2, &count);
//set(TIMSK0,OCIE0A); // Enable interrupt
//}
if(localization(blobs, calibration, location))
{
float goal[3] = {-115, 0, 0};
move(location, goal);
}
}
}
break;
case 4: // test mode(GOAL B), always play while the power is on.
m_red(TOGGLE);
m_green(OFF);
if(find(adc1, adc2, &count))
{
if(wii_flag)
{
if(localization(blobs, calibration, location))
{
float goal[3] = {115, 0, 0};
move(location, goal);
}
}
}
break;
case 5:
m_green(ON);
m_red(ON);
OCR1B = OCR1A;
OCR1C = 0.98*OCR1A;
break;
case 6:
qualify_test(&command, blobs, location, &orientation_flag, &west_flag, &east_flag, calibration);
break;
case 7:
OCR1B = OCR1A;
OCR1C = 0;
m_wait(3200);
OCR1B = OCR1A/2;
OCR1C = OCR1A/2;
m_wait(32000);
break;
default:
OCR1B = 0.5*OCR1A;
OCR1C = 0.5*OCR1A;
m_green(OFF);
m_red(OFF);
}
m_usb_tx_int((int)(location[0]));
m_usb_tx_char('\t');
m_usb_tx_int((int)(location[1]));
m_usb_tx_char('\t');
m_usb_tx_int((int)(adc1[0]));
m_usb_tx_char('\t');
m_usb_tx_int((int)(adc2[0]));
m_usb_tx_char('\t');
m_usb_tx_int((int)(check(PIND, 7)));
m_usb_tx_char('\t');
m_usb_tx_char('\r');
if(!check(ADCSRA, ADIF)) set(ADCSRA,ADSC);
}
}
void reportADC() {
m_red(ON);
m_green(ON);
m_red(OFF);
m_green(OFF);
char rx_buffer; //computer interactions
int index = 0;
int maxval = 0;
float deg = 0.0;
float diff = 0.0;
getADC();
index = 0;
maxval = 0;
//m_green(TOGGLE);
for(int i = 0; i < 8; i++) {
if (ADCarr[i] > maxval) {
index = i;
maxval = ADCarr[i];
}
}
switch (index) {
case 0:
diff = ADCarr[0] - ADCarr[6];
deg = exp(-1.0*fabs(((float)diff))/400.0);
//m_green(ON);
//m_red(OFF);
break;
case 6:
diff = ADCarr[6] - ADCarr[0];
deg = exp(-1.0*fabs(((float)diff))/400.0);
//m_green(OFF);
//m_red(ON);
break;
}
//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
m_usb_tx_int(ADCarr[0]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[1]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[2]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[3]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[4]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[5]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[6]);
m_usb_tx_char('\t');
m_usb_tx_int(ADCarr[7]);
m_usb_tx_char('\t');
m_usb_tx_int(index);
m_usb_tx_char('\t');
m_usb_tx_int((int)(deg*100));
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();
}
}
