From d3ef408ebe404884f4ba51929baf7b7ffb2f7c7b Mon Sep 17 00:00:00 2001
From: BOTAlex <zhentao2004@gmail.com>
Date: Sat, 18 Jan 2025 14:09:13 +0100
Subject: [PATCH] hmm, didn't know i had a git for this one

---
 ClawMachineOverhaul.ino | 70 +++++++++++++++++++++++++++--------------
 StepperController.h     |  4 +--
 StepperXController.h    |  4 +--
 3 files changed, 51 insertions(+), 27 deletions(-)

diff --git a/ClawMachineOverhaul.ino b/ClawMachineOverhaul.ino
index af14f55..4168eb6 100644
--- a/ClawMachineOverhaul.ino
+++ b/ClawMachineOverhaul.ino
@@ -2,8 +2,7 @@
 #include "StepperXController.h"
 #include "Time.h"
 
-#define LeftPin 3
-#define RightPin 2
+#define JoystickPin 63
 
 #define grabPin 19
 
@@ -14,46 +13,71 @@ void setup(){
     Serial.begin(115200);
     
     // Set the pin mode for the defined pins
-    pinMode(LeftPin, INPUT_PULLUP);
-    pinMode(RightPin, INPUT_PULLUP);
     pinMode(grabPin, INPUT_PULLUP);
 
     initSteppers();
     initXStepper();
 }
 
-float position = 0;
+bool isGrabbing = false;
+float startGrabbingTime = 0;
+const float moveToDropZoneTime = 1750; // How long to move right (relative to me)
+
+const int joyCriticalPoint = 10;
 
 void loop(){
-    // Updates deltatime
-    time.update();
+  // Updates deltatime
+  time.update();
 
-    stepperXLoop();
+  stepperXLoop();
 
-    bool left = digitalRead(LeftPin) == LOW;
-    bool right = digitalRead(RightPin) == LOW;
+  // Joystick. TODO: Make this only run when needed. Too lazy now.
+  int joystickValueRaw = analogRead(JoystickPin);
+  int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100);
+  bool left = joystickValue > 100-joyCriticalPoint;
+  bool right = joystickValue < joyCriticalPoint;
 
-    processJoystickInput(left, right);
+  // Serial.print("Left: ");
+  // Serial.print(left);
+  // Serial.print("    Right: ");
+  // Serial.println(right);
 
-    if (digitalRead(grabPin) == LOW){
-        beginGrabSequence();
-    }
+  if (isGrabbing && millis() > startGrabbingTime + moveToDropZoneTime) {
+    isGrabbing = false;
+    processJoystickInput(false, false); // Quickly stop motor
+    delay(1000);
+    moveStepper(StepperAxis::Head, 10000, 100); // open hand
+  }
 
-    // //Serial.print(millis());
+  processJoystickInput(left, right);
 
-    // Serial.print("Left state: ");
-    // Serial.print(left);
-    // Serial.print("  |  ");
-    // Serial.print("Right state: ");
-    // Serial.println(right);
+  if (digitalRead(grabPin) == LOW){
+      beginGrabSequence();
+      //initGrabber();
+  }
+
+  // //Serial.print(millis());
+
+  // Serial.print("Left state: ");
+  // Serial.print(left);
+  // Serial.print("  |  ");
+  // Serial.print("Right state: ");
+  // Serial.println(right);
 
 
-    delay(10);  // delay in milliseconds
+  delay(10);  // delay in milliseconds
+}
+
+void initGrabber(){
+  moveStepper(StepperAxis::Head, 100000, 1000);
 }
 
 void beginGrabSequence(){
-    moveStepper(StepperAxis::Head, 1000, 100);
+    moveStepper(StepperAxis::Head, 10000, 100); // open hand
     moveStepper(StepperAxis::Y, 10000, -1100);
-    moveStepper(StepperAxis::Head, 1000, -100, 100);
+    moveStepper(StepperAxis::Head, 1000, -100, 100); // close hand
     moveStepper(StepperAxis::Y, 10000, 1100);
+
+    isGrabbing = true;
+    startGrabbingTime = millis();
 }
diff --git a/StepperController.h b/StepperController.h
index 58f7ca2..487a6b0 100644
--- a/StepperController.h
+++ b/StepperController.h
@@ -70,7 +70,7 @@ void initSteppers(){
         steppers[i].setStepsPerRevolution(200);
     }
 
-    moveStepper(StepperAxis::Y, 10000, 1100);
+    moveStepper(StepperAxis::Y, 10000, 1000);
+    moveStepper(StepperAxis::Head, 10000, 100);
 
-    // moveStepper(StepperAxis::Head, 10000, 50);
 }
\ No newline at end of file
diff --git a/StepperXController.h b/StepperXController.h
index 9c1bf4a..7e0fc04 100644
--- a/StepperXController.h
+++ b/StepperXController.h
@@ -20,14 +20,14 @@ void processJoystickInput(bool left, bool right){
         //     isMovingLeft = true;
         //     stepper.stop();
         // }
-        stepper.spin(16000);
+        stepper.spin(14000);
     }
     else if (right){
         // if (isMovingLeft){
         //     isMovingLeft = false;
         //     stepper.stop();
         // }
-        stepper.spin(-16000);
+        stepper.spin(-14000);
     }
     else
         stepper.stop();