// --------------------------------------------------------------
// Initialize M2
// --------------------------------------------------------------
void init(void){
// Initialize RED LED as on for the Initialization of M2
m_red(ON);
// Set the System Clock at 16MHz
m_clockdivide(0);
// Initialize the mBus
m_bus_init();
if (USB) {
m_usb_init(); // Initialize USB and
while(!m_usb_isconnected()); // wait for a connection
}
m_rf_open(CHANNEL,RXADDRESS,PACKET_LENGTH);
m_red(OFF);
// Turn off the RED LED when done
}
void init(){
m_usb_init(); //initialize usb communication
while(!m_usb_isconnected()); // wait for a connection
m_clockdivide(0); // 16Mhz
m_red (OFF); // Start with the LED off
// Voltage Reference
clear(ADMUX,REFS1); // Vcc
set(ADMUX,REFS0); // ^
// ADC Prescalar
set(ADCSRA,ADPS2); // prescaler 128 to 125kHz
set(ADCSRA,ADPS1); // ^
set(ADCSRA,ADPS0); // ^
// Disable other inputs
set(DIDR0,ADC0D); // disable F0
set(DIDR0,ADC1D); // disable F1
set(DIDR0,ADC4D); // disable F4
set(DIDR0,ADC5D); // disable F5
set(DIDR0,ADC6D); // disable F6
set(DIDR0,ADC7D); // disable F7
// Set Desired Pin Input
clear(ADCSRB, MUX5); //set pin to F0
clear(ADCSRB, MUX2); //^
clear(ADCSRB, MUX1); //^
clear(ADCSRB, MUX0); //^
// Enable Conversion
set(ADCSRA,ADEN); // enable conversion
}
int main(void)
{
m_red(ON);
m_usb_init();
while(!m_usb_isconnected()); // wait for a serial msg
m_red(OFF);
m_clockdivide(0); // set clock frequency to 16MHz
adc_setup();
m_disableJTAG();
set(ADCSRA,ADEN);//enable ADC
sei();//enable global interrupts
set(ADCSRA,ADSC);//start conversion
while(1){
if(FLAG){ // compare readings and move motor accordingly
m_green(TOGGLE);
clear(ADCSRA,ADEN); // Disable ADC
m_usb_tx_string("\nF0:");
m_usb_tx_int(ir[0]);
m_usb_tx_string("\tF1: ");
m_usb_tx_int(ir[1]);
m_usb_tx_string("\tF4: ");
m_usb_tx_int(ir[2]);
FLAG = 0;
set(ADCSRA,ADEN); // enable ADC again
set(ADCSRA,ADSC);//start next conversion
}
}
}
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);
}
}
}
int main(void)
{
// unsigned int value;
/* insert your hardware initialization here */
m_usb_init();
while (!m_usb_isconnected());
for(;;) {
// if (m_usb_rx_available()) {
// value = m_usb_rx_char();
// m_usb_tx_uint(value);
// }
m_wait(500);
m_green(TOGGLE);
}
return 0; /* never reached */
}
void gen_setup(void){
usb_conn_test = Matlab_pos_tracking || go_to_location_test || debug_com || debug_goto || debug_find_puck;
if (usb_conn_test ){
m_usb_init();
m_green(ON);
while (!m_usb_isconnected());
m_green(OFF);
}
m_bus_init();
m_wii_open();
// m_port_init(PORT_ADD);
setup_pins();
setup_puckfinding(); //Puck Finding Intialization
setup_timer_1();
setup_timer_3();
setup_ADC();
}
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)
{
unsigned long runtime = 0;
m_clockdivide(0); // 16 MHz
OCR1B = 521; // initialize output compare register (interrupt every 521 cycles)
set(DDRB,6); // set B6 as output (output compare pin)
set(DDRB,4); // set B4 as output (green LED for data synchronization)
set(PORTB,4); // turn on green LED on B4
set(DDRF,0); // set F0 as output
set(DDRF,1); // set F1 as output
set(DDRB,1); // set B1 as output
m_usb_init();
while(!m_usb_isconnected()){
m_green(ON);
}
m_green(OFF);
set(DDRB,2);
while(!m_usb_rx_available()); // wait for runtime argument from Python script
m_wait(5);
while(m_usb_rx_available()){
runtime = runtime * 10 + (m_usb_rx_char() - '0'); // build number of seconds
}
runtime = runtime * 1000000; // convert to microseconds
clear(PORTF,0); // S0
clear(PORTF,1); // S1
clear(PORTB,1); // S2
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU0 could not connect");
}
/*
set(PORTF,0);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU1 could not connect");
}
clear(PORTF,0);
set(PORTF,1);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU2 could not connect");
}
set(PORTF,0);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU3 could not connect");
}
set(PORTB,1);
clear(PORTF,0);
clear(PORTF,1);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU4 could not connect");
}
set(PORTF,0);
m_wait(10);
if(!m_imu_init(0,1)){
m_red(ON); // RED LED turns on if there's a problem
m_usb_tx_string("IMU5 could not connect");
}
*/
//calibrate(0);
//calibrate(1);
//calibrate(2);
//calibrate(3);
//calibrate(4);
//calibrate(5);
// Initialize Timer 1 output compare interrupt
set(TIMSK1,OCIE1B);
set(TCCR1B,CS12);
clear(TCCR1B,CS11);
set(TCCR1B,CS10);
clear(TCCR1B,WGM13);
set(TCCR1B,WGM12);
clear(TCCR1A,WGM11);
clear(TCCR1A,WGM10);
clear(TCCR1A,COM1B1);
set(TCCR1A,COM1B0);
// Initialize Timer 1 overflow interrupt
set(TIMSK1,TOIE1);
// Initialize Timer 3 overflow interrupt
overflow = 0;
set(TIMSK3,TOIE3);
clear(TCCR3B,CS32);
clear(TCCR3B,CS31);
set(TCCR3B,CS30);
sei(); // enable global interrupts
clear(PORTB,4); // turn off green LED --> signal to camera
while(overflow * 4096 < runtime){
m_green(TOGGLE);
}
m_usb_tx_string(" ");
m_green(OFF);
//cli();
while(1);
}
void setupUSB(){
m_usb_init();
m_green(ON);
while (!m_usb_isconnected()){m_wait(1);}
m_green(OFF);
}
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);
}
}
}
}
void setup(void)
{
m_red(ON);
m_green(ON);
// ***********************************
// Clock Speed
// ***********************************
m_clockdivide(0); // Set to 16MHz ('0' = divide by 1)
// ***********************************
// Watchdog
// ***********************************
wdt_reset();
wdt_enable(WDTO_2S); // Watchdog timer has a 2s interrupt
// ***********************************
// Debugging
// ***********************************
if(DEBUG){
/* initialize m_usb com */
m_usb_init();
while(!m_usb_isconnected());
}
// ***********************************
// Timer 1 Setup
// ***********************************
// Timer 1 used to control PWM into motor 1 - left
clear(TCCR1B, CS12); // Turn Prescaler to /8
set(TCCR1B, CS11); // ^
clear(TCCR1B, CS10); // ^
set(TCCR1B, WGM13); // mode 15 - up to 0CR1A
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); // ^
set(DDRB, 6); // enable B6 as an output pin for PWM
set(DDRB, 7); // enable B7 as an output pin for PWM
OCR1A = FREQ; // Frequency of PWM
// ***********************************
// Timer 3 Setup
// ***********************************
// clear(TCCR3B, CS32); // Scale by 1
// clear(TCCR3B, CS31);
// set(TCCR3B, CS30);
clear(TCCR3B, CS32); // Scale by 8
set(TCCR3B, CS31);
clear(TCCR3B, CS30);
clear(TCCR3B, WGM33); // Timer Mode 0
clear(TCCR3B, WGM32);
clear(TCCR3A, WGM31);
clear(TCCR3A, WGM30);
// ***********************************
// External Interrupts
// ***********************************
// PCINT0 = D0 = CH3, throttle
// PCINT1 = D1 = CH4, direction
// PCINT2 = D2 = CH5, kill switch
clear(EIMSK,INT0); // PCINT0
clear(EIMSK,INT1); // PCINT1
clear(EIMSK,INT2); // PCINT2
// PCINT0: Interrupt on either falling or rising edge
clear(EICRA,ISC01);
set(EICRA,ISC00);
// PCINT1: Interrupt on either falling or rising edge
clear(EICRA,ISC11);
set(EICRA,ISC10);
// PCINT2: Interrupt on either falling or rising edge
clear(EICRA,ISC21);
set(EICRA,ISC20);
// Remove masks for the corresponding interrupt
set(EIMSK,INT0); // PCINT0
set(EIMSK,INT1); // PCINT1
set(EIMSK,INT2); // PCINT2
sei(); // Enable global interrupts
m_red(OFF);
m_green(OFF);
}
/* Setup USB */
void usb_enable(void)
{
m_usb_init();
while(!m_usb_isconnected());
}
void init(void)
{
m_red(ON);
m_clockdivide(0);
m_bus_init();
m_usb_init();
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
WAIT(5); // wait for user to pull up log for connection
send(0,0,0,0); //USB and wireless initialized
while(!m_usb_isconnected());
m_red(OFF);
//m_usb_tx_string("Waiting for robot to respond...");
send(0,0,0,1); //Waiting for robot to respond
while(!connected)
{
m_rf_send(TXADDRESS, in_buffer, PACKET_LENGTH);
BLINK(100);
}
//m_usb_tx_string("Robot responded. \nRequsting PID variables.\n");
send(0,1,0,2); // Robot responded. Requesting PID variables
BLINK(500);BLINK(500);
// Set up ADC for PID
// Set to external AREF
clear(ADMUX, REFS1);
set(ADMUX, REFS0);
// Set ADC prescaler
set(ADCSRA, ADPS2);
set(ADCSRA, ADPS1);
set(ADCSRA, ADPS0);
set(DIDR0, ADC4D);
set(DIDR0, ADC5D);
set(DIDR0, ADC6D);
sei();
set(ADCSRA, ADIE);
clear(ADCSRA, ADATE);
clear(ADCSRA, ADEN);
// MUX selection
clear(ADCSRB, MUX5);
set(ADMUX, MUX2);
clear(ADMUX, MUX1);
clear(ADMUX, MUX0);
// enables the system
set(ADCSRA, ADEN);
// starts the conversion
set(ADCSRA, ADSC);
m_red(OFF);
m_green(OFF);
}
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;
}
}
/*char isStuck() {
m_wait(100);
//localize(data);
distfrombound = data[1] + ((float)BOUNDARYTHRESHOLD)*sin((data[2]+90.0)*3.14/180.0*-1.0);
if (distfrombound > 60.0 || distfrombound < -60.0) {
return 1;
}
m_usb_tx_int((int)distfrombound);
m_usb_tx_char('\n');
}
long loccounter = 0;
float prevx = 0.0;
float prevy = 0.0;
*/
int main(void)
{
//goal side
clear(DDRB,1);
clear(PORTB,1);
if (check(PINB,1)) {
goal = 1;
}
set(DDRB,0);
set(PORTB,0);
set(DDRD,5);
set(DDRD,3);
//wireless stuffs
m_bus_init();
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
int counter = 0;
//
//m_num_init();
int flag;
m_clockdivide(0);
m_disableJTAG();
//TIMER 0: For Controlling the solenoid
//
// set(TCCR0B, WGM02);
// set(TCCR0A, WGM01);
// set(TCCR0A, WGM01);
//
// set(TCCR0A, COM0B1);
// clear(TCCR0A, COM0B0);
//
// set(TCCR0B, CS02);
// set(TCCR0B, CS01);
// set(TCCR0B, CS00);
//
set(DDRB,7);
// OCR0A = 0xFF;
// OCR0B = 0xff;
//
//TIMER 1: For Controlling the left wheel
set(TCCR1B, WGM13);
set(TCCR1B, WGM12);
set(TCCR1A, WGM11);
set(TCCR1A, WGM10);
set(TCCR1A, COM1B1);
clear(TCCR1A, COM1B0);
clear(TCCR1B, CS12);
clear(TCCR1B, CS11);
set(TCCR1B, CS10);
set(DDRB,6);
OCR1A = 0xFFFF;
OCR1B = 0;
//TIMER 3: For Controlling the right wheel
//up to ICR3, clear at OCR3A & set at rollover
set(TCCR3B, WGM33);
set(TCCR3B, WGM32);
set(TCCR3A, WGM31);
clear(TCCR3A, WGM30);
set(TCCR3A, COM3A1);
clear(TCCR3A, COM3A0);
clear(TCCR3B, CS32);
clear(TCCR3B, CS31);
set(TCCR3B, CS30);
ICR3 = 0xFFFF;
OCR3A = 0;
// //Set TCNT0 to go up to OCR0A
// clear(TCCR0B, WGM02);
// set (TCCR0A, WGM01);
// clear(TCCR0A, WGM00);
//
// // Don't change pin upon hitting OCR0B
// clear(TCCR0A, COM0A1);
// clear(TCCR0A, COM0A0);
//
// // Set clock scale to /1024
// set(TCCR0B, CS02);
// clear(TCCR0B, CS01);
// set(TCCR0B, CS00);
//
// // Set Frequency of readings to 1/SAMPLERATE; dt = 1/SAMPLERATE
// OCR0A = (unsigned int) ((float) F_CPU / 1024 / REPORTRATE);
// OCR0B = 1;
// Set up interrupt for reading values at sampling rate
//Pin for controlling solenoid pulse
set(DDRB,7);
//Pins for controlling speed of left and right wheel
set(DDRB,6);
set(DDRC,6);
//Pins for determining direction of wheels
set(DDRB,2);
set(DDRB,3);
//Blue LED for Comm Test
//set(DDRB,5);
//ADC's
sei(); //Set up interrupts
set(ADCSRA, ADIE);
clear(ADMUX, REFS1); //Voltage reference is AR pin (5V)
clear(ADMUX, REFS0); //^
set(ADCSRA, ADPS2); //Set scale to /128
set(ADCSRA, ADPS1); //^
set(ADCSRA, ADPS0); //^
set(DIDR0, ADC0D); //Disable digital input for F0
set(DIDR0, ADC1D),
set(DIDR0, ADC4D);
set(DIDR0, ADC5D);
set(DIDR0, ADC6D);
set(DIDR0, ADC7D);
set(DIDR2, ADC8D);
set(DIDR2, ADC9D);
set(ADCSRA, ADATE); //Set trigger to free-running mode
chooseInput(0);
set(ADCSRA, ADEN); //Enable/Start conversion
set(ADCSRA, ADSC); //^
set(ADCSRA, ADIF); //Enable reading results
//Limit Switch stuffs
// clear(DDRB,0); //set to input, RIGHT LIMIT SWITCH
// clear(DDRB,1); //set to input, LEFT LIMIT SWITCH
//
// clear(PORTB,0); //disable internal pull up resistor
// clear(PORTB,1); //disable internal pull up resistor
//int state; // state variable
state = 0; //set state
play = 0;
long count = 0;
if (state == 70) {
m_usb_init(); // connect usb
while(!m_usb_isconnected()); //wait for connection
}
//m_bus_init();
m_wii_open();
//m_usb_init();
m_red(ON);
local_init();
localize(data);
m_red(OFF);
//set(TIMSK0, OCIE0A);
while(1)
{
if (play) {m_red(ON);}
else {m_red(OFF);}
// m_wait(100);
// m_red(TOGGLE);
// localize(data);
/*if (loccounter == LOCCOUNT) {
if (sqrt((data[0]-prevx)*(data[0]-prevx)+(data[1]-prevy)-(data[1]-prevy)) < 1.0) {
m_red(ON);
reverse();
m_wait(100);
state = 6;
}
else {
m_red(OFF);
state = 2;
}
prevx = data[0];
prevy = data[1];
loccounter = 0;
}*/
//loccounter++;
// localize(data);
// locdata[1] = (char)data[0];
// locdata[2] = (char)data[1];
// toggle(PORTD,3);
changedState = 0;
angle_dif = 0;
//Detect weather we have the puck
getADC();
//if (!play) game_pause();
if (ADCarr[7] > 500) {
//set(PORTD,5);
//set(PORTD,5);
iHaveThePuck = 1;
m_green(ON);
if (play && goal == A) state = 3;
if (play && goal == B) state = 4;
} else {
//clear(PORTD,5);
iHaveThePuck = 0;
m_green(OFF);
if (play) state = 2;
}
if(iHaveThePuck && state == 2) {
//set(PORTB,5);
//drive_to_goalA();
}
else {
//clear(PORTB,5);
//if (state != 0) state = 2;
}
// if(isStuck()) {
// //set(PORTD,5);u
// }
// else {
// //clear(PORTD,5);
// }
//localize(data);
// if (check(PINB,0)) {
// set(PORTD,5);
// state = 0x1A;
// } else {
// clear(PORTD,5);
// state = 2;
// }
if (!play) state = buffer[0];
//switch states
switch (state) {
case -2:
getADC();
if (ADCarr[0] > 500) {
m_green(ON);
}
else {m_green(OFF)}
break;
case -1: //test Limit switches
//m_green(ON);
if (check(PINB,1)) {
m_green(ON);
}
else if (check(PINB,0)) {
m_red(ON);
}
else {
m_red(OFF);
m_green(OFF);
}
break;
case 0:
//drive_to_point2(-100,0);
game_pause();
break;
case 1:
findPuck();
break;
case 2:
//m_red(ON);
if (!iHaveThePuck) {
if (play)
drive_to_puck();
}
break;
case 3:
if (play)
drive_to_goalA();
break;
case 4:
if (play)
drive_to_goalB();
break;
case 5:
getADC();
if (ADCarr[7] > 500) {
//set(PORTD,5);
} else {
//clear(PORTD,5);
}
break;
case 6:
if (data[2] > 0) {
rotate(RIGHT,1);
} else {
rotate(LEFT,1);
}
break;
case 7:
drive_to_point2(-50,0);
break;
case 0xA0:
comm_test();
break;
case 0xA1:
play = 1;
drive_to_puck();
break;
case 0xA2:
play = 0;
game_pause();
break;
case 0xA3:
play = 0;
game_pause();
break;
case 0xA4:
play = 0;
game_pause();
break;
case 0xA6:
play = 0;
game_pause();
break;
case 0xA7:
game_pause();
break;
case 69:
set(PORTB,2);
set(PORTB,3);
OCR1B = OCR1A;
OCR3A = ICR3;
break;
case 70:
reportADC();
break;
default:
game_pause();
break;
}
}
}
