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);
}
}
}
}
char current_location(int* position)
{
// int buffer to hold blob data
unsigned int buffer[12];
// read wii camera blob data into int buffer
m_wii_read(buffer);
// points
int x1 = (int)buffer[0];
int y1 = (int)buffer[1];
int x2 = (int)buffer[3];
int y2 = (int)buffer[4];
int x3 = (int)buffer[6];
int y3 = (int)buffer[7];
int x4 = (int)buffer[9];
int y4 = (int)buffer[10];
// array of points
int x_points[4] = {x1, x2, x3, x4};
int y_points[4] = {y1, y2, y3, y4};
int i;
for(i = 0; i<=4;i++)
{
if (x_points[i]==1023 && y_points[i]==1023)
{
x_points[i] = NaN;
y_points[i] = NaN;
}
}
// calculate all possible distances
int d12 = calc_dist(x_points[0], y_points[0], x_points[1], y_points[1]);
int d13 = calc_dist(x_points[0], y_points[0], x_points[2], y_points[2]);
int d14 = calc_dist(x_points[0], y_points[0], x_points[3], y_points[3]);
int d23 = calc_dist(x_points[1], y_points[1], x_points[2], y_points[2]);
int d24 = calc_dist(x_points[1], y_points[1], x_points[3], y_points[3]);
int d34 = calc_dist(x_points[2], y_points[2], x_points[3], y_points[3]);
// store all distances in an array
int all_distances[6] = {d12,d13,d14,d23,d24,d34};
// store all pair
int all_pairs[6][2] = {{0,1},{0,2},{0,3},{1,2},{1,3},{2,3}};
// index and value of max and min distance
int max_dat[2];
int min_dat[2];
// fill min and max arrays
max_array(all_distances, 6, max_dat);
min_array(all_distances, 6, min_dat);
// index of max and min points
int max_index = max_dat[1];
int min_index = min_dat[1];
// array of pair indicies
int long_pair[2] = {all_pairs[max_index][0], all_pairs[max_index][1]};
int short_pair[2] = {all_pairs[min_index][0], all_pairs[min_index][1]};
int top_point[2];
int bottom_point[2];
// finds top and bottom points
if (long_pair[0] == short_pair[0] || long_pair[0] == short_pair[1])
{
top_point[0] = x_points[long_pair[0]];
top_point[1] = y_points[long_pair[0]];
bottom_point[0] = x_points[long_pair[1]];
bottom_point[1] = y_points[long_pair[1]];
}
else
{
top_point[0] =x_points[long_pair[1]];
top_point[1] =y_points[long_pair[1]];
bottom_point[0] =x_points[long_pair[0]];
bottom_point[1] =y_points[long_pair[0]];
}
// calculate center of rink
double x_center = ((double)top_point[0] + (double)bottom_point[0])/2;
double y_center = ((double)top_point[1] + (double)bottom_point[1])/2;
//double theta = acos(y_unit);
double x_diff = (double)top_point[0] - x_center;
double y_diff =(double)top_point[1] - y_center;
// calculate position of camera relative to camera coordinate axes
double camera_x = 512;
double camera_y = 384;
// calculate angle theta
double theta = atan2(x_diff, y_diff);
// rotated points
float rotated_x = ((((camera_x-x_center)*cos(theta)) + ((camera_y-y_center)*-sin(theta))) + camera_x);
float rotated_y = ((((camera_x-x_center)*sin(theta)) + ((camera_y-y_center)*cos(theta))) + camera_y);
// position and angle relative to rink axes
position[0] = (int) (10*((512 - (rotated_x)))/28);
position[1] = (int) (10*(((rotated_y) - 384))/28);
position[2] = (int) (theta*(180/M_PI));
return 1;
}
void localizef(void){
//long PtoC = .30851; //converts pixels to centimeters
// if x(1) ~= 1023 && x(2) ~= 1023 && x(3) ~= 1023 && x(4) ~= 1023 && y(1) ~= 1023 && y(2) ~= 1023 && y(3) ~= 1023 && y(4) ~= 1023
m_wii_read(blobs);
x_1 = blobs[0];
y_1 = blobs[1];
x_2 = blobs[3];
y_2 = blobs[4];
x_3 = blobs[6];
y_3 = blobs[7];
x_4 = blobs[9];
y_4 = blobs[10];
if( (x_1 == 1023 && y_1 == 1023) ||(x_2 == 1023 && y_2 == 1023) || (x_3 == 1023 && y_3 == 1023) ||(x_4 == 1023 && y_4 == 1023)){
return;
}
OneTwo=sqrt(((x_2-x_1)*(x_2-x_1))+((y_2-y_1)*(y_2-y_1)));//Find distances to all possible stars
OneThree=sqrt(((x_3-x_1)*(x_3-x_1))+((y_3-y_1)*(y_3-y_1)));
OneFour=sqrt(((x_4-x_1)*(x_4-x_1))+((y_4-y_1)*(y_4-y_1)));
TwoThree=sqrt(((x_3-x_2)*(x_3-x_2))+((y_3-y_2)*(y_3-y_2)));
TwoFour=sqrt(((x_4-x_2)*(x_4-x_2))+((y_4-y_2)*(y_4-y_2)));
ThreeFour=sqrt(((x_4-x_3)*(x_4-x_3))+((y_4-y_3)*(y_4-y_3)));
D[0] = OneTwo;
D[1] = OneThree;
D[2] = OneFour;
D[3] = TwoThree;
D[4] = TwoFour;
D[5] = ThreeFour;
// a = [d1, d2, d3, d4, d5, d6];
// O(n) = n;
int i = 0;
big = 0;
next = 0;
for(i=0;i<6;i++){
if(D[i]>big){
next=big;
big=D[i];
}
else if (D[i] > next && D[i] != big){
next = D[i];
}
}
// m_usb_tx_uint(D[0]);
// m_usb_tx_string(" ");
//----------------------------------------1
if (big == OneTwo){
if(next == TwoThree){
StarA[0] = x_1;
StarA[1] = y_1;
StarB[0] = x_3;
StarB[1] = y_3;
StarC[0] = x_2;
StarC[1] = y_2;
StarD[0] = x_4;
StarD[1] = y_4;
}
if (next == TwoFour) {
StarA[0] = x_1;
StarA[1] = y_1;
StarB[0] = x_4;
StarB[1] = y_4;
StarC[0] = x_2;
StarC[1] = y_2;
StarD[0] = x_3;
StarD[1] = y_3;
}
if (next == OneThree){
StarA[0] = x_2;
StarA[1] = y_2;
StarB[0] = x_3;
StarB[1] = x_3;
StarC[0] = x_1;
StarC[1] = y_1;
StarD[0] = x_4;
StarD[1] = y_4;
}
if (next == OneFour){
StarA[0] = x_2;
StarA[1] = y_2;
StarB[0] = x_4;
StarB[1] = y_4;
StarC[0] = x_1;
StarC[1] = y_1;
StarD[0] = x_3;
StarD[1] = y_3;
}
}
//-------------------------------------2
if (big == OneThree){
if (next == TwoThree){
StarA[0] = x_1;
StarA[1] = y_1;
StarB[0] = x_2;
StarB[1] = y_2;
StarC[0] = x_3;
StarC[1] = y_3;
StarD[0] = x_4;
StarD[1] = y_4;
}
if (next == ThreeFour){
StarA[0] = x_1;
StarA[1] = y_1;
StarB[0] = x_4;
StarB[1] = y_4;
StarC[0] = x_3;
StarC[1] = y_3;
StarD[0] = x_2;
StarD[1] = y_2;
}
if (next == OneTwo){
StarA[0] = x_3;
StarA[1] = y_3;
StarB[0] = x_2;
StarB[1] = y_2;
StarC[0] = x_1;
StarC[1] = y_1;
StarD[0] = x_4;
StarD[1] = y_4;
}
if (next == OneFour){
StarA[0] = x_3;
StarA[1] = y_3;
StarB[0] = x_4;
StarB[1] = y_4;
StarC[0] = x_1;
StarC[1] = y_1;
StarD[0] = x_2;
StarD[1] = y_2;
}
}
//-------------------------------------3
if (big == OneFour){
if (next == ThreeFour){
StarA[0] = x_1;
StarA[1] = y_1;
StarB[0] = x_3;
StarB[1] = y_3;
StarC[0] = x_4;
StarC[1] = y_4;
StarD[0] = x_2;
StarD[1] = y_2;
}
if (next == TwoFour){
StarA[0] = x_1;
StarA[1] = y_1;
StarB[0] = x_2;
StarB[1] = y_2;
StarC[0] = x_4;
StarC[1] = y_4;
StarD[0] = x_3;
StarD[1] = y_3;
}
if (next == OneThree){
StarA[0] = x_4;
StarA[1] = y_4;
StarB[0] = x_3;
StarB[1] = y_3;
StarC[0] = x_1;
StarC[1] = y_1;
StarD[0] = x_2;
StarD[1] = y_2;
}
if (next == OneTwo){
StarA[0] = x_4;
StarA[1] = y_4;
StarB[0] = x_2;
StarB[1] = y_2;
StarC[0] = x_1;
StarC[1] = y_1;
StarD[0] = x_3;
StarD[1] = y_3;
}
}
//-------------------------------------4
if (big == TwoThree){
if (next == OneThree){
StarA[0] = x_2;
StarA[1] = y_2;
StarB[0] = x_1;
StarB[1] = y_1;
StarC[0] = x_3;
StarC[1] = y_3;
StarD[0] = x_4;
StarD[1] = y_4;
}
if (next == ThreeFour){
StarA[0] = x_2;
StarA[1] = y_2;
StarB[0] = x_4;
StarB[1] = y_4;
StarC[0] = x_3;
StarC[1] = y_3;
StarD[0] = x_1;
StarD[1] = y_1;
}
if (next == OneTwo){
StarA[0] = x_3;
StarA[1] = y_3;
StarB[0] = x_1;
StarB[1] = y_1;
StarC[0] = x_2;
StarC[1] = y_2;
StarD[0] = x_4;
StarD[1] = y_4;
}
if (next == TwoFour){
StarA[0] = x_3;
StarA[1] = y_3;
StarB[0] = x_4;
StarB[1] = y_4;
StarC[0] = x_2;
StarC[1] = y_2;
StarD[0] = x_1;
StarD[1] = y_1;
}
}
//-------------------------------------5
if (big == TwoFour){
if (next == ThreeFour){
StarA[0] = x_2;
StarA[1] = y_2;
StarB[0] = x_3;
StarB[1] = y_3;
StarC[0] = x_4;
StarC[1] = y_4;
StarD[0] = x_1;
StarD[1] = y_1;
}
if (next == OneFour){
StarA[0] = x_2;
StarA[1] = y_2;
StarB[0] = x_1;
StarB[1] = y_1;
StarC[0] = x_4;
StarC[1] = y_4;
StarD[0] = x_3;
StarD[1] = y_3;
}
if (next == TwoThree){
StarA[0] = x_4;
StarA[1] = y_4;
StarB[0] = x_3;
StarB[1] = y_3;
StarC[0] = x_2;
StarC[1] = y_2;
StarD[0] = x_1;
StarD[1] = y_1;
}
if (next == OneTwo){
StarA[0] = x_4;
StarA[1] = y_4;
StarB[0] = x_1;
StarB[1] = y_1;
StarC[0] = x_2;
StarC[1] = y_2;
StarD[0] = x_3;
StarD[1] = y_3;
}
}
//-------------------------------------6
if (big == ThreeFour){
if (next == OneFour){
StarA[0] = x_3;
StarA[1] = y_3;
StarB[0] = x_1;
StarB[1] = y_1;
StarC[0] = x_4;
StarC[1] = y_4;
StarD[0] = x_2;
StarD[1] = y_2;
}
if (next == TwoFour){
StarA[0] = x_3;
StarA[1] = y_3;
StarB[0] = x_2;
StarB[1] = y_2;
StarC[0] = x_4;
StarC[1] = y_4;
StarD[0] = x_1;
StarD[1] = y_1;
}
if (next == OneThree){
StarA[0] = x_4;
StarA[1] = y_4;
StarB[0] = x_1;
StarB[1] = y_1;
StarC[0] = x_3;
StarC[1] = y_3;
StarD[0] = x_2;
StarD[1] = y_2;
}
if (next == TwoThree){
StarA[0] = x_4;
StarA[1] = y_4;
StarB[0] = x_2;
StarB[1] = y_2;
StarC[0] = x_3;
StarC[1] = y_3;
StarD[0] = x_1;
StarD[1] = y_1;
}
}
//-----------------------FIND OFFSET--------------------------------------
AvgX = (StarA[0] + StarC[0])/2;
AvgY = (StarA[1] + StarC[1])/2;
OffsetX = AvgX - Center[0];
OffsetY = AvgY - Center[1];
//------------------------FIND THE ROTATION-------------------------------
changeX = StarC[0] - StarA[0];
changeY = StarC[1] - StarA[1];
// m_usb_tx_int(changeX);
// m_usb_tx_string(" ");
// m_usb_tx_int(changeY);
// m_usb_tx_string("\n");
Theta = atan2(changeY,changeX);
a = Theta*57.296;
XRobot = (OffsetY*cos(Theta) - OffsetX*sin(Theta)); //Converts the offset from the frame of the robot to the frame of the playing field
YRobot = (OffsetY*sin(Theta) + OffsetX*cos(Theta));
//m_usb_tx_int(XRobot);
//m_usb_tx_string(" ");
//m_usb_tx_int(YRobot);
//m_usb_tx_string("\n");
if (XRobot < 0) {
x_target = -280;
y_target = 0 ;
clearDisplay();
// m_port_set(0x20, PORTG,1); //Display 2
// m_port_set(0x20, PORTG,2);
// m_port_set(0x20, PORTG,3);
// m_port_set(0x20, PORTG,4);
// m_port_set(0x20, PORTG,6);
}
else {
x_target = 280;
y_target = 0;
clearDisplay();
// m_port_set(0x20, PORTG,1); //Display 3
// m_port_set(0x20, PORTG,3);
// m_port_set(0x20, PORTG,4);
// m_port_set(0x20, PORTG,4);
// m_port_set(0x20, PORTG,6);
}
m_red(OFF);
deltay = y_target-YRobot;
deltax = 1*(x_target-XRobot);
theta_target = atan2(deltay, deltax);
theta_rotate = theta_target - Theta;
d = -1*theta_target*57.3;
// d = theta_target*57.3;
// m_usb_tx_int(a);
// m_usb_tx_string(" ");
// m_usb_tx_int(d);
// m_usb_tx_string(" ");
// m_usb_tx_int(XRobot);
// m_usb_tx_string(" ");
// m_usb_tx_int(YRobot);
// m_usb_tx_string("\n");
}
void state_play()
{
unsigned int blobs[12];
double x = 0;
double y = 0;
double t = 0;
a_center = 1010;
a_left = 1005;
a_right = 1005;
a_pos = 1005;
while(1)
{
m_wii_read(blobs);
get_position(blobs, &x, &y, &t);
t = fix_theta(t);
if(first)
{
if(t < pi/2 || t > 3*pi/2)
{
direction = POSITIVE;
}
else
{
direction = NEGATIVE;
}
first = false;
}
adc();
// if one of the values sees the puck
if(a_center > 500 || a_left > 500 || a_right > 500)
{
// If the center value is the most itense
if(a_center >= a_left && a_center >= a_right)
{
// if it has possetion of the puck
if(a_pos > 1003)
{
if(y > 6)
{
if(direction == POSITIVE)
{
indicate_r();
turning = RIGHT;
if(t < 3*pi / 4 || t > 7*pi/8)
{
set_left(60);
set_right(10);
}
else
{
if(t < 3*pi / 2)
{
set_right(60);
set_left(10);
turning = LEFT;
}
else
{
set_left(60);
set_right(60);
}
}
}
else
{
indicate_l();
turning = LEFT;
if(t > pi / 4 && t < 5 * pi /4)
{
set_left(10);
set_right(60);
}
else
{
if(t < 3*pi/2)
{
set_left(60);
set_right(10);
turning = RIGHT;
}
else
{
set_left(60);
set_right(60);
}
}
}
}
else
{
if(y < -6)
{
if(direction == POSITIVE)
{
indicate_l();
turning = LEFT;
if(t < pi / 4 || t > 5 * pi / 4)
{
set_left(15);
set_right(60);
}
else
{
if(t > pi / 2)
{
set_left(60);
set_right(10);
turning = RIGHT;
}
else
{
set_left(60);
set_right(60);
}
}
}
else
{
indicate_r();
turning = RIGHT;
if(t < 3*pi/4 && t < 7*pi/8)
{
set_left(60);
set_right(15);
}
else
{
if(t >= 7*pi/ 8 || t < pi/2)
{
set_right(60);
set_left(10);
turning = LEFT;
}
else
{
set_right(60);
set_left(60);
}
}
}
}
else
{
indicate_f();
if(direction == POSITIVE)
{
if(t< pi && t > pi/8)
{
set_left(60);
set_right(0);
}
else
{
if(t > pi && t < 15*pi/16)
{
set_left(0);
set_right(60);
}
else
{
set_left(60);
set_right(60);
}
}
}
else
{
if(t< 7* pi/8 && t > 0)
{
set_left(0);
set_right(60);
}
else
{
if(t > 9*pi/8 && t < 2*pi)
{
set_left(60);
set_right(0);
}
else
{
set_left(60);
set_right(60);
}
}
}
}
}
}
// if it doesn't have possetion of the puck
else
{
indicate_f();
set_left(20);
set_right(20);
}
}
// if the center value is not the most intense value
else
{
// if the left value is the most itense
if(a_left >= a_center && a_left > a_right)
{
indicate_l();
if(a_left - 20 > a_center)
{
set_right(40);
set_left(-40);
}
else
{
set_left(0);
set_right(40);
}
}
// if the right value is the most intense
else
{
indicate_r();
if(a_right - 20 > a_center)
{
set_left(40);
set_right(-40);
}
else
{
set_left(40);
set_right(0);
}
}
}
}
// No puck found
else
{
indicate_n();
set_left(15);
set_right(-15);
}
if(wireless_buffer_f)
{
wireless_buffer_f = false;
switch(wireless_buffer[0])
{
case 0x01:
g_pos = true;
break;
case 0x02:
g_pos = false;
break;
case PAUSE:
state_pause();
return;
case PLAY:
1 == 1;
return;
case HALFTIME:
direction = !direction;
state_pause();
break;
case COMMTEST:
event_comm_test();
break;
default:
state_pause();
return;
}
}
}
/*
m_green(ON);
set_right(30);
set_left(-30);
for(int i = 0; i < 30 && !wireless_buffer_f; i++)
{
m_wait(100);
}
set_right(-20);
set_left(20);
for(int i = 0; i < 30 && !wireless_buffer_f; i++)
{
m_wait(100);
}
wireless_buffer_f = false;
char inst = wireless_buffer[0];
switch(inst)
{
case 0xA4:
state_pause();
break;
case 0xA1:
state_play();
return;
default:
state_before_game();
return;
}
*/
}
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()
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;
}
}
}
int main(void) {
m_clockdivide(0);
gen_setup();
unsigned int wii_data[12] = {0};
sei();
m_rf_open(chan,rf_address,p_len);
m_wii_read(wii_data);
localize( wii_data , position, b_angle); // <--- takes in wii_data and updates position in place to house {x,y,angle} of robot
int enemy_goalX;
int enemy_goalY = 0;
int own_goalX;
int own_goalY = 0;
int goalXPosition = 120;
if ((int)(position[0])<0){ enemy_goalX = goalXPosition;
own_goalX = -goalXPosition;}
else { enemy_goalX = -goalXPosition;
own_goalX = goalXPosition;}
find_puck();
set_motors(800,800);
//bool toggle = true;
int lastX = 0;
int lastY = 0;
int lastAng = 0;
while (1) {
//find angle to own Goal
rxOG = own_goalX - (int)position[0];
ryOG = own_goalY - (int)position[1];
psiOG = (int)(b_angle[0] * RAD_DEG_RATIO) - 90;
if (psiOG<-180) { psiOG +=360; }
if (psiOG>180) { psiOG -=360; }
//error in angle, factoring in rotational velocity should be taken care of by kalman filter
phi_robotOG = (int)(atan2(ryOG,rxOG) * RAD_DEG_RATIO); // compute angle from robot to target (in global coordinates)
theta_angleOG = (psiOG-phi_robotOG);
//make sure the data within -180 to 180 degrees for ease of calculation
if (theta_angleOG>180) { theta_angleOG-=360; }
if (theta_angleOG<=-180) { theta_angleOG+=360;}
/*if (debug_goto) {
m_usb_tx_string("\n\r");
m_usb_tx_int(theta_angleOG);
// m_usb_tx_string("\t");
//m_usb_tx_int(OCR1B);
m_usb_tx_string("\n\r");
}*/
last_Super_state = Super_state;
if (comm_cmd == true) { get_command(); }
if (check(TIFR3,OCF3A)){ ///// timer 3 runs at 100Hz
set(TIFR3,OCF3A);
if (timer_3_cnt == 1000){ timer_3_cnt = 0 ; } else{ timer_3_cnt++;} // timer_3_cnt counts 10 seconds and then rolls over
m_wii_read(wii_data);
localize( wii_data , position, b_angle); // <--- takes in wii_data and updates position in place to house {x,y,angle} of robot
m_rf_open(chan,rf_address,p_len);
find_puck();
if (get_unstuck && timer_3_cnt%100 == 0){
if ( (( lastX < position[0]+pos_change) && lastX > position[0]-pos_change)
&& ( lastY < position[1]+pos_change && (lastY > position[1]-pos_change) ) ){
// && ( ((int)lastAng < (int)b_angle*100 +20) && ((int)lastAng > (int)b_angle*100 - 20) )){
Super_state = New_Loc_super;
set_motors(-700,-700);
m_wait(200);
set_motors(0,0);
}
lastX = (int)position[0];
lastY = (int)position[1];
lastAng = (int)b_angle*100.0;
}
switch (Super_state){
case PT_super:
break;
case ADC_super:
ADC_go_to_puck();
break;
case To_Goal_super:
drivePuck(enemy_goalX, enemy_goalY ,position, b_angle[0]);
break;
case New_Loc_super:
break;
}
if (Super_state != To_Goal_super) keepSpin = 0;
if ( Matlab_pos_tracking ){ matlab_output( position, b_angle , wii_data ); }
}
}
}
