prune back to blinky for new hardware

Author: Ben Chung

deploy/Manifest.toml

@@ -1,4 +1,6 @@
 [deps]
+PIOLib = "a21b0171-75fd-41b6-8640-f97fa3fa9a76"
+PIOLib_jll = "71578703-7aff-5c16-bbae-7ba9c160b2b9"
 Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 Timers = "21f18d07-b854-4dab-86f0-c15a3821819a"

deploy/Project.toml

@@ -27,13 +27,6 @@ const PWM_MAX_VALUE = 1024  # PWM duty cycle range (0-1024)
 # Global handles for GPIO pins and PWM channels
 mutable struct HardwareContext
     gpio::GPIO.GPIOController
-    ain1::GPIO.GPIOPin
-    bin1::GPIO.GPIOPin
-    stby::GPIO.GPIOPin
-    ltrans_oe::GPIO.GPIOPin
-    pwm_chip::PWM.PWMChip
-    pwm_left::PWM.PWMChannel
-    pwm_right::PWM.PWMChannel
 end
 
 """
@@ -44,13 +37,6 @@ Called on Ctrl-C or program exit.
 """
 function shutdown!(hw::HardwareContext)
     println(Core.stdout, "Shutting down hardware...")
-    # Stop PWM first
-    PWM.pwmWrite(hw.pwm_left, 0, PWM_MAX_VALUE)
-    PWM.pwmWrite(hw.pwm_right, 0, PWM_MAX_VALUE)
-    # Put motor controller into standby
-    GPIO.set_value(hw.stby, 0)
-    # Disable level translator
-    GPIO.set_value(hw.ltrans_oe, 0)
     println(Core.stdout, "Hardware shutdown complete.")
 end
 
@@ -62,99 +48,32 @@ function handle_err(ec)
     return
 end
 
-
-# =============================================================================
-# Motor Output Functions
-# =============================================================================
-
-"""
-    car_stop!(hw::HardwareContext)
-
-Stop both motors by setting PWM to 0 and appropriate direction pins.
-"""
-function car_stop!(hw::HardwareContext)
-    GPIO.set_value(hw.ain1, 1)  # HIGH
-    GPIO.set_value(hw.bin1, 0)  # LOW
-    GPIO.set_value(hw.stby, 1)  # HIGH (standby off = enabled, but PWM=0)
-    PWM.pwmWrite(hw.pwm_left, 0, PWM_MAX_VALUE)
-    PWM.pwmWrite(hw.pwm_right, 0, PWM_MAX_VALUE)
-end
-
-"""
-    apply_motor_output!(hw::HardwareContext, pwm_left, pwm_right)
-
-Apply PWM signals to motors based on computed control values.
-Handles direction pin setting based on PWM sign.
-"""
-function apply_motor_output!(hw::HardwareContext, pwm_left::Float32, pwm_right::Float32)
-    # Left motor
-    if pwm_left < 0
-        GPIO.set_value(hw.ain1, 1)
-        PWM.pwmWrite(hw.pwm_left, round(Int, -pwm_left), PWM_MAX_VALUE)
-    else
-        GPIO.set_value(hw.ain1, 0)
-        PWM.pwmWrite(hw.pwm_left, round(Int, pwm_left), PWM_MAX_VALUE)
-    end
-
-    # Right motor
-    if pwm_right < 0
-        GPIO.set_value(hw.bin1, 1)
-        PWM.pwmWrite(hw.pwm_right, round(Int, -pwm_right), PWM_MAX_VALUE)
-    else
-        GPIO.set_value(hw.bin1, 0)
-        PWM.pwmWrite(hw.pwm_right, round(Int, pwm_right), PWM_MAX_VALUE)
-    end
-end
+const CM_PRESENT=23
+const D1=5
+const D2=24
 
 function (@main)(args)::Cint
     println(Core.stdout, "Balance car starting...")
 
     # Initialize GPIO controller
     gpio = GPIO.open_gpio("/dev/gpiochip0")
