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();}
}
}
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 TX_comm(){
if (m_rf_send( RX_add, send_buffer, p_length) == 0 ){m_red(1);m_green(1);};
}
void grinder(void)
{
init_all();
//set voltage reference to 5V
clear(ADMUX, REFS1);
set(ADMUX, REFS0);
//set ADC pre-scaler
set(ADCSRA,ADPS2);
set(ADCSRA,ADPS1);
set(ADCSRA,ADPS0);
//set pin to turn off digital circuitry
set(DIDR0,ADC6D);
set(DIDR0,ADC7D);
set(DIDR2,ADC10D);
//set B0 for puck possession input
clear(DDRB,0);
//initialize position
m_red(ON); m_green(ON); m_wait(300);
m_red(OFF); m_green(OFF); m_wait(300);
m_red(ON); m_green(ON); m_wait(300);
m_wii_read(blobs);
set_position(512,384);
get_position(blobs, &x, &y, &theta);
theta_goal = fix_theta(theta);
m_green(OFF); m_red(OFF); m_wait(300);
m_red(ON); m_green(ON); m_wait(300);
m_red(OFF); m_green(OFF); m_wait(300);
char inst = wireless_buffer[0];
while(1)
{
if(wireless_buffer_f)
{
wireless_buffer_f = false;
inst = wireless_buffer[0];
}
switch(inst)
{
case 1:
inst = 0xA1;
break;
case 2:
inst = 0xA1;
break;
case 0xA0: //command test
m_green(ON); m_red(ON); m_wait(300);
m_green(OFF); m_red(OFF); m_wait(300);
m_green(ON); m_red(ON); m_wait(300);
m_green(OFF); m_red(OFF);
while(!wireless_buffer_f);
wireless_buffer_f = false;
break;
case 0xA1: //play
//set channel selection to D7 for puck location
clear(ADMUX,MUX0);
set(ADMUX,MUX1);
clear(ADMUX,MUX2);
set(ADCSRB,MUX5);
set(ADCSRA,ADEN);
set(ADCSRA,ADSC);
while(!check(ADCSRA,ADIF));
while(start && !wireless_buffer_f)
{
wireless_buffer_f = false;
if(ADC >= 870){set(ADCSRA,ADIF); set(ADCSRA,ADSC); set_left(100); set_right(100); m_green(ON);}
if(ADC < 870){set(ADCSRA,ADIF); set(ADCSRA,ADSC); set_left(-50); set_right(-50); m_green(OFF); start = false; m_wait(500); set_left(30); set_right(-30); m_wait(500);}
}
start = false;
m_wii_read(blobs);
get_position(blobs, &x, &y, &theta);
theta = fix_theta(theta);
if(ADC < 930 && !target)
{
m_green(OFF);
if(i < 2000){set_left(8); set_right(24); i = i + 1;}
if(i == 2000){set_left(-50); set_right(-50); m_wait(500); i = i + 1;}
if(i > 4000 && i < 5000){set_left(24); set_right(8); i = i + 1;}
if(i == 5000){set_left(-50); set_right(-50); m_wait(500); i = 0;}
target = false;
}
if(ADC < 930 && target)
{
set_left(-50);
set_right(-50);
m_wait(500);
set_left(0);
set_right(0);
target = false;
}
if(ADC >= 930 && !target){m_green(ON); set_left(90); set_right(90); i = 0; target = true;}
set(ADCSRA,ADIF);
clear(ADCSRA,ADEN);
if(target && !wireless_buffer_f)
{
wireless_buffer_f = false;
//set channel selection to F6 for puck possession
clear(ADMUX,MUX0);
set(ADMUX,MUX1);
set(ADMUX,MUX2);
clear(ADCSRB,MUX5);
set(ADCSRA,ADEN);
set(ADCSRA,ADSC);
while(!check(ADCSRA,ADIF));
if(ADC < 930){possession = false;}
if(ADC >= 930)
{
set(ADCSRA,ADIF);
clear(ADCSRA,ADEN);
//set channel selection to F6 for puck possession
set(ADMUX,MUX0);
set(ADMUX,MUX1);
set(ADMUX,MUX2);
clear(ADCSRB,MUX5);
set(ADCSRA,ADEN);
set(ADCSRA,ADSC);
while(!check(ADCSRA,ADIF));
if(ADC >= 930){possession = true; m_red(ON);}
if(ADC < 930){possession = false;}
}
}
if(possession && !wireless_buffer_f)
{
wireless_buffer_f = false;
buffer[0] = 1;
send_message_to_bot(buffer, 0x24);
if(theta > theta_goal + 0.79){set_left(35); set_right(15); sit_1 = true;}
if(theta < theta_goal - 0.79){set_left(15); set_right(35); sit_2 = true;}
if(!sit_1 && !sit_2){set_left(100); set_right(100);}
sit_1 = false;
sit_2 = false;
}
if(!possession){buffer[0] = 2; send_message_to_bot(buffer, 0x24); m_red(OFF);}
set(ADCSRA,ADIF);
clear(ADCSRA,ADEN);
m_red(OFF);
m_green(OFF);
break;
//case 0xA2 goes to default (GOAL A)
//case 0xA3 goes to default (GOAL B)
//case 0xA4 goes to default (PAUSE)
//case 0xA5 goes to default (DETANGLE)
case 0xA6: //halftime
set_left(0);
set_right(0);
theta_goal = theta_goal + pi;
theta_goal = fix_theta(theta_goal);
start = true;
i = 0;
wireless_buffer_f = false;
while(!wireless_buffer_f);
break;
//case 0xA7 goes to default (GAMEOVER)
default:
set_left(0);
set_right(0);
start = true;
i = 0;
wireless_buffer_f = false;
while(!wireless_buffer_f);
break;
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// localization
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
void localize(unsigned int* mWii_packet, int* localizationData, float* bot_angle){
// SUBROUTINE VARIABLES
velocity = sqrt((float)(last_pos[0]*last_pos[0] + localizationData[0]*localizationData[0]));
last_pos[0] = localizationData[0];
last_pos[1] = localizationData[1];
int X[STARS] = {0}; int Y[STARS] = {0};
long Dists[6]= {1}; // dists[0] = s0s1 [1] = s0s2 [2] = s0s3 [3] = s1s2 [4] = s1s3 [5] = s2s3
int s_map[STARS] = {0}; // star map...will probably get rid of in future versions
int avail_stars = 0;
// fills X[] and Y[] with all of the good star readings
int cnt;
m_red(OFF);
for (cnt = 0 ; cnt< STARS;cnt++){
if (mWii_packet[cnt*3] == 1023){ /* m_red(ON);*/ }
//else {
X[avail_stars]=mWii_packet[ 3*cnt ] - X_BOT;
Y[avail_stars]=mWii_packet[ 3*cnt + 1] - Y_BOT;
avail_stars++;
//}
}
// computes distances between all available stars
int distIndex=0; int index; int other;
for (index = 0; index< avail_stars ; index++){
for(other = index+1; other < avail_stars; other ++){
Dists[distIndex] = pow((X[index] - X[other]),2) + pow(( Y[index] - Y[other] ),2);
distIndex++;
}
}
// finds largest inter star distance
int minIndex=-1; int maxIndex=-1;
long max = 0; long min = 90000;
for (index = 0 ; index < 6 ; index++){
if (Dists[index] < min) { min = Dists[index]; minIndex = index; }
if ( Dists[index] > max ){ max = Dists[index]; maxIndex = index; }
}
/* 0 : 1 0 : 2 0 : 3 1 : 2 1 : 3 2 : 3 */
switch (maxIndex){
case 0: switch (minIndex) {
case 1: case 2: s_map[1] = 0; s_map[3] = 1; break;
case 3: case 4: s_map[1] = 1; s_map[3] = 0; break;
default: m_red(ON); } break;
case 1: switch (minIndex) {
case 0: case 2: s_map[1] = 0; s_map[3] = 2; break;
case 3: case 5: s_map[1] = 2; s_map[3] = 0; break;
default: m_red(ON); } break;
case 2: switch (minIndex) {
case 0: case 1: s_map[1] = 0; s_map[3] = 3; break;
case 4: case 5: s_map[1] = 3; s_map[3] = 0; break;
default: m_red(ON); } break;
case 3: switch (minIndex) {
case 0: case 4: s_map[1] = 1; s_map[3] = 2; break;
case 1: case 5: s_map[1] = 2; s_map[3] = 1; break;
default: m_red(ON); } break;
case 4: switch (minIndex) {
case 0: case 3: s_map[1] = 1; s_map[3] = 3; break;
case 2: case 5: s_map[1] = 3; s_map[3] = 1; break;
default: m_red(ON) } break;
case 5: switch (minIndex){
case 1: case 3: s_map[1]=2; s_map[3]=3; break;
case 2: case 4: s_map[1] = 3; s_map[3] = 2; break;
default: m_red(ON); } break;
default: m_red(ON);
}
float s_Vert = sqrt(max); // find the pixel dist b/w stars 2 & 4
float scale = s_Vert/29.0; // find the ration of pixel/cm
// find the location of global center in local coordinates.
long x_cent, y_cent;
x_cent = ( X[ s_map[1] ] + X[s_map[3]] ) / 2.0 ; // define the local position of the origin
y_cent = ( Y[ s_map[1] ] + Y[s_map[3]] ) / 2.0 ; // define the local position of the origin
// find the dist from bot to global center
long r = (long) sqrt( (float)x_cent*x_cent +(float)y_cent*y_cent)/scale;
// find the angle between the local frame and the global frame
float theta = - atan2( X[ s_map[1] ] - X[s_map[3]] , Y[ s_map[1] ] - Y[s_map[3]] );
// find the angle between the local x-axis and the vector to the global origin
float alpha = atan2( y_cent , x_cent );
// find the angle between the global x-axis and the vector to the origin
float phi = -theta +alpha;
int XBOT =(int) r*cos(phi); // determine the bot's global position
int YBOT =(int) -r*sin(phi); // ^
position[0] =(int) XBOT;// x_cent/scale;
position[1] =(int) YBOT; //y_cent/scale;
bot_angle[0]= theta - PI/2;
}
//c7 red
/// e6 blue
int main(void)
{
m_disableJTAG(); // Turn off JTAG
//m_red(ON); // check code runs
m_clockdivide(0); // clock speed 16 MHz
m_bus_init(); // initialize m_bus communications
m_usb_init(); // USB COM
setup(); // motor setup
ADC_setup(); // setup anlog conversions
// start wireless communications
m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
m_wii_open(); // and open wii camera
sei(); // enable global interrups
set(DDRC,7);
set(DDRB,7);
int position[3]; // array for robot position
int x_curr; // current x position
int y_curr; // current y position
int theta_curr; // current angle
int i;
int a;
// x, y and theta values
int x_pos[11];
int y_pos[11];
int theta[11];
char send[10];
int side;
int x1;
// calculate initial positions
int status;
// int x;
// int y;
// int theta1;
int hasPuck = 0;
// main LOOP
while(1)
{
if (flag)
{
m_rf_read(buffer1, PACKET_LENGTH);
testing = (unsigned char)buffer1[0];
a = state(testing);
// m_rf_read(buffer, PACKET_LENGTH);
// x = (unsigned char)buffer[0];
// y = (unsigned char)buffer[1];
// theta1 = (unsigned char)buffer[2];
if (flag1){
for(i = 0; i<10; i++)
{
current_location(position);
x1 = position[0];
}
// figure out what side robot starts on
if (x1 > 0)
{
side = -1;
}
else
{
side = 1;
}
}
flag = 0;
}
// m_rf_open(CHANNEL, RXADDRESS, PACKET_LENGTH);
// store 10 most recent position readings
for (i=0; i < 11; i++)
{
current_location(position);
x_pos[i] = position[0];
y_pos[i] = position[1];
theta[i] = position[2];
}
// filter position
filter(x_pos,y_pos,theta);
// current position
x_curr = x_pos[11];
y_curr = y_pos[11];
theta_curr = theta[11];
if (x_curr > 0)
{
m_red(ON);
}
else if (x_curr<0)
{
m_red(OFF);
}
if (y_curr > 0)
{
m_green(ON);
}
else if (y_curr<0)
{
m_green(OFF);
}
// send[0] = x_curr;//(char) x_curr;
// send[1] = y_curr;//(char) y_curr;
// send[2] = theta_curr;//(char) theta_curr;
// status = m_rf_send(TXaddress, send, PACKET_LENGTH);
// if (status){m_red(TOGGLE)};
// m_usb_tx_string("X Pos: ");
// m_usb_tx_int(x);
// m_usb_tx_string(" ");
// m_usb_tx_string("Y POSITION: ");
// m_usb_tx_int(y);
// m_usb_tx_string(" ");
// m_usb_tx_string("ANGLE: ");
// m_usb_tx_int(theta1);
// m_usb_tx_string(" ");
// m_usb_tx_string("\n");
a = 2;
// COMM TEST
if (a == 1)
{
// ADD CODE
// m_red(ON);
if (side == 1)
{
set(PORTC,7);
}
else {
set(PORTB,7);
}
}
// PLAY GAME
else if (a == 2) {
// m_red(OFF);
//int hasPuck = follow_puck();
//hasPuck = 1;
// goalie(x_curr, y_curr, theta_curr, side);
if(hasPuck)
{
//score_goal(x_curr, y_curr, theta_curr, side);
//turn_robot(0,0,0,0);
}
}
// GOAL R
else if (a == 3)
{
//side = 1;
//m_green(TOGGLE);
}
// GOAL B
else if ( a == 4)
{
//side = -1;
//m_green(TOGGLE);
}
else if(a == 5)
{
turn_robot(0,0,0,0);
// m_red(ON);
}
// PAUSE, HALF TIME, GAME OVER
if (a == 6 || a == 7)
{
// STOP ROBOT
turn_robot(0,0,0,0);
m_red(ON);
}
else
{}
// // COMM PROTOCOL INSTRUCTIONS
// m_usb_tx_string("STATE: ");
// m_usb_tx_int(a);
// m_usb_tx_string(" ");
// m_usb_tx_string("FLAG: ");
// m_usb_tx_int(flag);
// m_usb_tx_string(" ");
// m_usb_tx_string("TESTING: ");
// m_usb_tx_int(testing);
// m_usb_tx_string(" ");
// m_usb_tx_string("\n");
}
}
/************************************************************
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);
}
}
}
