#include

 <Wire.h>  
#include  <Adafruit_MotorShield.h>  
#include  "utility/Adafruit_MS_PWMServoDriver.h"  
#include  <Servo.h>  
#include  "Adafruit_VL53L0X.h"  
   
Adafruit_VL53L0X  lox  =  Adafruit_VL53L0X();  
 
Servo  servo1;                          //  create  a  servo  object  
   
 
//  Create  the  motor  shield  object  with  the  default  I2C  address  
Adafruit_MotorShield  AFMS  =  Adafruit_MotorShield();  
   
//  Select  which  'port'  M1,  M2,  M3,  M4.  
Adafruit_DCMotor  *myMotor1  =  AFMS.getMotor(1);  
Adafruit_DCMotor  *myMotor2  =  AFMS.getMotor(2);  
Adafruit_DCMotor  *myMotor3  =  AFMS.getMotor(3);  
Adafruit_DCMotor  *myMotor4  =  AFMS.getMotor(4);  
   
   
void  setup()  {  
Serial.begin(115200);              //  set  up  serial  library  at  9600  bps  
   Wire.begin();  
   
 //  wait  until  serial  port  opens  for  native  USB  devices  
   while  (!  Serial)  {  
       delay(1);  
   }  
   
   Serial.println("Adafruit  VL53L0X  test");  
       if  (!lox.begin())  {  
       Serial.println(F("Failed  to  boot  VL53L0X"));  
       while(1);  
   
   }  
   //  power  
   Serial.println(F("VL53L0X  API  Simple  Ranging  example\n\n"));  
   
   AFMS.begin();    //  create  with  the  default  frequency  1.6KHz  
   AFMS.begin(1000);    //  OR  with  a  different  frequency,  say  1KHz  
   
   //  Set  the  speed  to  start  to  200,  255  is  max  speed  
    myMotor1-­‐>setSpeed(200);  
   myMotor2-­‐>setSpeed(200);  
   myMotor3-­‐>setSpeed(200);  
   myMotor4-­‐>setSpeed(200);  
   
   servo1.attach(10);    //attach  servo  to  pin  10    
}  
   
void  loop()  {  
   uint8_t  i;  
   
   VL53L0X_RangingMeasurementData_t  measure;  
       
   Serial.print("Reading  a  measurement...  ");  
   lox.rangingTest(&measure,  false);  //  pass  in  'true'  to  get  debug  data  printout!  
   
   if  (measure.RangeStatus  !=  4)  {    //  phase  failures  have  incorrect  data  
       Serial.print("Distance  (mm):  ");  Serial.println(measure.RangeMilliMeter);  
   }  else  {  
       Serial.println("  out  of  range  ");  
   }  
   
myMotor1-­‐>run(FORWARD);  //run  forward  
myMotor2-­‐>run(FORWARD);  
myMotor3-­‐>run(FORWARD);    
myMotor4-­‐>run(FORWARD);  
   
   int  angle  =  analogRead(0);  
   angle=  map(angle,0,1023,0,180);    //map  the  values  from  0  to  180  degrees  
   
   servo1.write(20);          //  rotate  servo  to  20°  
   delay(300);  
   servo1.write(0);              //  rotate  servo  to  0°  
   delay(300);  
   servo1.write(30);          //  rotate  servo  to  30°  
   delay(300);  
   servo1.write(20);          //  rotate  servo  to  20°  
   delay(100);  
   
   
   if  (measure.RangeMilliMeter  <  700)  {              //  if  the  distance  sensed  is  less  than  
700mm  
       myMotor1-­‐>run(RELEASE);              //  stop  motors  
        myMotor2-­‐>run(RELEASE);  
       myMotor3-­‐>run(RELEASE);  
       myMotor4-­‐>run(RELEASE);  
       delay  (2000);                //  wait  for  2  seconds  
   
       myMotor1-­‐>setSpeed(110);              //  change  speed  to  110    
       myMotor2-­‐>setSpeed(110);  
       myMotor3-­‐>setSpeed(110);  
       myMotor4-­‐>setSpeed(110);  
       delay(1000);  
   
       myMotor1-­‐>run(FORWARD);            //  two  motors  rotate  forward  
       myMotor2-­‐>run(BACKWARD);      //  two  motors  rotate  backward  
       myMotor3-­‐>run(BACKWARD);    
       myMotor4-­‐>run(FORWARD);  
       delay(2000);                                                                    //  the  rover  has  now  turned  90°  
   
       myMotor1-­‐>setSpeed(200);  
       myMotor2-­‐>setSpeed(200);  
       myMotor3-­‐>setSpeed(200);  
       myMotor4-­‐>setSpeed(200);  
       delay(500);  
   
       myMotor1-­‐>run(FORWARD);          //  run  forward  
       myMotor2-­‐>run(FORWARD);  
       myMotor3-­‐>run(FORWARD);    
       myMotor4-­‐>run(FORWARD);  
       }  
       else  {                          //  if  the  distance  sensed  is  larger  than  700  mm,  the  motors  keep  
running  
           myMotor1-­‐>run(FORWARD);  
           myMotor2-­‐>run(FORWARD);  
           myMotor3-­‐>run(FORWARD);    
           myMotor4-­‐>run(FORWARD);  
   
   }  
}