+    hw = HardwareContext(gpio)
 
     # Setup GPIO output pins for motor direction
-    ain1 = GPIO.request_output(gpio, AIN1, "motor_ain1", 0)
-    bin1 = GPIO.request_output(gpio, BIN1, "motor_bin1", 0)
-    stby = GPIO.request_output(gpio, STBY_PIN, "motor_stby", 0)
-    ltrans_oe = GPIO.request_output(gpio, LTRANS_OE, "ltrans_oe", 0)
+    cm_present = GPIO.request_output(gpio, CM_PRESENT, "cm_present", 0)
+    d1 = GPIO.request_output(gpio, D1, "d1", 0)
+    d2 = GPIO.request_output(gpio, D2, "d2", 0)
     println(Core.stdout, "GPIO configured")
 
-    # Setup hardware PWM via sysfs
-    pwm_chip = PWM.open_chip(0)
-
-    pwm_left = PWM.export_channel_for_gpio(pwm_chip, PWMA_LEFT)
-    PWM.set_period_hz(pwm_left, PWM_FREQ_HZ)
-    PWM.set_duty_cycle_ns(pwm_left, 0)
-    PWM.enable(pwm_left)
-
-    pwm_right = PWM.export_channel_for_gpio(pwm_chip, PWMB_RIGHT)
-    PWM.set_period_hz(pwm_right, PWM_FREQ_HZ)
-    PWM.set_duty_cycle_ns(pwm_right, 0)
-    PWM.enable(pwm_right)
-    println(Core.stdout, "PWM configured")
-
-    # Create hardware context
-    hw = HardwareContext(gpio, ain1, bin1, stby, ltrans_oe, pwm_chip, pwm_left, pwm_right)
-
-    # Initialize IMU (I2C bus 1, address 0x68)
-    imu = MPU6000(1, 0x68)
-    wake!(imu)
-    println(Core.stdout, "IMU initialized")
-
-    # Initialize encoders (single-threaded, poll-based)
-    enc_left = Encoder(gpio, M1A)
-    enc_right = Encoder(gpio, M2A)
-    println(Core.stdout, "Encoders initialized")
-
-    # Initialize balance controller
-    ctrl = BalanceController.BalanceController()
-
     # Enable hardware
-    GPIO.set_value(ltrans_oe, 1)
-    GPIO.set_value(stby, 1)
+    GPIO.set_value(cm_present, 1)
+    GPIO.set_value(d1, 1)
+    GPIO.set_value(d2, 0)
+    d1v = 1
+    d2v = 0
 
     # Control loop timing (5ms = 200Hz)
-    loop_period_ns = 5_000_000
+    loop_period_ns = 500_000_000
 
     println(Core.stdout, "Starting control loop...")
     Base.exit_on_sigint(false)
@@ -166,30 +85,10 @@ function (@main)(args)::Cint
             wait_until(loop_start + loop_period_ns)
             loop_start = time_ns()
 
-            # Read IMU (synchronous, ~1.3ms at 100kHz SMBus for 14 bytes)
-            imu_data = read_all_raw(imu)
-
-            # Drain encoder events that occurred since last loop
-            drain_events!(enc_left)
-            drain_events!(enc_right)
-            enc_left_cnt = enc_left.count
-            enc_right_cnt = enc_right.count
-            reset!(enc_left)
-            reset!(enc_right)
-
-            # Run balance controller
-            command = BalanceController.balance_car!(ctrl,
-                enc_left_cnt, enc_right_cnt,
-                imu_data.accel_x, imu_data.accel_y, imu_data.accel_z,
-                imu_data.gyro_x, imu_data.gyro_y, imu_data.gyro_z)
-
-            # Apply motor output
-            if isnothing(command)
-                car_stop!(hw)
-            else
-                left, right = command
-                apply_motor_output!(hw, left, right)
-            end
+            d1v = 1-d1v
+            d2v = 1-d2v
+            GPIO.set_value(d1, d1v)
+            GPIO.set_value(d2, d2v)
         end
     catch e
         if e isa InterruptException