void track(int color)
{
track_update();
if(track_size(color,0) > TRACKSIZETHRESHOLD) //if object with color is visible
{
create_drive_direct(MAXSPEED+(track_x(color,0)-80)*TURNSPEED,MAXSPEED-(track_x(color,0)-80)*TURNSPEED);
}
else //if object with color is not visible
{
create_drive_direct(200,-200);
}
}
/*Function definitions go below.*/
void CenterCamera2() //CAMERA MUST BE AT CENTER OF CREATE, POINTING Right RESPECT TO CREATE
{
if (track_x(0,0) > 81) //if blocks are bd
{
for(create_spin_CCW(100); track_x(0,0) > 81; track_update()){} //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY -- remember to calibrate CBC to blocks
create_stop();
}
if (track_x(0,0) < 78) //if blocks are fd
{
for(create_spin_CW(100); track_x(0,0) < 78; track_update()){} //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY -- remember to calibrate CBC to blocks
create_stop();
}
}
void FORTE_TrkEclipse::executeEvent(int pa_nEIID){
QO() = QI();
switch (pa_nEIID){
case scm_nEventREQID:
if(true == QI()){
//TODO: test with and without type casts
track_update(); // get new image data
COUNT() = static_cast<TForteInt16>(track_count(CH()));
if(COUNT() > I_BLOB()){ // there is at least I_BLOBs on CH()
SIZE() = static_cast<TForteInt16>(track_size(CH(), I_BLOB())); //
FRAME() = static_cast<TForteInt16>(track_get_frame()); //
CONFIDENCE() = static_cast<TForteInt16>(track_confidence(CH(), I_BLOB()));
X() = static_cast<TForteInt16>(track_x(CH(), I_BLOB())); // get image data
Y() = static_cast<TForteInt16>(track_y(CH(), I_BLOB())); // and data
MAJOR() = static_cast<TForteInt16>(track_major_axis(CH(), I_BLOB())); // get image data
MINOR() = static_cast<TForteInt16>(track_minor_axis(CH(), I_BLOB())); // and data
if(true == INFO()){
if(COUNT() > I_BLOB()){ // there is a blob
printf("On Channel=%d Eclipse Blob#%d is at (X,Y,MAJOR,MINOR)=(%d,%d,%d,%d)\n", (int) CH(), (int) I_BLOB(), (int) X(), (int) Y(), (int) MAJOR(), (int) MINOR());
printf("\t Confidence=%d; Size=%d;Frame=%d\n", (int) CONFIDENCE(), (int) SIZE(), (int) FRAME());
}
else{
printf("No Blob#%i in sight on Color Channel %i\n", (int) I_BLOB(), (int) CH());
}
}
}
}
sendOutputEvent(scm_nEventCNFID);
break;
}
}
void CenterCamera() //CAMERA MUST BE AT CENTER OF CREATE, POINTING RIGHT RESPECT TO CREATE
{
create_drive_straight(-200);
while (track_x (0,0) > 84) //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY
{
track_update();
}
create_stop();
if (track_x (0,0) < 75)
{
create_drive_straight(200);
while (track_x (0,0) < 75)
{
track_update();
}
create_stop();
}
}
int main ()
{
float CENTER = 79.5;
float OFFSET = 0.05;
drone_connect();
enable_drone_vision();
drone_front_camera();
sleep(2);
drone_takeoff();
printf("Black button to exit test loop and land the drone");
float x, y;
track_update();
while(!black_button())
{
x = OFFSET * ((track_x(0,0) - CENTER) / CENTER);
y = OFFSET * ((track_y(0,0) - CENTER) / CENTER);
if(track_count(0) > 0)
{
printf("Items tracking: %d\n", track_count(0));
drone_move(0,0, x, y);
}
els
{
printf("No items: Hovering \n");
drone_hover();
}
sleep(4);
track_update();
}
printf("Landing");
drone_move(0,0,0,-.25);
sleep(0.5);
drone_land();
drone_disconnect();
return 0;
}
/* Global variables go below (if you absolutely need them).*/
int look_for_color_green() {
int x, y, color=2; // set up for color channel 2 (green)
while (black_button() == 0) //run till button is pressed
{
track_update(); // process the most recent image
if (track_count(color) > 0)
{ // get x, y for the biggest blob the channel sees
x = track_x(color,2); y = track_y(color,2);
printf("Biggest blob at (%d,%d)\n",x,y);
}
else
{ printf("No color match in Frame\n"); }
sleep(0.2); // give print time to register
}
printf("Program is done.\n");
}
void cameratrack()
{
//enable_servos();
//set_servo_position(1,0);
track_update();
if (track_x(0,0) < track_x(2,0)) //Pink on left
{
track_update();
while (track_x (0,0) > 82 || track_x (0,0) <77)
{
if (track_x(0, 0) > 82)
{
while(track_x(0,0) > 82)
{
track_update();
left(700);
}
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(0, 0) < 77)
{
track_update();
right(700);
}
}
}
ao();
movecenterfast(800);
}
else
{
track_update();
while (track_x (2,0) > 82 || track_x (2,0) < 77)
{
if (track_x(2, 0) > 82)
{
while(track_x(2,0) > 82)
{
track_update();
left(100);
}
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(2, 0) < 77)
{
track_update();
right(100);
}
}
}
ao();
movecenterfast(800);
// set_servo_position(1,800);
}
}
int main()
{
//Init Variables
int color = RED;
int lastSeen = right;
int pumpTime = 0;
//Enable Servos
enable_servos();
sleep(.2);
beep();
while(1) {
//Check if we have time left on our Pump Counter
if(pumpTime > 0) {
//Turn Pump On
set_servo_position(servoPump, pumpOn);
}
else {
//Turn Pump Off
set_servo_position(servoPump, pumpOff);
}
//Image Capture
track_update();
//Check if We See a blob
if(track_count(color) > 0 ) {
//Find out where blobs are
//Left
if( track_x(color, 0) < 80) {
move(diagLeft, 500);
lastSeen = left;
}
//Right
else if( track_x(color, 0) > 120 ) {
move(diagRight, 500);
lastSeen = right;
}
//Middle
else {
move(forward, 500);
}
//Check if the Blob is close
if( track_bbox_top(color, 0) > 80) {
//Motors Off
ao();
//Extend Arm (Triggers Pneumatics)
set_servo_position(servoSwitch, 44);
sleep(1);
//Retract Arm (Triggers Pneumatics)
set_servo_position(servoSwitch, 1024);
sleep(1);
//We need to refill tanks
pumpTime += 100;
}
//****Screen Debug Info****
//printf("red blobs: %d\n", track_count(color));
printf("Blob X Cord: %d\n", track_x(color,0));
printf("Blob Y Cord: %d\n", track_y(color,0));
//printf("Blob Box Height: %d\n", track_bbox_height(color,0));
//printf("Blob Box Width: %d\n", track_bbox_width(color,0));
printf("track_size: %d\n", track_size(color, 0));
printf( "top y: %d\n\n", track_bbox_top(color, 0));
}
//Else We didn't find a blob
else{
printf( "Don't see the color blob.\n" );
// Search for ball using last known direction
move(lastSeen, 500);
}
//Decrement PumpTime
if(pumpTime > 0) pumpTime--;
}
}
void Luggagecartcafe()
{
enable_servos();
set_servo_position(1,200);
track_update();
int X = 69;
if (track_x(0,0) < track_x(2,0)) //Pink on left
{
track_update();
while (track_x (0,0) > X+5 || track_x (0,0) < X-5)
{
if (track_x(0, 0) > X+5)
{
while(track_x(0,0) > X)
{
track_update();
left(250);
}
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(0, 0) < X)
{
track_update();
right(250);
}
ao();
}
}
}
else if (track_x(2,0) < track_x(0,0)) //Pink on left
{
track_update();
while (track_x (2,0) > X+5 || track_x (2,0) < X-5)
{
if (track_x(2, 0) > X+5)
{
while(track_x(2,0) > X+5)
{
track_update();
left(250);
}
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(2, 0) < X-5)
{
track_update();
right(250);
}
}
}
}
}
void CenterCamera() //CAMERA MUST BE AT CENTER OF CREATE, POINTING Right RESPECT TO CREATE
{
track_update();
if (track_x (0,0) > 81)
{
create_drive_straight(-200);
while (track_x (0,0) > 81) //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY
{
track_update();
}
create_stop();
drivemm(inchestomm(4.9), 1);
set_create_total_angle(0);
create_drive(101.3,1); //uses int. does it round?
sleep(2.057252314);
create_stop();
get_create_total_angle(.1);
printf("asdb");
if (get_create_total_angle(.1) != 90)
{
set_create_total_angle(get_create_total_angle(.1) - 90);
printf("%f\n",get_create_total_angle(.1));
if (get_create_total_angle(.1) > 0) //CCW ADDS TO TOTAL ANGLE
{
create_spin_CW(100);
while (get_create_total_angle(.1) < 0);
{
printf("aeqweqw");
}
create_stop();
}
if (get_create_total_angle(.1) < 0)
{
create_spin_CCW(100);
while (get_create_total_angle(.1) > 0);
{
printf("BBBB");
}
create_stop();
}
}
}
/* (track_x (0,0) < 79)
{
create_drive_straight(200);
while (track_x (0,0) < 79)
{
track_update();
}
create_stop();
beep();
set_create_distance(0);
drivemm(-inchestomm(5), 1);
create_drive(206.972051,1);
sleep(1);
create_stop();
create_drive_straight(-200);
while(digital(8 == 0))
{
}
create_stop();
}*/
}
int main()
{
/*printf("Press A to start\n");
while (a_button() == 0)
{
}
*/
/*
while (track_count(0) > 0 && track_count(2) == 0) //If seeing pink, not green, turn right
{
track_update();
right(250);
}
while (track_count(2) > 0 && track_count(0) == 0) //If seeing green, not pink, turn left
{
track_update();
left(250);
}*/
track_update();
if (track_x(0,0) < track_x(2,0))//track_x(0,0) > X || track_x(2,0) < rightX)
{
//while (track(0,0) < track_x(2,0))//track_x(0,0) > X || track_x(2,0) < rightX)
//{
//int X = 0;
//int Y=0;
track_update();
if (track_x(0, 0) > X)
{
while(track_x(0,0) > X)
{
track_update();
left(100);
}
//X = X+30;
}
ao();
if (track_x(0,0) < X)
{
while (track_x(0, 0) < X)
{
track_update();
right(100);
}
//Y = Y+30;
}
ao();
//int i = 0;
while (a_button() == 0)//while (i < 15)
{
//printf("%d\n", i);
//i = i + 1;
forward(500);
}
}
if (track_x(0,0) > track_x(2,0))
{
//while (track(0,0) < track_x(2,0))//track_x(2,0) > X || track_x(0,0) < rightX)
//{
//track_update();
// int X = 0;
// int Y=0;
// if (track_x(0, 0) > X)
//{
while(track_x(2,0) > X)
{
track_update();
left(100);
}
//X = X+30;
//}
ao();
//if (track_x(0,0) < rightX)
//{
while (track_x(0, 0) < rightX)
{
track_update();
right(100);
}
//Y = Y+30;
//}
}
ao();
//int i = 0;
while (a_button() == 0)//while (i < 15)
{
//printf("%d\n", i);
//i = i + 1;
forward(500);
}
}
/**
* process_and_show_images
*
* show the different images and save a video file with the results
*
*/
int
process_and_show_images (const char* name_output_video,
track_t list_of_tracks [MAX_CAM - MIN_CAM][MAX_FRAMES])
{
IplImage *ground_plane_image = NULL, *cam[MAX_CAM - MIN_CAM];
char cam_name [5][255];
IplImage *DispImage;
int current_frame, i, j ,k;
float x_fused = 0.0, y_fused = 0.0, x_fused_errors = 0.0, y_fused_errors = 0.0;
CvVideoWriter *writer = NULL;
CvSize s = cvSize (X_RESOLUTION, Y_RESOLUTION);
int contcam[MAX_CAM - MIN_CAM] = {0, 0, 0, 0, 0};
float acumcam_x[MAX_CAM - MIN_CAM] = {0.0, 0.0, 0.0, 0.0, 0.0};
float acumcam_y[MAX_CAM - MIN_CAM] = {0.0, 0.0, 0.0, 0.0, 0.0};
#ifdef DEBUG
printf ("process_and_show_images() \n");
#endif
#ifdef SHOW_IMAGE
printf ("Creating output video file: %s\n", name_output_video);
writer = cvCreateVideoWriter (name_output_video, CV_FOURCC ('M', 'J', 'P', 'G'), 15, s, 1);
//writer = cvCreateVideoWriter (name_output_video, -1, 15, s, 1);
cvNamedWindow ("Apidis", CV_WINDOW_AUTOSIZE);
DispImage = cvCreateImage (cvSize (X_RESOLUTION, Y_RESOLUTION), IPL_DEPTH_8U, 3);
#endif
/* for each frame */
for (current_frame = 0; current_frame < MAX_FRAMES; current_frame++) {
printf (".");
fflush (stdout);
#ifdef SHOW_IMAGE
sprintf (cam_name[0], "%s-%04d.jpeg", LOCATION_CAM1_IMAGES, current_frame);
sprintf (cam_name[1], "%s-%04d.jpeg", LOCATION_CAM2_IMAGES, current_frame);
sprintf (cam_name[2], "%s-%04d.jpeg", LOCATION_CAM4_IMAGES, current_frame);
/* Note that cam3 and cam4 are mapped to camera 6 and camera 7 images */
sprintf (cam_name[3], "%s-%04d.jpeg", LOCATION_CAM6_IMAGES, current_frame);
sprintf (cam_name[4], "%s-%04d.jpeg", LOCATION_CAM7_IMAGES, current_frame);
for (i = 0; i < MAX_CAM; i++) {
cam[i] = cvLoadImage (cam_name[i], CV_LOAD_IMAGE_COLOR);
if (cam[i] == NULL) {
printf ("Error opening image: %s \n", cam_name[i]);
exit (-1);
}
}
if (ground_plane_image == NULL)
ground_plane_image = cvLoadImage (LOCATION_GROUND_PLANE_IMAGE, CV_LOAD_IMAGE_COLOR);
cvSetImageROI (DispImage, cvRect (0, 0, 400, 300));
#endif
for (i = 0; i < MAX_CAM; i++) {
if (track_x (i, current_frame) != 0) { /* if camera 1 in current frame have information */
#ifdef SHOW_IMAGE
cvRectangle (cam[i], cvPoint ((int) track_x (i, current_frame), (int) track_y (i, current_frame)),
cvPoint ((int) track_x (i, current_frame) + track_w (i, current_frame),
(int) track_y (i, current_frame) + track_h (i, current_frame)), CV_RGB (255,255,0), 2, 8, 0);
cvRectangle (cam[i], cvPoint ((int) track_x_kalman (i, current_frame), (int) track_y_kalman (i, current_frame)),
cvPoint ((int) track_x_kalman (i, current_frame) + track_w_kalman (i, current_frame),
(int) track_y_kalman (i, current_frame) + track_h_kalman (i, current_frame)), CV_RGB (255,0,0), 2, 8, 0);
#endif
if (contcam[i] < 5) /* wait for having 5 values */
contcam[i]++;
if (contcam[i] == 5) {/* save stats */
/* get the current mean */
acumcam_x[i] = 0.0;
acumcam_y[i] = 0.0;
for (j = 0; j< 5; j++) {
acumcam_x[i] += track_px_global_kalman (i, current_frame - j);
acumcam_y[i] += track_py_global_kalman (i, current_frame - j);
}
px_global_mean_5 (i, current_frame) = acumcam_x[i] / (float) contcam[i];
py_global_mean_5 (i, current_frame) = acumcam_y[i] / (float) contcam[i];
for (j = 0; j < 5; j++) {
px_global_var_5 (i, current_frame) +=
pow(track_px_global (i, current_frame-j) - px_global_mean_5 (i, current_frame),2);
py_global_var_5 (i, current_frame) +=
pow(track_py_global (i, current_frame-j) - py_global_mean_5 (i, current_frame),2);
}
px_global_var_5 (i, current_frame) = px_global_var_5 (i, current_frame) / 5.0;
py_global_var_5 (i, current_frame) = py_global_var_5 (i, current_frame) / 5.0;
}
}
/* if we lost the objective initialize */
if (track_x (i, current_frame) == 0)
contcam[i] = 0;
#ifdef SHOW_IMAGE
cvResize (cam[i], DispImage, CV_INTER_AREA);
cvResetImageROI (DispImage);
if (i == 0)
cvSetImageROI (DispImage, cvRect (400, 0, 400, 300));
if (i == 1)
cvSetImageROI (DispImage, cvRect (800, 0, 400, 300));
if (i == 2)
cvSetImageROI (DispImage, cvRect (0, 300, 400, 300));
if (i == 3)
cvSetImageROI (DispImage, cvRect (400, 300, 400, 300));
if (i == 4)
cvSetImageROI (DispImage, cvRect (800, 300, 400, 300));
#endif
} /* for each camera */
x_fused = fused_px_global (current_frame, list_of_tracks);
y_fused = fused_py_global (current_frame, list_of_tracks);
x_fused_errors = fused_px_global_errors (current_frame, list_of_tracks);
y_fused_errors = fused_py_global_errors (current_frame, list_of_tracks);
/* for each camera, save the fused data in the data structure list_of_tracks */
for (k = 0; k < (MAX_CAM - MIN_CAM); k++) {
track_fused_x (k, current_frame) = x_fused;
track_fused_y (k, current_frame) = y_fused;
}
/* save the fused error (difference between filtered data and fused data) */
for (k = 0; k < (MAX_CAM - MIN_CAM); k++) {
if (track_py_local (k, current_frame) > 0) {
track_error_x (k, current_frame) = abs (track_fused_x (k, current_frame) - track_px_global_kalman (k, current_frame));
track_error_y (k, current_frame) = abs (track_fused_y (k, current_frame) - track_py_global_kalman (k, current_frame));
}
}
/* get the absolute errors between local data and fused data */
obtain_track_errors (current_frame, list_of_tracks);
#ifdef SHOW_IMAGE
/* drawing the player positions on the ground plane */
cvCircle (ground_plane_image, cvPoint (((int)(track_px_global (1, current_frame)/SCALE_X)),
((int) (track_py_global(1, current_frame)/SCALE_Y))) , 4, CV_RGB(255, 0, 0), CV_FILLED, CV_AA, 0);
printf ("Frame:%d, establishing player @: %f,%f --%f,%f --%f,%f\n", current_frame, track_fused_x(0, current_frame),
track_fused_y(0, current_frame), x_fused, y_fused, track_px_global(1, current_frame), track_py_global (1, current_frame));
cvResize (ground_plane_image, DispImage, CV_INTER_AREA);
cvResetImageROI (DispImage);
cvShowImage ("Apidis", DispImage);
cvWriteFrame (writer, DispImage);
/* free the memory */
for (i = 0; i < MAX_CAM; i++)
cvReleaseImage (&cam[i]);
cvWaitKey(2);
#endif
} /* for each frame */
/* get the mean errors of each camera */
obtain_mean_errors (list_of_tracks);
#ifdef SHOW_IMAGE
cvReleaseImage (&ground_plane_image);
cvReleaseVideoWriter (&writer);
cvDestroyWindow ("Apidis");
#endif
return 0;
}
void LuggageTrack()
{
enable_servos();
set_servo_position(1,0);
track_update();
int X = 73; //x coord of center of left cart
int y = 5; //how far right/left of center luggage carts can be
if (track_x(0,0) < track_x(2,0)) //Pink on left
{
track_update();
while (track_x (0,0) < X-y || track_x (0,0) > X+y)
{
if (track_x(0, 0) > X+y)
{
while(track_x(0,0) > X+y)
{
track_update();
spinCW(50);
}
ao();
}
else if (track_x(0,0) < X-y)
{
while (track_x(0, 0) < X-y)
{
track_update();
spinCCW(50);
}
}
}
}
else
{
track_update();
while (track_x (2,0) > X+y || track_x (2,0) < X-y)
{
if (track_x(2, 0) > X+y)
{
while(track_x(2,0) > X+y)
{
track_update();
spinCCW(50);
}
ao();
}
else if (track_x(0,0) < X-y)
{
while (track_x(2, 0) < X-y)
{
track_update();
spinCW(50);
}
ao();
}
}
}
}
void LuggageTrack2()
{
enable_servos();
z = 0;
set_servo_position(1,0);
track_update();
int X = 72;
if (track_x(0,0) < track_x(2,0)) //Pink on left
{
track_update();
while (track_x (0,0) > X+5 || track_x (0,0) < X-5)
{
if (track_x(0, 0) > X+5)
{
while(track_x(0,0) > X+5)
{
track_update();
spinCW(30);
}
z=1;
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(0, 0) < X-5)
{
track_update();
spinCCW(30);
}
z=1;
ao();
}
}
ao();
movecenterfast(500);
while (digital(12) == 0)
{}
ao();
set_servo_position(1,800);
}
else
{
track_update();
while (track_x (2,0) > X+5 || track_x (2,0) < X-5)
{
if (track_x(2, 0) > X+5)
{
while(track_x(2,0) > X+5)
{
track_update();
spinCW(30);
}
z=1;
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(2, 0) < X-5)
{
track_update();
spinCCW(30);
}
z=1;
ao();
}
}
ao();
movecenterfast(500);
}
}
void cameratrack()
{
//enable_servos();
//set_servo_position(1,0);
track_update();//=========00
//while(digital(12) == 0)
//{
if (track_x(0,0) < track_x(2,0)) //Pink on left
{
track_update();
while (track_x (0,0) > 82 || track_x (0,0) <77)
{
while(digital(12) == 0)
{ //ANDREW
if (track_x(0, 0) > 82)
{
while(track_x(0,0) > 82)
{
while(digital(12) ==0){
track_update();
left(400);
}
}
ao();
}
}
}
}
/*
else // (track_x(0,0) < X)
{
while(digital(12) == 0)
{//ANDREW
while (track_x(0, 0) < 77)
{
while(digital(12) == 0){
track_update();
right(400);
}
}
}//ANDREW
}
ao();
movecenterfast(400);
if (track_x(0,0) > track_x(2,0))*/
else if (track_x(0,0) > track_x(2,0));
{
track_update();
while (track_x (2,0) > 82 || track_x (2,0) < 77)
{
while(digital(12) == 0)
{//ANDREW
if (track_x(2, 0) > 82)
{
while(track_x(2,0) > 82){//00000
while(digital(12) == 0){
track_update();
left(100);
}
}
}
ao();
}
}//ANDREW
}/*
else // (track_x(0,0) < X)
{
while(digital(12) == 0)
{//ANDREW
while (track_x(2, 0) < 77)
{
while(digital(12) == 0){
track_update();
right(100);
}
}
}//ANDREW
}
}
ao();
//movecenterfast(800);
// set_servo_position(1,800);*/
}
