From 1f85d7a671d49bfb3296d8bcc6ef0d78c0a1b959 Mon Sep 17 00:00:00 2001
From: tmei005 <tmei@ufl.edu>
Date: Tue, 23 Apr 2024 20:40:42 -0400
Subject: [PATCH] wheel commands

---
 electrical_code/arduino_wheels.py | 40 +++++++++++++++++++++++++++++++
 electrical_code/serialSend.py     |  1 +
 electrical_code/wheels.py         |  1 +
 electrical_code/wheels_test.py    | 21 +++++++++-------
 4 files changed, 54 insertions(+), 9 deletions(-)
 create mode 100644 electrical_code/arduino_wheels.py

diff --git a/electrical_code/arduino_wheels.py b/electrical_code/arduino_wheels.py
new file mode 100644
index 0000000..589abaf
--- /dev/null
+++ b/electrical_code/arduino_wheels.py
@@ -0,0 +1,40 @@
+import serial
+import time
+
+arduino = serial.Serial(port='COM5', baudrate=115200, timeout=.1)
+time_divisor = 10
+
+def write_serial(x):
+    arduino.write(bytes(x, 'utf-8'))
+    time.sleep(0.05)
+
+
+def turnRight(angle):
+    command = "0" + str(angle).zfill(3) + "0000"
+    write_serial(command)
+    time.sleep(angle * (time_divisor) / 1000)
+
+
+def turnLeft(angle):
+    command = "1" + str(angle).zfill(3) + "0000"
+    write_serial(command)
+    time.sleep(angle * (time_divisor) / 1000)
+
+
+def goForward(dist):
+    print('hello')
+    command = "00001" + str(dist).zfill(3)
+    print("forward: " + command)
+    write_serial(command)
+    time.sleep(dist * (time_divisor) / 1000)
+
+
+def goBackward(dist):
+    command = "00000" + str(dist).zfill(3)
+    print("backward: " + command)
+    write_serial(command)
+    time.sleep(dist * (time_divisor) / 1000)
+
+if __name__ == "__main__":
+    goForward(100)
+    time.sleep(2)
\ No newline at end of file
diff --git a/electrical_code/serialSend.py b/electrical_code/serialSend.py
index 71bb31d..2c16828 100644
--- a/electrical_code/serialSend.py
+++ b/electrical_code/serialSend.py
@@ -14,6 +14,7 @@ def move(angle, dist):
    angle = '0' * (3 - len(angle))  + angle
    dist = '0' * (3 - len(dist)) + dist
    command = angle + dist
+   print(command)
    write_read(command)
    #print(command)
 
diff --git a/electrical_code/wheels.py b/electrical_code/wheels.py
index da4d88b..8f5f0e6 100644
--- a/electrical_code/wheels.py
+++ b/electrical_code/wheels.py
@@ -5,6 +5,7 @@ class Wheels:
     time_divisor = 10
 
     def __init__(self, port_num):
+        print(port_num)
         self.arduino = serial.Serial(port=port_num, baudrate=115200, timeout=0.1)
         self.arduino.flush()
 
diff --git a/electrical_code/wheels_test.py b/electrical_code/wheels_test.py
index 46c212c..267e692 100644
--- a/electrical_code/wheels_test.py
+++ b/electrical_code/wheels_test.py
@@ -1,13 +1,16 @@
 from wheels import Wheels
+import serial
 
-# Instantiate the Wheels class with your port number
-myFirstWheel = Wheels("COM5")
+while True:
+    arduino = serial.Serial(port='COM5', baudrate=115200, timeout=.1)
 
+    # Instantiate the Wheels class with your port number
+    myFirstWheel = Wheels('COM5')
 
-# Test movement commands
-# myFirstWheel.move(1,000,110)
-myFirstWheel.goForward(100)  # Go forward 100 units
-myFirstWheel.goBackward(111)  # Go backward 50 units
-# myFirstWheel.turnRight("90")  # Turn right by 90 degrees
-# myFirstWheel.turnLeft("45")  # Turn left by 45 degrees
-
+    # Test movement commands
+    # myFirstWheel.move(1,000,110)
+    # myFirstWheel.goForward(100)  # Go forward 100 units
+    myFirstWheel.move("", 100, 431)
+    # myFirstWheel.goBackward(111)  # Go backward 50 units
+    # myFirstWheel.turnRight("90")  # Turn right by 90 degrees
+    # myFirstWheel.turnLeft("45")  # Turn left by 45 degrees