From e3f7d1ca396f33fbb4247c4741f3d195d3d61d5b Mon Sep 17 00:00:00 2001
From: Trison Li <trisonli0809@gmail.com>
Date: Wed, 4 Dec 2024 11:43:57 -0500
Subject: [PATCH] Update GARY.cpp

Test that I made for funsies, try this first and modify if needed.
---
 GARY.cpp | 149 ++++++++++++++++++++++++++++++++++++++++++-------------
 1 file changed, 114 insertions(+), 35 deletions(-)

diff --git a/GARY.cpp b/GARY.cpp
index 8664741..4a86fbe 100644
--- a/GARY.cpp
+++ b/GARY.cpp
@@ -8,7 +8,7 @@ const int backRightWheel    = 0;
 
 // Arm
 const int hingeServo        = 0; // high values is pi radians, low values pi 3 radians
-const int topServo          = 1; // 0 is down 45 degrees, 2000 is 60 degrees in the unit circle
+const int topServo          = 2; // 0 is down 45 degrees, 2000 is 60 degrees in the unit circle
 
 // Sensors
 const int frontLeftTape = 1;
@@ -22,6 +22,8 @@ const int backRightButton = 2;
 
 const int whiteTapeValue = 3500; // HIGHER IS DARKER
 
+void closeClaw();
+void openClaw();
 void calibration();
 void resetMotors();
 void measureDistances();
@@ -29,44 +31,121 @@ void yIncrease(int distance);
 void xIncrease(int distance);
 void yDecrease(int distance);
 void xDecrease(int distance);
-void rotateLeft();
-void rotateRight();
+void rotateLeft(int time);
+void rotateRight(int time);
 void calibrateTape();
+void startArm();
+void upArm();
 
 int main()
 {
-    yDecrease(10);
-    xIncrease(500);
-    xDecrease(5);
-    
-    xIncrease(5);
-    yIncrease(10);
-    xDecrease(15);
-    // Yeah top right board
+    startArm(); // set values future me btw
+    set_servo_position(hingeServo, 1730);
+    set_servo_position(topServo, 1300);
+
+   	msleep(500);
+    // yDecrease(10);
+    xIncrease(450);
+    set_servo_position(hingeServo, 1870);
+    msleep(1000);
+    set_servo_position(topServo, 200);
+    upArm();
     msleep(500);
-    xDecrease(150);
-    yDecrease(400);
-    xDecrease(60);
+    motor(frontLeftWheel, -15);
+    motor(backRightWheel, -15);
+    motor(frontRightWheel, -15);
+    motor(backLeftWheel, -15);
     msleep(500);
-    //  first stove
-    xIncrease(50);
-    yIncrease(400);
-    xIncrease(150);
-    // seasoning
-    yDecrease(140);
-    yIncrease(10);
+    set_servo_position(hingeServo, 1750);
+    msleep(20);
+    set_servo_position(hingeServo, 1700);
+    msleep(20);
+    set_servo_position(hingeServo, 1650);
+    msleep(20);
+    set_servo_position(hingeServo, 1600);
+    msleep(20);
+    set_servo_position(hingeServo, 1550);
+    msleep(1550);
+    set_servo_position(hingeServo, 1450);
+    ao();
+    msleep(300);
+    set_servo_position(hingeServo, 1300);
+    msleep(300);
     xDecrease(150);
-    yDecrease(580);
-    xDecrease(70);
+    rotateLeft(1650);
+    xIncrease(100);
+    msleep(300);
+    yDecrease(380);
+    msleep(50);
+    ao();
+    set_servo_position(hingeServo, 1400);
     msleep(500);
-    // stove
-    xIncrease(50);
-    yIncrease(600);
-    
+    set_servo_position(topServo, 1300);
+    msleep(500);
+    printf("cooking");
+    msleep(3000);
+    set_servo_position(topServo, 1800);
+    msleep(500);
+    set_servo_position(hingeServo, 1730)
+    msleep(300);
+    ao();
+    yIncrease(380);
+    msleep(300);
+    xDecrease(100);
+    rotateRight(1650);
+    yDecrease(100);
+    msleep(500);
+    xIncrease(30);
+    set_servo_position(hingeServo, 1300);
+    msleep(500);
+    set_servo_position(topServo, 200);
+    upArm();
+    msleep(500);
+    motor(frontLeftWheel, -15);
+    motor(backRightWheel, -15);
+    motor(frontRightWheel, -15);
+    motor(backLeftWheel, -15);
+    msleep(500);
+    set_servo_position(hingeServo, 1750);
+    msleep(20);
+    set_servo_position(hingeServo, 1700);
+    msleep(20);
+    set_servo_position(hingeServo, 1650);
+    msleep(20);
+    set_servo_position(hingeServo, 1600);
+    msleep(20);
+    set_servo_position(hingeServo, 1550);
+    msleep(1550);
+    set_servo_position(hingeServo, 1450);
+    ao();
+    msleep(300);
+    set_servo_position(hingeServo, 1300);
+    msleep(5000);
+    //  first stove
+
 }
 
 
+void closeClaw() {
+    set_servo_position(topServo, 350);
+    msleep(1500);
+}
 
+void openClaw() {
+    set_servo_position(topServo, 1300);
+    msleep(1500);
+}
+
+void startArm() {
+    enable_servos();
+    set_servo_position(hingeServo, 1800);
+    set_servo_position(topServo, 1300);
+}
+
+void upArm() {
+    set_servo_position(hingeServo, 1300);
+    
+}
 
 void calibration() {
     resetMotors();
@@ -137,21 +216,21 @@ void xDecrease(int distance) {
     msleep(300);
 }
 
-void rotateLeft() {
-    motor(frontLeftWheel, -100);
-    motor(backRightWheel, 100);
+void rotateLeft(int time) {
+    motor(frontLeftWheel, 0);
+    motor(backRightWheel, 0);
     motor(frontRightWheel, 100);
     motor(backLeftWheel, -100);
-    msleep(1000);
+    msleep(time);
     ao();
 }
 
-void rotateRight() {
-    motor(frontLeftWheel, 100);
-    motor(backRightWheel, -100);
+void rotateRight(int time) {
+    motor(frontLeftWheel, 0);
+    motor(backRightWheel, 0);
     motor(frontRightWheel, -100);
     motor(backLeftWheel, 100);
-    msleep(1000);
+    msleep(time);
     ao();
 }