void cube_is_near(){
camera_update();
msleep(500);
camera_update();
enable_servos();
claw_open();
printf("cube is near");
freeze(Motor_Left);
freeze(Motor_Right);
if(get_object_count(0)!=0){
printf("gleich gefunden\n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
funden=1;
found_something();
}
}
double s=seconds();
motor(Motor_Right,30);
while(funden==0){//nichts gefunden
camera_update();
printf("nach rechts\n");
if(get_object_count!=0){
printf("1 gefunden \n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
found_something();
funden=1;
break;
}
}
if(seconds()>s+2){
printf("zeit rechts\n");
freeze(Motor_Right);
break;
}
}
s=seconds();
motor(Motor_Left,30);
while(funden==0){
camera_update();
printf("nach links drehen \n");
if(get_object_count!=0){
printf("1 gefunden \n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
found_something();
funden=1;
break;
}
}
if(seconds()>s+4){
printf("zeit links\n");
freeze(Motor_Left);
break;
}
}
}
void found_something(){
ao();
camera_update();
msleep(100);
camera_update();
msleep(100);
camera_update();
msleep(100);
while(get_object_bbox(0,0).width < 80){
camera_update();
if(get_object_center(0,0).x > 120)
{
motor(Motor_Left,Drivespeed_middle/2);
motor(Motor_Right,-Drivespeed_middle/2);
camera_update();
} else if(get_object_center(0,0).x < 70) {
motor(Motor_Left,-Drivespeed_middle/2);
motor(Motor_Right,Drivespeed_middle/2);
camera_update();
} else {
motor(Motor_Left,Drivespeed_middle/2);
motor(Motor_Right,Drivespeed_middle/2);
camera_update();
}
msleep(5);
}
freeze(Motor_Left);
freeze(Motor_Right);
claw_close();
msleep(1000);
claw_up();
}
void main()
{
calibrate();
set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_RIGHT);
camera_open(LOW_RES);
enable_servos();
set_analog_pullup(3,0);
while(!b_button()){
claw_at_linefollowing();
do {
camera_update();
linefolowing();
} while(get_object_count(green) ==0); //linefolowing bis er was sieht
if(get_object_bbox(0,0).width>BLOB_SIZE_TAKE&&get_object_bbox(0,0).height>BLOB_SIZE_TAKE){
takepom();
}
printf("kompletdurch \n");
}
disable_servos();
stop();
}
void takepom(){
printf("takepom \n");
set_servo_position(CLAW1, CLAW1_FRONT);
set_servo_position(CLAW2, CLAW2_FRONT);
set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_FRONT);
stop();
msleep(1000);
turn_right();
msleep(500);
int findit=1;
int left_or_right=1; //ob nach linksoder rechts drehen l=0, r=1
int getit=0; //wenn er das pom hat
int foundNothingToLong=0;
double sec=seconds();
while(!getit){
if((sec+10)<seconds()){ //wenn er 10 sec nichts findet
if(foundNothingToLong=2){
forward();
while((analog10(ET)<310&&( analog10(LEFT_SENSOR)<left_blk-150||analog10(RIGHT_SENSOR)<right_blk-150))){
msleep(5);
}
if(analog10(ET)<310){
turn_left();
msleep(600);
forward();
while( analog10(LEFT_SENSOR)<left_blk-150||analog10(RIGHT_SENSOR)<right_blk-150){}
}
}
else{
foundNothingToLong++;
back();
msleep(400); //etwas rückwerts
sec=seconds();
if(!get_object_count(green)>0){ //wenn ers nicht hat
forward();
msleep(400);
findit=0; //nicht gefunden
turn_left();
while(analog10(LEFT_SENSOR)<left_blk-150){}
turn_left();
while(analog10(MIDDLE_SENSOR)<mid_blk-150){}
break;
}
}
}
if(left_or_right){
turn_right_speed(slow_speed);
while(analog10(RIGHT_SENSOR)<right_blk-150&&!get_object_count(green)>0){ //solange Kammera und Sensor nichts sehen rechts drehen
camera_update();
}
stop(); //wenn etwas gefunden stehen bleiben
msleep(200);
if(analog10(RIGHT_SENSOR)>right_blk-150){ //schauen ob Kamera oder Sensor
left_or_right=0; //bei sensor drehrichtung ändern
}
else{
camera_update();
if(get_object_bbox(0,0).width>5&&get_object_bbox(0,0).height>5&&analog10(RIGHT_SENSOR)<right_blk-150){ //wenn Kammera : Wa blob ein zu kleiner Blob(was falsches gesehen), wenn blob groß stehen bleiben und feinjustireung(foundit) machen
getit=foundit();
}
}
}
else{
turn_left_speed(slow_speed);
while(analog10(LEFT_SENSOR)<left_blk-150&&!get_object_count(green) >0){
camera_update();
}
stop();
msleep(200);
if(analog10(LEFT_SENSOR)>left_blk-150){
left_or_right=1;
}
else{
camera_update();
if(get_object_bbox(0,0).width>5&&get_object_bbox(0,0).height>5){
getit=foundit();
}
}
}
}
if(findit){
put_poms_out(); // poms nach drausen schieben
}
}
int main(int argc, char *argv[])
{
int blob_count = 0, i = 0;
int main_channel = 2; /* red - botguy */
double time_of_snapshot = 0.0;
double start = 0.0;
printf("args: %d\n", argc);
if (argc > 1)
{
if (argv[1][0] == '0') main_channel = 0;
else if (argv[1][0] == '1') main_channel = 1;
else if (argv[1][0] == '2') main_channel = 2;
else if (argv[1][0] == '3') main_channel = 3;
}
for (i = 0; i < argc; i++) {
printf("arg[%d] = %s\n", i, argv[i]);
}
printf("\n");
int area[NUM_TRACKING];
point2 center[NUM_TRACKING];
rectangle blob[NUM_TRACKING];
//int area1 = 0, areaX = 0;
//point2 center;
init_grid();
//wait_for_light(0);
camera_open();
printf("Num channels: %d", get_channel_count());
camera_update();
time_of_snapshot = seconds();
msleep(300);
start = seconds();
do {
// Initialize
memset(center, 0, sizeof(center));
memset(area, 0, sizeof(area));
clear_grid();
// Begin
// Do not do anything with the camera until enough time has passed
if (seconds() - time_of_snapshot >= 0.1) {
camera_update();
time_of_snapshot = seconds();
blob_count = get_object_count(main_channel);
//sprintf(msg, "#:%2d", blob_count); diag();
printf("#:%-2d ", blob_count);
if (blob_count > count_of(area)) {
blob_count = count_of(area);
}
if (blob_count > 0) {
for (i = 0; i < blob_count; ++i) {
area[i] = get_object_area(main_channel, i);
center[i] = get_object_center(main_channel, i);
blob[i] = get_object_bbox(main_channel, i);
plot(&blob[i], i + '0');
}
for (i = 0; i < blob_count; ++i) {
printf(" %3d", blob[i].width * blob[i].height);
}
for (; i < count_of(blob); ++i) {
printf(" ");
}
printf(" ");
for (i = 0; i < blob_count; ++i) {
printf(" %2dx%2d", blob[i].width, blob[i].height);
}
//show_xy(center[0].x, center[0].y);
}
//printf(" |%3d %4d <-> %4d %3d| %s\n", delta_left, left, right, delta_right, message);
printf("%s\n", message);
message[0] = 0;
diag();
show_grid();
}
//printf("The time is: %f %f\n", seconds(), seconds() - start);
} while (seconds() - start <= 4.0);
camera_close();
return 0;
}
