Added motor test code

Code/LIDAR_Code/Lidar_demo/Lidar_demo.ino

-
 #include <Wire.h>
 #include <SparkFun_VL53L1X.h> // https://github.com/sparkfun/SparkFun_VL53L1X_Arduino_Library/
 
@@ -9,7 +8,7 @@
 #include <WiFi.h>
 #include <WiFiClient.h>
 
-char auth[] = "cLUKk5penym7JGnFxo61dvNzG5p4y4Za";
+char auth[] = "4Mp_V5mFllQKyaSjzqwILkMZZTRSuPD-";
 char ssid[] = "lemur";
 char pass[] = "lemur9473";
 
@@ -31,7 +30,7 @@ int new_speed = 0 ;
 int flag = 0;
 
 void setup() {
-  Serial.begin(9600);
+//  Serial.begin(9600);
   Blynk.begin(auth, ssid, pass);
   AFMS.begin();
 
@@ -63,7 +62,7 @@ void turn_left(){
 }
 
 void turn_right(){
-  myMotor3->run(BACKWARD );
+  myMotor3->run(BACKWARD);
   myMotor4->run(RELEASE);
 }
 
@@ -98,14 +97,16 @@ void SpeedControl(int Speed){
 void loop() {
   dist = distanceSensor.getDistance();
   Serial.println(distanceSensor.getDistance());
-  if (flag == 0){
-    stopfb();
-    stopud();
-  }else if (flag == 1){
+//  if (flag == 0){
+//    stopfb();
+//    stopud();
+//  }
+//  else if (flag == 1){
     if (dist < detect_thres){
       SpeedControl(mt_speed);
       forward();
-    }else if (dist > detect_thres){
+    }
+    else if (dist > detect_thres){
       /*
       Kp = (dist - detect_thres)/ 4000;
       Serial.println("kp: ");
@@ -117,7 +118,7 @@ void loop() {
       SpeedControl(150);
       forward();
     }
-  }
+//  }
   Blynk.run();
 }
 

Code/Motor-Test-Aaron/Motor-Test-Aaron.ino

+// Motor Test Code - Aaron
+
+#include <Wire.h>
+#include <Adafruit_MotorShield.h>
+
+int largeVal = 255;
+int smallVal = 127;
+
+Adafruit_MotorShield AFMS = Adafruit_MotorShield();
+Adafruit_DCMotor *motorVL = AFMS.getMotor(1);
+Adafruit_DCMotor *motorVR = AFMS.getMotor(2);
+Adafruit_DCMotor *motorHL = AFMS.getMotor(3);
+Adafruit_DCMotor *motorHR = AFMS.getMotor(4);
+void setup() {
+  AFMS.begin();
+}
+
+void loop() {
+  motorHR->setSpeed(0);
+  motorVL->setSpeed(largeVal);
+  motorVL->run(FORWARD);
+  delay(5000);
+  motorVL->setSpeed(smallVal);
+  motorVL->run(BACKWARD);
+  delay(5000);
+  motorVL->setSpeed(0);
+  motorVR->setSpeed(largeVal);
+  motorVR->run(FORWARD);
+  delay(5000);
+  motorVR->setSpeed(smallVal);
+  motorVR->run(BACKWARD);
+  delay(5000);
+  motorVR->setSpeed(0);
+  motorHL->setSpeed(largeVal);
+  motorHL->run(FORWARD);
+  delay(5000);
+  motorHL->setSpeed(smallVal);
+  motorHL->run(BACKWARD);
+  delay(5000);
+  motorHL->setSpeed(0);
+  motorHR->setSpeed(largeVal);
+  motorHR->run(FORWARD);
+  delay(5000);
+  motorHR->setSpeed(smallVal);
+  motorHR->run(BACKWARD);
+  delay(5000);
+}