From 73c8fa3b5898f518185f7fa7f396d9fd3e5d047a Mon Sep 17 00:00:00 2001
From: Eli Ribble <eli@theribbles.org>
Date: Tue, 15 Apr 2025 08:22:46 -0700
Subject: [PATCH] Initial box runner

---
 main.py | 127 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 127 insertions(+)
 create mode 100644 main.py

diff --git a/main.py b/main.py
new file mode 100644
index 0000000..7db46e4
--- /dev/null
+++ b/main.py
@@ -0,0 +1,127 @@
+import time
+
+# Library imports
+from vex import *
+
+class Pierre:
+    def __init__(self) -> None:
+        self.brain = Brain()
+        self.brain_inertial = Inertial()
+        self.left_drive_smart = Motor(
+            Ports.PORT1,
+            GearSetting.RATIO_2_1,
+            False)
+        self.right_drive_smart = Motor(
+            Ports.PORT6,
+            GearSetting.RATIO_2_1,
+            True)
+        self.drivetrain = SmartDrive(
+            lm=self.left_drive_smart, 
+            rm=self.right_drive_smart, 
+            g=self.brain_inertial, 
+            wheelTravel=200,
+            trackWidth=292,
+            wheelBase=127,
+            units=DistanceUnits.MM,
+            externalGearRatio=2.0,
+        )
+        # touchled_2 = Touchled(Ports.PORT2)
+        # optical_3 = Optical(Ports.PORT3)
+        self.color_sensor = ColorSensor(Ports.PORT3)
+        # distance_7 = Distance(Ports.PORT7)
+        # bumper_8 = Bumper(Ports.PORT8)
+        self.velocity = 50
+        self.brain.buttonLeft.pressed(self.velocity_change, (-10,))
+        self.brain.buttonRight.pressed(self.velocity_change, (10,))
+        self.brain.buttonCheck.pressed(self.forward_routine)
+
+    def action_loop(self):
+        while True:
+            #brain.screen.set_cursor(1, 1)
+            #brain.screen.print("Hue " + str(color_3.hue()))
+            #brain.screen.set_cursor(2, 1)
+            #brain.screen.print("Brt "  + str(color_3.brightness()))
+            #brain.screen.set_cursor(3, 1)
+            #brain.screen.print("Obj " + str(color_3.is_near_object()))
+            time.sleep(.05)
+                    
+    def calibrate_drivetrain(self):
+        # Calibrate the Drivetrain Inertial
+        sleep(200, MSEC)
+        self.brain.screen.print("Calibrating")
+        self.brain.screen.next_row()
+        self.brain.screen.print("Inertial")
+        self.brain_inertial.calibrate()
+        while self.brain_inertial.is_calibrating():
+            sleep(25, MSEC)
+        self.brain.screen.clear_screen()
+        self.brain.screen.set_cursor(1, 1)
+        self.brain.screen.print("Pierre is ready.")
+
+    def forward_routine(self) -> None:
+        self.brain.screen.set_cursor(1, 1)
+        self.brain.screen.print("driving routine")
+        self.play_confirm_sound()
+        
+        self.drivetrain.set_drive_velocity(self.velocity, VelocityUnits.PERCENT)
+        self.drivetrain.set_turn_velocity(self.velocity, VelocityUnits.PERCENT)
+        for _ in range(4):
+            self.brain.screen.clear_line(2)
+            self.brain.screen.set_cursor(2, 1)
+            self.brain.screen.print("straight")
+            self.drivetrain.drive_for(DirectionType.FORWARD, 20.0, DistanceUnits.IN, wait=True)
+
+            self.play_pause_sound()
+            self.brain.screen.clear_line(3)
+            self.brain.screen.set_cursor(3, 1)
+            self.brain.screen.print("turn")
+            self.drivetrain.turn_for(
+                direction=TurnType.LEFT,
+                angle=90,
+                wait=True)
+
+        self.brain.screen.set_cursor(2, 1)
+        self.brain.screen.print("Complete.")
+        self.play_complete_sound()
+
+    def play_confirm_sound(self) -> None:
+        self.brain.play_note(1, 1, 100)
+        self.brain.play_note(1, 2, 100)
+        self.brain.play_note(1, 3, 100)
+        self.brain.play_note(1, 6, 100)
+        self.brain.play_note(1, 4, 100)
+
+    def play_complete_sound(self) -> None:
+        self.brain.play_note(1, 5, 100)
+        time.sleep(0.2)
+        self.brain.play_note(1, 5, 400)
+
+    def play_pause_sound(self) -> None:
+        self.brain.play_note(1, 7, 100)
+        time.sleep(0.2)
+        self.brain.play_note(1, 6, 200)
+        time.sleep(0.2)
+        self.brain.play_note(1, 5, 300)
+        
+    def velocity_change(self, delta: int) -> None:
+        self.velocity -= delta
+        if self.velocity < 10:
+            self.velocity = 10
+        elif self.velocity > 100:
+            self.velocity = 100
+        self.brain.screen.set_cursor(1, 1)
+        self.brain.screen.print("Velocity: " + str(self.velocity) + "%")
+        
+
+    
+def main() -> None:
+    # Brain should be defined by default
+    pierre = Pierre()
+    pierre.calibrate_drivetrain()
+    pierre.action_loop()
+
+#drivetrain.drive_for(FORWARD, 10)
+#drivetrain.drive_for(REVERSE, 10)
+#color_3.set_light(LedStateType.ON)
+if __name__ == "__main__":
+    main()
\ No newline at end of file