Merge branch 'main' of https://github.com/AlanWang611/SET2023

AlanWang611 · Apr 17, 2024 · b91fae4 · b91fae4
2 parents 5888c0e + 8d765bc
commit b91fae4
Showing 1 changed file with 68 additions and 10 deletions.
diff --git a/electrical_code/lidar/lidar_data/lidar_data.ino b/electrical_code/lidar/lidar_data/lidar_data.ino
@@ -1,13 +1,25 @@
 #include <SoftwareSerial.h>
+#include <Servo.h>
+
+Servo myservo;
+int servo1_pin = 12;
+int servo2_pin = 13;
+// int motor_pin1 = 6;
+// int motor_pin2 = 7;
+int motor_pin3 = 8;
+int motor_pin4 = 9;
+int pin_input = 2;
 
 const int stepPin = 4;
-const int enPin = 2;
+const int enPin = 5;
 const int dirPin = 3;
 int totalSteps = 0;
 float currentAngle = 0;
 const int STEPS = 1600; // resolution || 1600 steps per rev (on motor driver)
 const float angle_per_step = 360.0 / float(STEPS);
 
+int servoAngle;
+
 const int HEADER = 0x59;
 // Variables
 int TF01_pix;
@@ -25,25 +37,49 @@ void setup() {
   digitalWrite(enPin, LOW);
   digitalWrite(dirPin, LOW); // default counter clockwise
 
+  myservo.attach(servo1_pin);
+  // myservo.attach(servo2_pin);
+  // pinMode(motor_pin1, OUTPUT);
+  // pinMode(motor_pin2, OUTPUT);
+  pinMode(motor_pin3, OUTPUT);
+  pinMode(motor_pin4, OUTPUT);
+  pinMode(pin_input, INPUT);
+  attachInterrupt(digitalPinToInterrupt(pin_input), moveArm, CHANGE);
+
   Serial.begin(115200);
   Serial.print("serial started");
   mySerial.begin(115200);
   setAngle(0);
 }
 
 void loop() {
-  // rotate a step
-  rotate(1);
+  // myservo.write(servoAngle);
+  // digitalWrite(motor_pin1, LOW);
+  // digitalWrite(motor_pin2, HIGH);
+  digitalWrite(motor_pin3, HIGH);
+  digitalWrite(motor_pin4, LOW);
+  delay(5000);
+  // digitalWrite(motor_pin1, LOW);
+  // digitalWrite(motor_pin2, LOW);
+  digitalWrite(motor_pin3, LOW);
+  digitalWrite(motor_pin4, HIGH);
+  delay(5000);
 
-  // get distance 
-  Serial.println("dist : " + String(getDist()));
+  digitalWrite(motor_pin3, LOW);
+  digitalWrite(motor_pin4, LOW);
+  delay(5000);
+  // // rotate a step
+  // rotate(1);
 
-  // serial print angle
-  if (currentAngle >= 360.0) {
-    currentAngle = currentAngle - 360.0;
-  }
+  // // get distance 
+  // Serial.println("dist : " + String(getDist()));
+
+  // // serial print angle
+  // if (currentAngle >= 360.0) {
+  //   currentAngle = currentAngle - 360.0;
+  // }
 
-  Serial.println("angle : " + String(currentAngle));
+  // Serial.println("angle : " + String(currentAngle));
 }
 
 int getDist(){
@@ -115,4 +151,26 @@ void rotate(float moveSteps) {
   totalSteps += int(moveSteps);
   totalSteps %= STEPS; // capping @ # steps per rev
   currentAngle += moveSteps * angle_per_step;
+}
+
+void moveArm() {
+    // Run motor driver for 2 seconds
+    // Serial.println("Running motor driver");
+    // digitalWrite(motor_pin1, HIGH);
+    // digitalWrite(motor_pin2, LOW);
+    // digitalWrite(motor_pin3, HIGH);
+    // digitalWrite(motor_pin4, LOW);
+    // delay(3000);
+
+
+    // // Use servo for 120 degrees
+    // Serial.println("Servo angle increased");
+    // servoAngle = servoAngle + 2;
+    // servoAngle = servoAngle % 180;
+
+    // digitalWrite(motor_pin1, LOW);
+    // digitalWrite(motor_pin2, LOW);
+    // digitalWrite(motor_pin3, LOW);
+    // digitalWrite(motor_pin4, LOW);
+
 }