int turn_right(){
return turn_right_speed(DEFAULT_SPEED);
}
int foundit(){
printf("foundit \n");
int x;
camera_update();
if(get_object_count(green)>0){
while(get_object_center(green,0).y < 100){ //solange bis blob nahe genug
camera_update();
if(get_object_count(green)>0){ //schaut ob Blob noch in der Kamera
camera_update();
x = get_object_center(green,0).x;
if (x > 120) { //wenn Blob rechts--> rechts drehen ....
turn_right_speed(50); //je näher er dem pom kommt umso langsamer dreht er sich
} else if (x < 40) {
turn_left_speed(50); //je näher er dem pom kommt umso langsamer dreht er sich
} else if ((x > 40)||(x < 120)) {
forward();
if(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){
foundOutArea=1;
printf("drausen\n");
}
}
}
else{ //wenn blob nicht mehr in Kamera wieder in grobjustierung(takepom) gehen
return 0;
}
}
printf("target aquired! \n");
set_servo_position(CLAW1, CLAW1_FRONT);
set_servo_position(CLAW2, CLAW2_FRONT);
forward(); //etwas vor fahren um Blob sicher zu erwischen
int time=200;
while((analog10(ET)<310&&!digital(15)&&!digital(14))&&time>=0 ){
msleep(5);
time--;
}
printf("%i",digital(15));
printf("\nLast time ");
printf("%i",time);
stop();
msleep(100);
if(time>=0){ //fals er angefahren ist
back();
msleep(150);
motor(RIGHT_MOTOR,TURN_SPEED);
motor(LEFT_MOTOR,0);
msleep(1500);
claw_close();
}
claw_close();
if(foundOutArea){ //wenn ers auserhalb des Schwarzen bereichs gefunden hat
printf("zurück fahren\n");
back();
while(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){}
msleep(700);
}
back();
msleep(800);
if(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){ //wenn er so weit zurück sein sollte das er auf schwarz is
forward();
while(analog10(LEFT_SENSOR)<left_blk-150&&analog10(RIGHT_SENSOR)<right_blk-150){} //wieder forwärts bis er weis is damits funktioniert
}
camera_update();
return 1;
}
return 0;
}
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
}
}
void loop()
{
if (line_tracing == true)
{
unsigned char sensor_data = 0;
int z;
for(z=0;z<8;z++)
{
unsigned int val = digitalRead(SensorD[z]);
sensor_data |= (val << z);
}
sensor_data = ~sensor_data;
Serial.print(sensor_data, HEX);
Serial.write(" ");
switch (sensor_data)
{
//our move forward situations.
case 0x18:
case 0x10:
case 0x08:
case 0x38:
case 0x1c:
case 0x3c:
move_forward_speed(100, 100);
break;
//our turn right situations.
case 0x0c:
case 0x04:
case 0x06:
case 0x0e:
case 0x1e:
case 0x0f:
turn_right_speed(150, 10);
break;
//our turn left situations.
case 0x30:
case 0x20:
case 0x60:
case 0x70:
case 0x78:
case 0xf0:
turn_left_speed(10, 150);
break;
//our spin right situations.
case 0x07:
case 0x03:
case 0x02:
case 0x01:
turn_pivot_right_speed(-100, 100);
break;
//our spin left situations.
case 0xc0:
case 0x40:
case 0x80:
case 0xe0:
turn_pivot_left_speed(100, -100);
break;
case 0x00:
case 0xff:
move_stop();
break;
default:
move_stop();
break;
}
delay(5);
}
}
