From 767f23997ab2d5403ce56448b0b3f4e176e35ef8 Mon Sep 17 00:00:00 2001
From: zhaoliang <zhz03@g.ucla.edu>
Date: Mon, 25 Oct 2021 16:38:11 -0700
Subject: [PATCH] test motors
---
Code/Control/test_motor/test_motor.ino | 91 ++++++++++++++++++++++++++
1 file changed, 91 insertions(+)
create mode 100644 Code/Control/test_motor/test_motor.ino
diff --git a/Code/Control/test_motor/test_motor.ino b/Code/Control/test_motor/test_motor.ino
new file mode 100644
index 0000000..62e70e1
--- /dev/null
+++ b/Code/Control/test_motor/test_motor.ino
@@ -0,0 +1,91 @@
+#include <esp_now.h>
+#include <WiFi.h>
+#include <Wire.h>
+#include <SparkFun_VL53L1X.h>
+#include <Arduino.h>
+#include <Adafruit_MotorShield.h>
+
+int Rec_pwm1;
+int Rec_pwm2;
+int Rec_pwm3;
+int Rec_pwm4;
+String Rec_dir1;
+String Rec_dir2;
+String Rec_dir3;
+String Rec_dir4;
+
+// ========================== Motor part ====================================
+// Create the motor shield object with the default I2C address
+Adafruit_MotorShield AFMS = Adafruit_MotorShield();
+// changed
+Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1); // pwm1
+Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2); // pwm2
+Adafruit_DCMotor *motorLeft = AFMS.getMotor(3); // pwm3
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4); // pwm4
+
+void setup() {
+ // put your setup code here, to run once:
+ // Init Serial Monitor
+ Serial.begin(115200);
+ // Wire.begin();
+AFMS.begin(); // create with the default frequency 1.6KHz
+
+}
+void motor_control(int pwm1,int pwm2,int pwm3,int pwm4){
+ motorVertical_L->setSpeed(abs(pwm1));
+ motorVertical_R->setSpeed(abs(pwm2));
+ motorLeft->setSpeed(abs(pwm3));
+ motorRight->setSpeed(abs(pwm4));
+}
+
+void loop() {
+ // put your main code here, to run repeatedly:
+ int t_speed = 50;
+
+
+
+ motorVertical_L->setSpeed(t_speed);
+ motorVertical_R->setSpeed(t_speed);
+ motorLeft->setSpeed(t_speed);
+ motorRight->setSpeed(t_speed);
+ motorVertical_L->run(BACKWARD); //
+ motorVertical_R->run(BACKWARD); //
+ motorLeft->run(BACKWARD); //
+ motorRight->run(BACKWARD); //
+ delay(1000);
+ /*
+ delay(2000);
+ motor_control(t_speed,0,0,0);
+ motorVertical_L->run(BACKWARD); //
+ Serial.println("V_L: backward");
+ delay(2000);
+ motorVertical_L->run(FORWARD);
+ Serial.println("V_L: forward");
+
+ delay(2000);
+ motor_control(0,t_speed,0,0);
+ motorVertical_R->run(BACKWARD); //
+ Serial.println("V_R: backward");
+ delay(2000);
+ motorVertical_R->run(FORWARD); //
+ Serial.println("V_R: forward");
+
+ delay(2000);
+ motor_control(0,0,t_speed,0);
+ motorLeft->run(BACKWARD); //
+ Serial.println("H_L: backward");
+ delay(2000);
+ motorLeft->run(FORWARD); //
+ Serial.println("H_L: forward");
+
+ delay(2000);
+ motor_control(0,0,0,t_speed);
+ motorRight->run(BACKWARD); //
+ Serial.println("H_R: backward");
+ delay(2000);
+ motorRight->run(FORWARD); //
+ Serial.println("H_R: forward");
+ */
+
+
+}
--
GitLab