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");
}
Beispiel #2
0
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');
    }
}
Beispiel #3
0
void get_command(){

	comm_cmd = false;
	state_requested = buffer[0];
	if (debug_com){
		m_usb_tx_string("state requested:\t");
		m_usb_tx_int(state_requested);
		m_usb_tx_string("\n\r");
	}
	m_red(OFF);
	clear(PORTB,1);
	clear(PORTB,2);
	clear(PORTB,3);

	if(state_requested == commTest){ 
			set(PORTB,1);	
	}
	else if(state_requested == play){ 
			set(PORTB,2);	
	}
	else if(state_requested == goalA){ 
			set(PORTB,1);	
			set(PORTB,2);	
	}
	else if(state_requested == goalB){ 
			set(PORTB,3);	
	}
	else if(state_requested == pause){ 
			set(PORTB,3);	
			set(PORTB,1);	
	}
	else if(state_requested == halfTime){ 
			set(PORTB,3);	
			set(PORTB,2);	
	}
	else if(state_requested == gameOver){ 
			set(PORTB,3);	
			set(PORTB,2);	
			set(PORTB,1);	
	} else {
		m_red(ON);
	}
}
Beispiel #4
0
void select_goal(void) 
{
	/* Assign Defending goal */
	update_position();
	
	float position_buffer[3];
	get_position(position_buffer);
	m_usb_tx_int((int)position_buffer[0]);
	
	if (position_buffer[0]>0) {
		x_goal = -1*GOAL_X_DIST;
		goal = BLUE;
		//positioning_LED(RED);
		} else {
		x_goal = GOAL_X_DIST;
		goal = RED;
		//positioning_LED(BLUE);
	}
}
Beispiel #5
0
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');
	}
}
Beispiel #6
0
void position_demo()
{

	m_usb_init();
	

	set(DDRD,5);
	//init mWii
	char wii_status = m_wii_open();
	if(wii_status){
		m_green(ON);
	    set(PORTD,5);
	}else
	m_red(ON);

	
	while(!m_usb_isconnected())
	{
		m_green(ON);
	} 
  unsigned int blobs[] = {541,381,0,503,344,0,524,304,0,599,347,0};
  int blob_status = 0;


	while(1)
	{
		double x;
		double y;
		double theta;
    if(m_usb_isconnected()){
	  blob_status = m_wii_read(blobs);

	  if(blob_status){
      set_position(1024/2,768/2);
      

      get_position(blobs,&x,&y,&theta);
		  
		
      /*
      m_usb_tx_string("x: ");
      m_usb_tx_int((int)(x * 100));
      m_usb_tx_string("\n");
      m_usb_tx_string("y: ");
      m_usb_tx_int((int)(y * 100));
      m_usb_tx_string("\n");
      m_usb_tx_string("theta: ");
      m_usb_tx_int((int)(theta * 100));
      m_usb_tx_string("\n");

      m_usb_tx_string("blob1x: ");
      m_usb_tx_int((int) blobs[0]);
 
      m_usb_tx_string("blob1y: ");
      m_usb_tx_int((int) blobs[1]);
      m_usb_tx_string("\n");
      m_usb_tx_string("blob2x: ");
      m_usb_tx_int((int) blobs[3]);
 
      m_usb_tx_string("blob2y: ");
      m_usb_tx_int((int) blobs[4]);
      m_usb_tx_string("\n");
      m_usb_tx_string("blob3x: ");
      m_usb_tx_int((int) blobs[6]);
 
      m_usb_tx_string("blob3y: ");
      m_usb_tx_int((int) blobs[7]);
      m_usb_tx_string("\n");
      m_usb_tx_string("blob4x: ");
      m_usb_tx_int((int) blobs[9]);

      m_usb_tx_string("blob4y: ");
      m_usb_tx_int((int) blobs[10]);
      m_usb_tx_string("\n");

	  */
      m_usb_tx_int((int)(x * 100));
      m_usb_tx_int((int)(y * 100));
      m_usb_tx_int((int)(theta * 100));
	  m_usb_tx_int((int) blobs[0]);
	  m_usb_tx_int((int) blobs[1]);
	  m_usb_tx_int((int) blobs[3]);
	  m_usb_tx_int((int) blobs[4]);
	  m_usb_tx_int((int) blobs[6]);
	  m_usb_tx_int((int) blobs[7]);
	  m_usb_tx_int((int) blobs[9]);
	  m_usb_tx_int((int) blobs[10]);



   		m_wait(1000);
		  m_red(TOGGLE);
	  }
    }

	}

}
int main(void)
{	m_clockdivide(0);
	m_bus_init();
	m_usb_init();
	m_rf_open(CHANNEL,RXADDRESS,LENGTH);
	m_wii_open();
	sei();
	
	OCR1A = 31999;
	OCR1B = 0;
	OCR1C = 0;
	
	m_disableJTAG();

	
	clear(ADMUX,REFS1); // Setting the reference voltage for the ADC to Vcc
	set(ADMUX,REFS0); // ^
	
	set(ADCSRA,ADPS2); // set ADC prescaler to 128
	set(ADCSRA,ADPS1); // ^
	set(ADCSRA,ADPS0); //^
	
	set(DIDR0,ADC0D); // disable digital on F0 (ADC0)
	set(DIDR0,ADC1D); // disable digital to F1
	set(DIDR0,ADC4D); //disable digital to F4
	set(DIDR0,ADC5D); //disable digital to F5
	set(DIDR0,ADC6D); //disable digital to F6
	set(DIDR0,ADC7D); //disable digital to F7
	
	set(ADMUX,REFS0); // set ADC reference to Vcc
	
	set(ADCSRA,ADIE); //Enables interrups when conversion finishes
	//	clear(ADMUX,MUX2); // This is the front right. Range 50-1023. Port is F1
	//	set(ADMUX,MUX0);
	
	set(TCCR1B,WGM13);          // Mode 15: up to OCR1A, PWM, single slope
	set(TCCR1B,WGM12);          // ^
	set(TCCR1A,WGM11);          // ^
	set(TCCR1A,WGM10);          // ^
	
	set(TCCR1A,COM1B1);			//clear at OCR1B, set at rollover
	clear(TCCR1A,COM1B0);			// ^
	
	set(TCCR1A,COM1C1); // clear at OCR1C, set at rollover
	clear(TCCR1A,COM1C0);
	
	clear(TCCR1B,CS12);       // set clock pre-scaler to /1
	clear(TCCR1B,CS11);          // ^
	set(TCCR1B,CS10);         // ^
	
	
	m_port_init(0x20); // Initializes the port expander and sets all of the G pins as outputs
	m_port_set(0x20, DDRG,0);	
	m_port_set(0x20, DDRG,1);
	m_port_set(0x20, DDRG,2);
	m_port_set(0x20, DDRG,3);
	m_port_set(0x20, DDRG,4);
	m_port_set(0x20, DDRG,5);
	m_port_set(0x20, DDRG,6);
	m_port_set(0x20, DDRG,7);
	
	clearDisplay();
	// Set motor output ports
	set(DDRB,6);
	set(DDRB,7);
	set(DDRB,5);
	set(DDRB,3);
	
	// Turn off motors
	clear(PORTB,6);
	clear(PORTB,7);
	
	set(ADCSRA,ADEN); // enable ADC subsystem
	set(ADCSRA,ADSC); // start ADC conversion
	
	
	while(1){
		switch(state){
			case COMM:
				commtestf();
				break;
			case PAUSE:
				pausef();
				break;
			case PLAY:
				playf();
				localizef();
				defendf();
				//puckfindf();
				break;
			case GOTOGOAL:
				gotogoalf();
			case HALFTIME:
				pausef();
				break;
			case GOALA:
				pausef();
				break;
			case GOALB:
				pausef();
				break;
			case GAMEOVER:
				pausef();
				break;
			default:
				break;
		}
		defendf();
		m_usb_tx_int(photo1); 
		m_usb_tx_string(" ");
		m_usb_tx_int(photo2);
		m_usb_tx_string(" ");
		m_usb_tx_int(photo3);
		m_usb_tx_string(" ");
		m_usb_tx_int(photo4);
		m_usb_tx_string(" ");
		m_usb_tx_int(photo5);
		m_usb_tx_string("\n");
		
	}
}	
Beispiel #8
0
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);

}
Beispiel #9
0
int main(void)
{
    m_clockdivide(0); // clock speed 16 MHz
    sei(); // enable global interrups
    int derivative = 0;
    
    // initialize m_bus communications
    // start wireless communications
    // and open wii camera
    m_bus_init();
    m_wii_open();
    m_usb_init();
    m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
    
    setup();    // motor setup
    
    int side;   // integer to figure out which side we're on
    
    while (1)
    {
        // USB print testing
        m_usb_tx_string("testing: ");
        m_usb_tx_int(testing);
        m_usb_tx_string("   ");
        m_usb_tx_string("\n");
        
        // continue code if ply command is issued
        if ((unsigned char)buffer1[0] == 0xA1){
            break;
        }
    }
    
    // turn red light on if play command is issued
    m_red(ON);
    
    // wait 10 seconds to let M_Wii start recording stars
    m_wait(1000);
    
    int position[3];        // array for robot position
    int x1;                 // x position
    int i;
    
    // calculate initial positions
    for(i = 0; i<1000; i++)
    {
        current_location(position);
        x1 = position[0];
    }
    
    // figure out what side robot starts on
    if (x1 > 0)
    {
        side = -1;
    }
    else
    {
        side = 1;
    }
    
    // array to hold constants
    float constants[4];
    //goal();
    
    // main LOOP
    while(1)
    {
        // calculate positions
        int b = current_location(position);
        int x1 = position[0];
        int y1 = position[1];
        int angle = position[2];
        
        // usb prinitng
        m_usb_tx_string("LOCALIZATION: ");
        m_usb_tx_int(b);
        m_usb_tx_string("   ");
        m_usb_tx_string("X Position: ");
        m_usb_tx_int(x1);
        m_usb_tx_string("   ");
        m_usb_tx_string("Y Position: ");
        m_usb_tx_int(y1);
        m_usb_tx_string("   ");
        m_usb_tx_string("Angle: ");
        m_usb_tx_int(angle);
        m_usb_tx_string("   ");
        m_usb_tx_string("\n");
        
        // get constants
        if (side>0){
            get_constants1(constants, -115, 0, x1, y1, angle, side, derivative);
        }
        else
        {
            get_constants1(constants, 115, 0, x1, y1, angle, side, derivative);
        }
        
        
        float p = constants[0]; // speed constant
        int d = (int)constants[1]; // direction constant
        float j = constants[2]; // how much turn constant
        derivative = constants[3];
        
        turn_robot( d, 1, .9, j); // DRIVE THE BOT
    }
}
Beispiel #10
0
int main(void)
{
    m_clockdivide(0);
    Timer3_init();//timer for setting delta t for angle value
     timer1_init();
    m_usb_init();
    m_imu_init(2, 3);
    sei();
    //for motor control-testing
    while(1)
    {
        if (update_angle) {
            
            m_imu_raw(data);
            Ax = (float)data[0]*g/accel_scale;
            Ay = (float)data[1]*g/accel_scale;
            Az = (float)data[2]*g/accel_scale;
            Gx = (float)data[3]/gyro_scale;
            Gy = (float)data[4]/gyro_scale;
            Gz = (float)data[5]/gyro_scale;
           // Mx = (float)data[6];
           // My = (float)data[7];
           // Mz = (float)data[8];
            
            
            //angle calculation
            theta_Ay = (float)Ay*90/g;
            angle = alpha*(angle_old + Gx*timestep*5)+ (1-alpha)*theta_Ay;
            angle_old = angle;
            delta_angle = angle_old + Gx*timestep*5;
            integral_angle += angle;
            //PID
            //output = (Kp*angle) + Kd*(angle - angle_old)*timestep + (Ki*integral_angle*timestep) ;
            output = Kp*angle + Kd*Gx*timestep + Ki*integral_angle*timestep;
            
            m_usb_tx_string("\t Ax: ");
            m_usb_tx_int(Ax);
            m_usb_tx_string("\t Ay: ");
            m_usb_tx_int(Ay);
            m_usb_tx_string("\t Az: ");
            m_usb_tx_int(Az);
            m_usb_tx_string("\t Gx: ");
            m_usb_tx_int(Gx);
            m_usb_tx_string("\t Gy: ");
            m_usb_tx_int(Gy);
            m_usb_tx_string("\t Gz: ");
            m_usb_tx_int(Gz);
            m_usb_tx_string("\t angle_Gx: ");
            m_usb_tx_int(angle);
            m_usb_tx_string("\t output: ");
            m_usb_tx_int(output);
     
            m_usb_tx_string("\n");
            update_angle = 0;
        }
        
        //PID Control
      // 
        
        //pwm , sensor , whell rotation spd, fbk loop, pwm generate;
        //pseudo code to control motor
        //      initialise timer for motor here, and set OCR1A,OCR1B,OCR1C value based on the speed u want to control the bot.
         //                  go slow or fast, angle less, slow, angle more faster
                
                
                if (angle > base_angle){//>
                    //move forward
                    set(DDRB,6);//set Port B pin 6 for output
                    clear(PORTB,6); //clear Port B pin 6}
                    clear(DDRB,7);
                    flag_foward = 1;
                    flag_back = 0;
                                    }
                if (angle < -base_angle){
                    //move backwards
                    set(DDRB,7);//set Port B pin 7 for output
                    clear(PORTB,7); //clear Port B pin 7
                    clear(DDRB,6);
                    flag_back = 1;
                    flag_foward = 0;
                }
        //timer1_init();//testing fr working
        timer_update();
    }
}
Beispiel #11
0
int main(void)
{
m_clockdivide(3); //2MHz

//Timer
clear(TCCR1B,CS12);
set(TCCR1B,CS11);
clear(TCCR1B,CS10); //8 precale timer

set(TCCR1B,WGM13);
set(TCCR1B,WGM12);
set(TCCR1A,WGM11);
set(TCCR1A,WGM10);

set(TCCR1A,COM1B1);
clear(TCCR1A,COM1B0);

m_usb_init();

m_imu_init(0,0);

OCR1A = 250;


//read=1;

while(1){

read = m_imu_raw(data);

ax = data[0];
ay = data[1];
az = data[2];
gx = data[3];
gy = data[4];
gz = data[5];

gx_filtered=gx_filtered_prev+gx*dt;
gx_filtered_latest = 0.99*(gx_filtered_old + (gx_filtered-gx_filtered_prev));
//gx_previous = gx;
//gx = gx_previous

//ay_filtered = 0.99*ay_filtered + 0.01*ay;

//a = pow((pow(ax,2) + pow(ay,2) + pow(az,2)),0.5);
//g = pow((pow(gx,2) + pow(gy,2) + pow(gz,2)),0.5);

angle = 0.98*(angle+(gx_filtered*dt))+0.02*ay;


// ay_refined = (ay - ay_offset)*ay_scale;

error = target_angle-angle;
az = (kp * error) + (kd*(error-error_last)/dt) + (ki*sum_error*dt);
OCR1B = abs((kp * error) + (kd*(error-error_last)/dt) + (ki*sum_error*dt))*250/15000;
error_last = error;
sum_error = sum_error + error;
move = OCR1B;


if (angle>0) {
m_red(OFF);
//OCR1B = 65535 - (65535/12500)*abs(move);

clear(DDRB,1);
clear(PORTB,1);

set(DDRB,2);
set(PORTB,2);

set(DDRD,4);
set(PORTD,4);

clear(DDRD,6);
clear(PORTD,6);
} else {
m_red(ON);
//OCR1B = 65535 - (65535/12500)*abs(move);


set(DDRB,1);
set(PORTB,1);

clear(DDRB,2);
clear(PORTB,2);


set(DDRD,6);
set(PORTD,6);

clear(DDRD,4);
clear(PORTD,4);
}

m_usb_tx_string("error\t");
m_usb_tx_int(error);


//        m_usb_tx_string("\tax\t");
//        m_usb_tx_int(data[0]);
//        m_usb_tx_string("\tay\t");
//        m_usb_tx_int(data[1]);
//        m_usb_tx_string("\taz\t");
//        m_usb_tx_int(data[2]);
//        m_usb_tx_string("\tgx\t");
//        m_usb_tx_int(data[3]);
//        m_usb_tx_string("\tgy\t");
//        m_usb_tx_int(data[4]);
m_usb_tx_string("\toffset\t");
m_usb_tx_int(gz);
//        m_usb_tx_string("\ta\t");
//        m_usb_tx_int(a);
//        m_usb_tx_string("\tg\t");
//        m_usb_tx_int(g);
m_usb_tx_string("\tmove\t");
m_usb_tx_int(move);
m_usb_tx_string("\tgx_filtered\t");
m_usb_tx_int(gx_filtered);
m_usb_tx_string("\n");


}
}
Beispiel #12
0
int main(void)
{
    init();                             // Run Timer Initializacion Code
    m_bus_init();
	m_usb_init();                       // Initialize the USB connection.
    m_rf_open(1, 0x15, 3);
    set(DDRB, 0);
    set(DDRB, 1);
    set(DDRB, 2);
    set(DDRB, 3);
    set(DDRB, 7);
    set(DDRD, 3);
    set(DDRD, 4);
    set(DDRD, 5);
    set(DDRD, 6);
    set(DDRD, 7);
    
    //while(!m_usb_isconnected());      // wait for a connection

    while(1){
        
        if(m_usb_rx_available())
        {
            input = m_usb_rx_char();
            //m_usb_tx_int(input);
            m_usb_tx_string(" ");
            if (input == 119) {
                OCR3A = OCR3A - 1;
            }
            //if (input == 115) {
            //    OCR3A = OCR3A + 1;
            //}
            //if (input == 105) {
            //    OCR0A = OCR0A - 1;
            //}
            if (input == 107) {
                OCR0A = OCR0A + 1;
            }
            if (input == 105) {
                m_red(TOGGLE);
                //des_freq = 100;
                OCR3A = (float)62500/des_freq;
                duration = 100;
                flag_2 = 1;
                flag = 0;
                ;
            }
            if (input == 115) {
                des_freq += 10;
            }
            m_usb_tx_int(des_freq);
        }
        
        if (flag) {
            m_red(TOGGLE);
            m_rf_read(commands,3);
            des_freq = *(int*)&commands[0];
            des_freq /= 10;
            // Measured: 130 is 60 Hz. 20 is 370 Hz. THIS IS INT.
            OCR3A = 130 - (float)(des_freq - 60)/(float)2.8;
            duration = commands[2];
            flag_2 = 1;
            flag = 0;
        }
        
        // Max Sound duration is 255 centiseconds, which is 2.55 sec.
        if (flag_2) {
            // TCNT0 = 0;
            conta = 0;
            m_green(ON);
            set(DDRB, 6);
            clear(PORTD, 4);
            flag_2 = 0;
        }
        if (conta == (4*duration)) {
            m_green(OFF);
            clear(DDRB, 6);
            clear(PORTB, 6);
            set(PORTD, 4);
        }
        
        //if (TCNT0 == OCR0A) {
        if (check(TIFR0, OCF0A)){
            conta += 1;
            set(TIFR0,OCF0A);
        }
        
        /*
        if (check(ADCSRA,ADIF)) {
            set(ADCSRA, ADIF);
            if (ADC>100) {
                set(PORTB, 0);
            }
            else{
                clear(PORTB, 0);
            }
            if (ADC>200) {
                set(PORTB, 1);
            }
            else{
                clear(PORTB, 1);
            }
            if (ADC>300) {
                set(PORTB, 2);
            }
            else{
                clear(PORTB, 2);
            }
            if (ADC>400) {
                set(PORTB, 3);
            }
            else{
                clear(PORTB, 3);
            }
            if (ADC>500) {
                set(PORTB, 7);
            }
            else{
                clear(PORTB, 7);
            }
            if (ADC>600) {
                set(PORTD, 3);
            }
            else{
                clear(PORTD, 3);
            }
            if (ADC>700) {
                set(PORTD, 4);
            }
            else{
                clear(PORTD, 4);
            }
            if (ADC>800) {
                set(PORTD, 5);
            }
            else{
                clear(PORTD, 5);
            }
            if (ADC>900) {
                set(PORTD, 6);
            }
            else{
                clear(PORTD, 6);
            }
            if (ADC>1000) {
                set(PORTD, 7);
            }
            else{
                clear(PORTD, 7);
            }
        }
        */
        
        //m_usb_tx_string("\r");      // Carriage Return
        //m_usb_tx_uint(ADC);
        
        //m_usb_tx_string("\r");      // Carriage Return
        //m_usb_tx_hex(commands[0]);
        //m_usb_tx_uint(des_freq);
        //m_usb_tx_string(" ");
        //m_usb_tx_char(commands[2]);
        //m_usb_tx_string(" OCR3A: ");
        //m_usb_tx_uint(OCR3A);
        //m_usb_tx_string("             ");
        
        // Measured: 0x81 (129, we can use 130) is 60 Hz. 46 (70) is 110 Hz.
        // I get values from 60 to 110
        
        
        
        // Do only every time the Timer Overflows
        //while(!check(TIFR3, OCF3A));
        if (check(TIFR3, OCF3A)) {
            if(indx<255){
                indx += 1;
            }
            else{
                indx = 0;
            }
            //Define new value for Duty Cycle According to Sine Table
            OCR1B = sinewave_data[indx];
            set(TIFR3, OCF3A);              //Clear Flag
        }
        
        

        
        // Display information for the SCREEN.
       /*
        m_usb_tx_string("ADC Input Value: ");      //
        m_usb_tx_uint(ADC);         //transmit over USB
        m_usb_tx_string("    DIP Switch Inputs:");      //
        //m_usb_tx_uint(check(PIND,4));
        //m_usb_tx_string("  "); m_usb_tx_uint(check(PIND,5));
        //m_usb_tx_string("  "); m_usb_tx_uint(check(PIND,6));
        //m_usb_tx_string("  "); m_usb_tx_uint(check(PIND,7));
        m_usb_tx_string(" "); m_usb_tx_uint((check(PIND, 4) * 1 + check(PIND, 5) * 2 + check(PIND, 6) * 4 + check(PIND, 7) * 8 + 1));
        m_usb_tx_string("   Duty Cycle "); m_usb_tx_uint(((OCR1A - OCR1B)/OCR1A)*100);
        m_usb_tx_string("\r");      // Carriage Return
        //m_usb_tx_string("\n");      // Enter
        //m_usb_tx_string("\f");      // Form Feed
        //m_usb_tx_string("\f");      // Tab
        //m_usb_tx_string("\v");      // Vertical Tab
        
*/
    }
    return 0;                       /* never reached */
}
Beispiel #13
0
int main(void)
{
    m_clockdivide(0);
    m_bus_init();
    m_usb_init();
	m_rf_open(CHANNEL, RXADD, PACKET_LENGTH);  // Configure the RF module to listen on CHANNEL for RXADD
	sei(); //Enable global interrupt
	
	unsigned int star[12]={0};
    int i, j, k =0;
    int maxD, minD, dist, p, q, r, s=0; // p.q.r.s -> co-ordinates between which the midpoint is to be found(x2, x4)[p.q.r.s=x2.y2.x4.y4]
    int cx, cy, cxx, cyy, cX, cY=0; 
	int count=0;
    int pmax, qmax, rmax, smax, pmin, qmin, rmin, smin, pp, qq, rr, ss=0;
    float theta=0;
    float to_angle, to_angle_1, to_angle_2, to_angle_c=0;
    float temp1=0;
	float LESS2=0;
	float temp_theta=0;
	int trans_c,trans_fl,trans_fr,trans_bl,trans_br=0; 
	int puck_in_hand, puck_in_line=0;
	
    set(DDRD,7);    // Set as Comm test Led output
	set(DDRD,6);    // Set as PAUSE Led output
    /*
	//ADC initialization
	clear(ADMUX,REFS1); //Set voltage reference for ADC as Vcc
	set(ADMUX,REFS0);   
	
	set(ADCSRA,ADPS2); //Prescale ADC to 250 Khz
	set(ADCSRA,ADPS1);
	clear(ADCSRA,ADPS0);
	 
	set(DIDR0,ADC1D); //Disabling Digital inputs for ADC pins
	set(DIDR0,ADC4D);
	set(DIDR0,ADC5D);
	set(DIDR0,ADC6D);
	set(DIDR0,ADC7D);
	*/	
    while  (m_wii_open()!=1){}
    
    //Timer Initialization
    set(DDRB, 6); // Set (Reg B. Bit 6) as timer output. PWM 1
    set(DDRB, 7); // Set (Reg B. Bit 7) as timer output. PWM 2
   
    set(TCCR1B, WGM13); // Set Timer 1 Mode 15 (UP to OCR1A, PWM mode)
    set(TCCR1B, WGM12);
    set(TCCR1A, WGM11);
    set(TCCR1A, WGM10);
   
    set(TCCR1A, COM1C1); // Clear at OCR1B. Set at rollover |Timer B.6
    clear(TCCR1A, COM1C0);

    set(TCCR1A, COM1B1); // Clear at OCR1C. Set at rollover |Timer B.7
    clear(TCCR1A, COM1B0);

    OCR1A=1600; //1Khz Motor
	OCR1B=0;
	OCR1C=0;
	
	clear(TCCR1B, CS12); // Initialize timer & Prescale /1 (Start Timer)
    clear(TCCR1B, CS11);
    set(TCCR1B, CS10);
   
    while(1)
	{

		while(!play) //PAUSE
		{
			m_green(OFF); 
			set(PORTD, 6); //Switch on RED LED
			OCR1B=0; //Stop Motor
			OCR1C=0; //Stop Motor
			clear(PORTB,6); //Stop Motor
			clear(PORTB,7); //Stop Motor
		}
		/*	
		//Trans_C
		clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
		
		clear(ADCSRB,MUX5); //Set MUX bit to F1
		clear(ADMUX,MUX2);
		clear(ADMUX,MUX1);
		set(ADMUX,MUX0);
		 
		set(ADCSRA,ADEN); //Enable the system
		set(ADCSRA,ADSC); //Start the conversion
	
		while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
		set(ADCSRA,ADIF);// Clear the flag
		 
		trans_c=ADC;
	
		//Trans_F_LEFT
		clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
		 
		clear(ADCSRB,MUX5); //set MUX bit to F4
		set(ADMUX,MUX2);
		clear(ADMUX,MUX1);
		clear(ADMUX,MUX0);
		 
		set(ADCSRA,ADEN); //Enable the system
		set(ADCSRA,ADSC); //Start the conversion
		 
		while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
		set(ADCSRA,ADIF);// Clear the flag
		 
		trans_fl=ADC;
		 
		//TRANS_F_RIGHT
		clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
		 
		clear(ADCSRB,MUX5); //Set MUX bit to F5
		set(ADMUX,MUX2);
		clear(ADMUX,MUX1);
		set(ADMUX,MUX0);
		 
		set(ADCSRA,ADEN); //Enable the system
		set(ADCSRA,ADSC); //Start the conversion
		 
		while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
		set(ADCSRA,ADIF);// Clear the flag
		 
		trans_fr=ADC;
	
		//TRANS_B_LEFT
		clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
		 
		clear(ADCSRB,MUX5); //Set MUX bit to F6
		set(ADMUX,MUX2);
		set(ADMUX,MUX1);
		clear(ADMUX,MUX0);
		 		 
		set(ADCSRA,ADEN); //Enable the system
		set(ADCSRA,ADSC); //Start the conversion
	
		while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
		set(ADCSRA,ADIF);// Clear the flag
		 
		trans_bl=ADC;
		 
		//TRANS_B_RIGHT
		clear(ADCSRA,ADEN); // Disable ADC system to change MUX bit
		 
		clear(ADCSRB,MUX5); //set MUX bit to F7
		set(ADMUX,MUX2);
		set(ADMUX,MUX1);
		set(ADMUX,MUX0);
		 
		set(ADCSRA,ADEN); //Enable the system
		set(ADCSRA,ADSC); //Start the conversion
	
		while(!check(ADCSRA,ADIF)){} //ADC conversion ongoing
		set(ADCSRA,ADIF);// Clear the flag
		 
		trans_br=ADC;
		*/	
	
		clear(PORTD,6); //Switch OFF RED LED
	
		cli();
		m_wii_read(star);
		sei();
		count=0;
		//Localization of bot w.r.t constellation
		//Reading no. of detectable stars
		for(k=0;k<=3;++k)
		{ 
			if((int)star[3*k]==1023 && (int)star[3*k+1]==1023)
				{count++;} //Count = No. of stars lost
		}
		if(count<=1) //Enter this only if <= 1 star has been lost. 
		{   
			m_red(OFF);    
			//Finding max and min distance
			pmax=0; qmax=3; rmax=1; smax=4;  //p.r=(x1. y1)
			pmin=0; qmin=3; rmin=1; smin=4;  //q.s=(x2. y2)
			maxD=pow(((int)star[pmax]-(int)star[qmax]), 2) + pow(((int)star[rmax]-(int)star[smax]), 2);
			minD=maxD;
			for (i=0; i<3; i++)
			{
				for(j=i+1; j<=3; j++)
				{
					dist=pow(((int)star[i*3]-(int)star[j*3]), 2) + pow(((int)star[i*3+1]-(int)star[j*3+1]), 2);
					if(dist>maxD)
					{
						maxD=dist;
						pmax=i*3; qmax=j*3; rmax=pmax+1; smax=qmax+1;
					}
					if(dist<minD)
					{
						minD=dist;
						pmin=i*3; qmin=j*3; rmin=pmin+1; smin=qmin+1  ;
					}
				}   //for j
			}//for i

			//Check for intersection between the max & min points to find the top x.y
			if((int)star[pmax]==(int)star[pmin] || (int)star[pmax]==(int)star[qmin])
			{
				//m_red(OFF);
				p=pmax; r=rmax; q=qmax; s=smax;
				pp=p; rr=r; qq=q; ss=s;
			}
			else if ((int)star[qmax]==(int)star[pmin] || (int)star[qmax]==(int)star[qmin])
			{
				//m_red(OFF);            
				p=qmax; r=smax; q=pmax; s=rmax; 
				pp=p; rr=r; qq=q; ss=s;
			}
			else //Retain the previous co-ordinates if it can't find intersection
			{
			   p=pp; q=qq; r=rr; s=ss;
			   //m_red(ON);
			}
        
			//Center Point
			cx=((int)star[p]+(int)star[q])/2-512;
			cy=((int)star[r]+(int)star[s])/2-384;
			//Theta
			theta=3.14/2-atan2((int)star[s]-(int)star[r], (int)star[q]-(int)star[p]);
			cxx=1*(cx*cos(theta) - cy*sin(theta));
			cyy=1*(cx*sin(theta) + cy*cos(theta));
			//Center offset
			cxx=cxx-65-8;
			cyy=cyy+25-105;
			//Center in (cm)
			cX=-cxx/4;
			cY=cyy/4;
         
			m_usb_tx_string("  Theta_old: ");
			m_usb_tx_int(theta*57.6);
			//
			if(theta*57.6>=180)
			{	
				theta=theta-2*3.14;
			}
			/* if(theta*57.6<-180) // Why is this there?
			 { theta=2*3.14-theta;
			 }
			 */
	theta=-theta;
	/*
		//Puck detection
			//trans_c|trans_fl|trans_fr|trans_bl|trans_br| 
		while(!puck_in_hand)
		{
			if(trans_c>trans_fl && trans_c>trans_fr && trans_c>trans_bl && trans_c>trans_br) //Puck found! Go straight!
			{
				puck_in_line=1;
				temp1=MORE*OCR1A; //Straight!
				OCR1B=temp1;
				temp1=MORE*OCR1A;
				OCR1C=temp1;
			}
			else if( (trans_fr > trans_fl && trans_fr > trans_bl) || (trans_br >trans_fl && trans_br > trans_bl) ) //If right p_t> left p_t, Rotate Clockwise
			{
				temp1=LESS*OCR1A; //Clockwise
				OCR1B=temp1;
				temp1=MORE*OCR1A;
				OCR1C=temp1;
			}
			else //Rotate anti-clockwise
			{
				temp1=MORE*OCR1A;
				OCR1B=temp1;
				temp1=LESS*OCR1A;
				OCR1C=temp1;
				
			}
		
			}
	
		}*/
		
	
	
		//Goal detection (Activated once puck is detected <puck_in_hand>)
		to_angle_c=atan2(((goal_1+goal_2)/2-cY),(115-cX)); //Angle between the goal and current position
		to_angle_1=atan2((goal_1-cY),(115-cX)); //Angle between goal_1 (+y) and current position
		to_angle_2=atan2((goal_2-cY),(115-cX)); //Angle between goal_2 (-y) and current position
	
		
		if ( (to_angle_1*57.6>0 && to_angle_2*57.6>0) || (to_angle_1*57.6<0 && to_angle_2*57.6<0) ) //If both the angles are + or - (Out of scope of goal)
		{
			to_angle= (abs(to_angle_1*57.6) < abs(to_angle_2*57.6) ) ? to_angle_1 : to_angle_2; //to_angle = whichever is lesser 
		}
		else //Opposite sign (Within scope of goal)
		{
			to_angle=atan2(0,(115-cX)); //Go straight
		}
        //Difference 
		 m_usb_tx_string("  Theta-to_angle: ");
		 m_usb_tx_int((theta*57.6)-(to_angle*57.6));
		// temp_theta=(abs(theta*57.6)-abs(to_angle*57.6);
		// LESS2=( (MORE-LESS)/(10-temp_theta) )*(temp_theta-10) + MORE; 
		 
		if (theta*57.6<=to_angle*57.6+10 && theta*57.6>=to_angle*57.6-10) // Pointing to goal. Go straight.
		{
			temp1=MORE*OCR1A;
			OCR1B=temp1;
			temp1=MORE*OCR1A;
			OCR1C=temp1;
			m_green(ON);
			m_usb_tx_string("  ||STRAIGHT||");
		}
		else if( theta*57.6<(to_angle*57.6-10) ) //Rotate anti-clockwise
		{ 
			temp1=LESS*OCR1A;
			OCR1B=temp1;
			temp1=MORE*OCR1A;
			OCR1C=temp1;
			m_green(OFF);
			m_usb_tx_string("  ||ANTI-CLOCKWISE||");
		}
		   else if ( theta*57.6>(to_angle*57.6+10) ) //Rotate clockwise
		   {
				temp1=MORE*OCR1A;
				OCR1B=temp1;
				temp1=LESS*OCR1A;
				OCR1C=temp1;
				m_green(OFF);
			m_usb_tx_string("  ||CLOCKWISE||");
		   }
              

	set(PORTB,6);
	set(PORTB,7);
      
	}//end if (<= 1 star has been lost.)
   
    else //If more than 1 star has been lost:
    {   
        m_red(ON);
		OCR1B=0; //Stop the motor
		OCR1C=0;
        clear(PORTB,6); 
        clear(PORTB,7); 
        p=-1; q=-1; r=-1; s=-1;
        cxx=-1; cyy=-1;
        theta=0;
    }
    
    /*    
    m_usb_tx_string("X1: ");
    m_usb_tx_uint(star[0]);
    m_usb_tx_string("  Y1: ");
    m_usb_tx_uint(star[1]);
    m_usb_tx_string("  X2: ");
    m_usb_tx_uint(star[3]);
    m_usb_tx_string("  Y2: ");
    m_usb_tx_uint(star[4]);
    m_usb_tx_string("  X3: ");
    m_usb_tx_uint(star[6]);
    m_usb_tx_string("  Y3: ");
    m_usb_tx_uint(star[7]);
    m_usb_tx_string("  X4: ");
    m_usb_tx_uint(star[9]);
    m_usb_tx_string("  Y4: ");
    m_usb_tx_uint(star[10]);

m_usb_tx_string("  P: ");
    m_usb_tx_int(p);
m_usb_tx_string("  R: ");
    m_usb_tx_int(r);
m_usb_tx_string("  Q: ");
    m_usb_tx_int(q);
m_usb_tx_string("  S: ");
    m_usb_tx_int(s);
*/
    m_usb_tx_string("  Theta: ");
    m_usb_tx_int(theta*57.6);
	
    m_usb_tx_string("  CX (cm): ");
    m_usb_tx_int(cX);
    m_usb_tx_string("  CY (cm): ");
    m_usb_tx_int(cY);
    m_usb_tx_string("   Count: ");
    m_usb_tx_int(count);
    m_usb_tx_string("   Toangle_C: ");
    m_usb_tx_int(to_angle_c*57.6);
	m_usb_tx_string("   Toangle_1: ");
	m_usb_tx_int(to_angle_1*57.6);
	m_usb_tx_string("   Toangle_2: ");
	m_usb_tx_int(to_angle_2*57.6);
	m_usb_tx_string("   Toangle: ");
	m_usb_tx_int(to_angle*57.6);
    m_usb_tx_string("\n");
   m_wait(10);
   
    } ///while (1)
 }// main void()
