From 31ee07c943dad03f6972d25fe7a20d57afb6ddaf Mon Sep 17 00:00:00 2001
From: Aaron John Sabu <aaronjohnsabu1999@gmail.com>
Date: Fri, 8 Oct 2021 23:23:54 -0700
Subject: [PATCH] Added motor test code

---
 Code/LIDAR_Code/Lidar_demo/Lidar_demo.ino  | 21 +++++-----
 Code/Motor-Test-Aaron/Motor-Test-Aaron.ino | 47 ++++++++++++++++++++++
 2 files changed, 58 insertions(+), 10 deletions(-)
 create mode 100644 Code/Motor-Test-Aaron/Motor-Test-Aaron.ino

diff --git a/Code/LIDAR_Code/Lidar_demo/Lidar_demo.ino b/Code/LIDAR_Code/Lidar_demo/Lidar_demo.ino
index 43f00e1..9a68310 100644
--- a/Code/LIDAR_Code/Lidar_demo/Lidar_demo.ino
+++ b/Code/LIDAR_Code/Lidar_demo/Lidar_demo.ino
@@ -1,4 +1,3 @@
-
 #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();
 }
 
diff --git a/Code/Motor-Test-Aaron/Motor-Test-Aaron.ino b/Code/Motor-Test-Aaron/Motor-Test-Aaron.ino
new file mode 100644
index 0000000..9208590
--- /dev/null
+++ b/Code/Motor-Test-Aaron/Motor-Test-Aaron.ino
@@ -0,0 +1,47 @@
+// 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);
+}
-- 
GitLab