求教小车运动过程中发现物体并夹取的问题

maomao

#include <DCMotor.h>
DCMotor motor0(M0_EN, M0_D0, M0_D1);
DCMotor motor1(M1_EN, M1_D0, M1_D1);
#include <Servo.h>  
Servo myservo0;  // create servo object to control a servo
                // a maximum of eight servo objects can be created
Servo myservo1;  // create servo object to control a servo
                // a maximum of eight servo objects can be created  
int pos = 0;    // variable to store the servo position
void setup()
{
}
void gostraight()      // 直线。
{
motor0.setSpeed(100);
motor1.setSpeed(-100);  
}                          //直线   
void right()//右边出现危险品，捕捉
{   
motor0.setSpeed(0);
motor1.setSpeed(0);  
myservo0.attach(0);  // attaches the servo0 on pin 0 to the servo object
myservo1.attach(1);  // attaches the servo1 on pin 1 to the servo object
for(pos = 90; pos >0; pos -= 1)  // goes from 0 degrees to 180 degrees 发现右边危险品主舵机转到180度位副舵机转到0度位
{                                  // in steps of 1 degree
myservo0.write(pos);              // tell servo0 to go to position in variable 'pos'   
delay(5);                       // waits 15ms for the servo to reach the position
  }
for(pos = 90; pos <180; pos += 1)  // goes from 0 degrees to 180 degrees 副舵机张开
{                                  // in steps of 1 degree
myservo1.write(pos);              // tell servo0 to go to position in variable 'pos'   
delay(5);                       // waits 15ms for the servo to reach the position
}
delay(1000);                       //    wait 1 s  等待1s
for(pos = 180; pos >105; pos -= 1)  // goes from 0 degrees to 180 degrees 副舵机夹紧并保持
{                                  // in steps of 1 degree
  myservo1.write(pos);              // tell servo0 to go to position in variable 'pos'  
delay(5);                       // waits 15ms for the servo to reach the position   
   }
  for(pos = 0; pos <100; pos += 1)  // goes from 0 degrees to 180 degrees 主舵机归位100度
{                                  // in steps of 1 degree
  myservo0.write(pos);              // tell servo0 to go to position in variable 'pos'   
delay(5);                       // waits 15ms for the servo to reach the position
   }                           
for(pos =105; pos <150; pos += 1)  // goes from 0 degrees to 180 degrees 副舵机松开至150度
{                                  // in steps of 1 degree
myservo1.write(pos);              // tell servo0 to go to position in variable 'pos'   
  delay(5);                       // waits 15ms for the servo to reach the position
         }   
}
void loop()
{
   gostraight() ;                         // 直线。   
  if(300<analogRead(A1))                          
    {
     right();   
     }
}

小车可以直行 也可以夹取。但如何将二者结合起来？？？