Beispiel #14
0
void state_play()
{
	    //F4 for acd conversion
	    clear(ADCSRA,ADEN);
	    clear(ADCSRB,MUX5);//setting mux to F4
	    set(ADMUX,MUX2);
	    clear(ADMUX,MUX1);
	    clear(ADMUX,MUX0);

	    //starting adc for F4
	    set(ADCSRA,ADEN);
	    set(ADCSRA,ADSC);

	    while(!check(ADCSRA,ADIF));
	    adc_values[1] = ADC;//saving adc value
	    set(ADCSRA,ADIF);//clearing flag

  
	    //F6 for acd conversion
	    clear(ADCSRA,ADEN);
	    clear(ADCSRB,MUX5);//setting mux to F6
	    set(ADMUX,MUX2);
	    set(ADMUX,MUX1);
	    clear(ADMUX,MUX0);

	    //starting adc for F6
	    set(ADCSRA,ADEN);
	    set(ADCSRA,ADSC);

	    while(!check(ADCSRA,ADIF));
	    adc_values[0] = ADC;//saving adc value
	    set(ADCSRA,ADIF);//clearing flag
  
	    clear(ADCSRA,ADEN);
	    //D6 for acd conversion
	    set(ADCSRB,MUX5);//setting mux to D6
	    clear(ADMUX,MUX2);
	    clear(ADMUX,MUX1);
	    set(ADMUX,MUX0);

	    //starting adc for D6
	    set(ADCSRA,ADEN);
	    set(ADCSRA,ADSC);

	    while(!check(ADCSRA,ADIF));
	    adc_values[2] = ADC;//saving adc value
	    set(ADCSRA,ADIF);//clearing flag

	    //D4 for acd conversion
	    clear(ADCSRA,ADEN);
	    set(ADCSRB,MUX5);//setting mux to D4
	    clear(ADMUX,MUX2);
	    clear(ADMUX,MUX1);
	    clear(ADMUX,MUX0);

	    //starting adc for D4
	    set(ADCSRA,ADEN);
	    set(ADCSRA,ADSC);

	    while(!check(ADCSRA,ADIF));
	    adc_values[3] = ADC;//saving adc value
	    set(ADCSRA,ADIF);//clearing flag

	    puck_status = 0;

	    if(adc_values[0]>700  && adc_values[0]>adc_values[2] && adc_values[0]>adc_values[3])
	      puck_status = 2;

	    if(adc_values[1]>800 && (adc_values[1]-adc_values[2] < -100 || adc_values[1]-adc_values[2] > 100) && adc_values[1]>adc_values[3])
	      puck_status = 2;

	    if(adc_values[1]>800 && (adc_values[1]-adc_values[2] > -100 && adc_values[1]-adc_values[2] < 100))
	      puck_status = 0;

	    if(adc_values[2]>800 && (adc_values[1]-adc_values[2] < -100 || adc_values[1]-adc_values[2] > 100) && adc_values[2]>adc_values[1])
	      puck_status = 1;   

	    if(adc_values[3]>800  && adc_values[3]>adc_values[0] && adc_values[3]>adc_values[1])
	      puck_status = 1;
    
	    switch(puck_status){
	      case 1:
	        set_left(90);
	        set_right(50);
	        m_red(ON);
	        m_green(ON);
	        break;

	      case 2:
	        set_left(-90);
	        set_right(-50);
	        m_red(ON);
	        m_green(OFF);
	        break;

	      case 0:
	        set_left(0);
	        set_right(0);
	        m_red(OFF);
	        m_green(ON);
	        break;
	    }

      get_position(blobs, &x, &y, &theta);

	    if(m_usb_isconnected()){
	      //print out adc values to screen
	      /*m_red(TOGGLE);
	      m_usb_tx_string("Phototrans 1 = ");
	      m_usb_tx_int((int) adc_values[0]);
	      m_usb_tx_string("\n");
	      m_usb_tx_string("Phototrans 2 = ");
		  	m_usb_tx_int((int)adc_values[1]);
	      m_usb_tx_string("\n");
	      m_usb_tx_string("Phototrans 3 = ");
		  	m_usb_tx_int((int)adc_values[2]);
	      m_usb_tx_string("\n");
	      m_usb_tx_string("Phototrans 4 = ");
		  	m_usb_tx_int((int)adc_values[3]);
	      m_usb_tx_string("\n");
	      m_usb_tx_string("puck status = ");
		  	m_usb_tx_int((int)puck_status);
	      m_usb_tx_string("\n");
	      m_wait(1000);*/
        m_usb_tx_string("X = ");
	      m_usb_tx_int((int) x-x_center);
	      m_usb_tx_string("\n");
        m_usb_tx_string("Y = ");
	      m_usb_tx_int((int) y-y_center);
	      m_usb_tx_string("\n");
        m_usb_tx_string("Theta = ");
	      m_usb_tx_int((int) theta-theta_zero);
	      m_usb_tx_string("\n");
	    }
      
    
	wireless_buffer_f = false;
	char inst = wireless_buffer[0];
	
	switch(inst) 
	{
		case 0xA4:
			state_pause();
			break;
		case 0xA1:
			state_play();
			return;
    case 0xA5:
      state_detangle();
      return;
		default:
			state_play();
			return;
	}
}
Beispiel #15
0
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
        //}
}
Beispiel #16
0
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);
    }
}
Beispiel #17
0
void measure(int select){
	//m_green(TOGGLE);
	toggle(PORTB,2);
	int* bar;

	selectIMU(select);
	
	switch(select){
		case 0:
			bar = bar0;
			break;
		case 1:
			bar = bar1;
			break;
		case 2:
			bar = bar2;
			break;
		case 3:
			bar = bar3;
			break;
		case 4:
			bar = bar4;
			break;
		case 5:
			bar = bar5;
			break;
	}
	
	//m_usb_tx_int(select);
	//m_usb_tx_string("\t");
	
	micros = 4096 * overflow + (unsigned long)((float)(((unsigned long)(TCNT3H) << 8) | TCNT3L) * 4096 / 65536);
	m_usb_tx_ulong(micros);
	m_usb_tx_string("\t");
	
	m_imu_accel(raw_data_buffer);
	ax = raw_data_buffer[0] - bar[0];
	ay = raw_data_buffer[1] - bar[1];
	az = raw_data_buffer[2] - bar[2];
	m_usb_tx_int(ax);
	m_usb_tx_string("\t");
	m_usb_tx_int(ay);
	m_usb_tx_string("\t");
	m_usb_tx_int(az);
	m_usb_tx_string("\t");

	m_imu_gyro(raw_data_buffer);
	gx = raw_data_buffer[0] - bar[3];
	gy = raw_data_buffer[1] - bar[4];
	gz = raw_data_buffer[2] - bar[5];
	m_usb_tx_int(gx);
	m_usb_tx_string("\t");
	m_usb_tx_int(gy);
	m_usb_tx_string("\t");
	m_usb_tx_int(gz);
	m_usb_tx_string("\t");
	
	m_imu_mag(raw_data_buffer);
	mx = raw_data_buffer[0];
	my = raw_data_buffer[2];
	mz = raw_data_buffer[1];
	m_usb_tx_int(mx);
	m_usb_tx_string("\t");
	m_usb_tx_int(my);
	m_usb_tx_string("\t");
	m_usb_tx_int(mz);
	m_usb_tx_string("\n");
}
Beispiel #18
0
/************************************************************
Main Loop
************************************************************/
int main(void)
{
	/* Confirm Power */
	m_red(ON);

	/* Initializations */
	init();
	usb_enable();
	timer3_init();
	int gy_previous_reading = 0;

	/* Confirm successful initialization(s) */
	m_green(ON);

	/* Run */
	while (1){
		if (m_imu_raw(data))
		{
			m_green(ON);
			m_red(OFF);
			
			
			ax = lowpass(0.85,ax,data[0])+AX_OFFSET;
			az = lowpass(0.85,az,data[2])+AZ_OFFSET;
			gy = lowpass(ALPHA_LOW,gy,data[4])+GY_OFFSET;
			gy = highpass(ALPHA_HIGH,gy,gy_previous_reading,data[4]);
			gy_previous_reading = data[4];
			
			/*
			m_usb_tx_string("ax= ");
			m_usb_tx_int(ax);
			m_usb_tx_string("     az=");
			m_usb_tx_int(az);
			m_usb_tx_string("     gy=");
			m_usb_tx_long(gy);
			m_usb_tx_string("\n");
			*/
			
			
			int angle = ((float)ax*RAD2DEG)/sqrt(((float)ax*ax+(float)az*az));
			
			if (check(TIFR3,OCF3A)){	//check if timestep has completed 
				angle += gy*TIMESTEP;	//add thetadot*timestep to angle 
				set(TIFR3,OCF3A);		//reset flag 
			}
			
			m_usb_tx_int(angle);
			m_usb_tx_string("\n");
			
			/*
			m_usb_tx_string("ax= ");
			m_usb_tx_int(data[0]);
			m_usb_tx_string("     ay= ");
			m_usb_tx_int(data[1]);
			m_usb_tx_string("     az= ");
			m_usb_tx_int(data[2]);
			m_usb_tx_string("     gx= ");
			m_usb_tx_int(data[3]);
			m_usb_tx_string("     gy= ");
			m_usb_tx_int(data[4]);
			m_usb_tx_string("     gz= ");
			m_usb_tx_int(data[5]);
			m_usb_tx_string("\n");
			*/
			
		}
		else
		{
			m_green(OFF);
			m_red(ON);
		}
	}
}