From 0c033fb5570da57d0dbabdb5aea459b173642e5c Mon Sep 17 00:00:00 2001 From: hwii Date: Thu, 11 Apr 2024 21:16:20 -0400 Subject: [PATCH 1/2] combine the lidar code with the interrupt pin for servo --- .../lidar/lidar_data/lidar_data.ino | 63 ++++++++++++++++--- 1 file changed, 53 insertions(+), 10 deletions(-) diff --git a/electrical_code/lidar/lidar_data/lidar_data.ino b/electrical_code/lidar/lidar_data/lidar_data.ino index 4ed9993..1f7deb7 100644 --- a/electrical_code/lidar/lidar_data/lidar_data.ino +++ b/electrical_code/lidar/lidar_data/lidar_data.ino @@ -1,13 +1,25 @@ #include +#include + +Servo myservo; +int servo1_pin = 12; +int servo2_pin = 13; +int motor_pin1 = 5; +int motor_pin2 = 6; +int motor_pin3 = 7; +int motor_pin4 = 8; +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,6 +37,15 @@ void setup() { digitalWrite(enPin, LOW); digitalWrite(dirPin, LOW); // default counter clockwise + myservo.attach(servo1_pin); + pinMode(servo2_pin, OUTPUT); + 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); @@ -32,18 +53,19 @@ void setup() { } void loop() { - // rotate a step - rotate(1); + myservo.write(servoAngle); + // // rotate a step + // rotate(1); - // get distance - Serial.println("dist : " + String(getDist())); + // // get distance + // Serial.println("dist : " + String(getDist())); - // serial print angle - if (currentAngle >= 360.0) { - currentAngle = currentAngle - 360.0; - } + // // 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 +137,25 @@ 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 + // 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); + } \ No newline at end of file From 8d765bced4b5ccaa716fff59808c74adee95fad2 Mon Sep 17 00:00:00 2001 From: hwii Date: Tue, 16 Apr 2024 20:25:21 -0400 Subject: [PATCH 2/2] Test the function to run the motor driver --- .../lidar/lidar_data/lidar_data.ino | 39 +++++++++++++------ 1 file changed, 27 insertions(+), 12 deletions(-) diff --git a/electrical_code/lidar/lidar_data/lidar_data.ino b/electrical_code/lidar/lidar_data/lidar_data.ino index 1f7deb7..20dc1a2 100644 --- a/electrical_code/lidar/lidar_data/lidar_data.ino +++ b/electrical_code/lidar/lidar_data/lidar_data.ino @@ -4,10 +4,10 @@ Servo myservo; int servo1_pin = 12; int servo2_pin = 13; -int motor_pin1 = 5; -int motor_pin2 = 6; -int motor_pin3 = 7; -int motor_pin4 = 8; +// int motor_pin1 = 6; +// int motor_pin2 = 7; +int motor_pin3 = 8; +int motor_pin4 = 9; int pin_input = 2; const int stepPin = 4; @@ -38,9 +38,9 @@ void setup() { digitalWrite(dirPin, LOW); // default counter clockwise myservo.attach(servo1_pin); - pinMode(servo2_pin, OUTPUT); - pinMode(motor_pin1, OUTPUT); - pinMode(motor_pin2, OUTPUT); + // myservo.attach(servo2_pin); + // pinMode(motor_pin1, OUTPUT); + // pinMode(motor_pin2, OUTPUT); pinMode(motor_pin3, OUTPUT); pinMode(motor_pin4, OUTPUT); pinMode(pin_input, INPUT); @@ -53,7 +53,21 @@ void setup() { } void loop() { - myservo.write(servoAngle); + // 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); + + digitalWrite(motor_pin3, LOW); + digitalWrite(motor_pin4, LOW); + delay(5000); // // rotate a step // rotate(1); @@ -141,6 +155,7 @@ void rotate(float moveSteps) { 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); @@ -148,10 +163,10 @@ void moveArm() { // delay(3000); - // Use servo for 120 degrees - Serial.println("Servo angle increased"); - servoAngle = servoAngle + 2; - servoAngle = servoAngle % 180; + // // Use servo for 120 degrees + // Serial.println("Servo angle increased"); + // servoAngle = servoAngle + 2; + // servoAngle = servoAngle % 180; // digitalWrite(motor_pin1, LOW); // digitalWrite(motor_pin2, LOW);