From d86e388182f322d16b70977c270fd1f4574cb5e6 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 2 May 2024 11:32:27 -0500
Subject: [PATCH] added homing function

---
 src/main.rs | 26 ++++++++++++++++++++++----
 1 file changed, 22 insertions(+), 4 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index d5758e0..a1319c6 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -2,13 +2,21 @@
 #![no_main]
 #![feature(abi_avr_interrupt, cell_update)]
 
-use arduino_hal::pac::tc1::OCR1A;
-use encoder::{rotations, setup_encoder, TICKS_PER_ROT};
+use arduino_hal::{hal::port::PD4, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
+use avr_device::interrupt;
+use encoder::{rotations, setup_encoder, COUNTS, TICKS_PER_ROT};
 use panic_halt as _;
 mod servo;
 use servo::{configure_timer_one, Servo};
 mod encoder;
 
+// d1: encoder 
+// d2: encoder
+// d4: limit switch
+// d5: lift piston
+// d9: carriage
+// d10: claw
+
 #[arduino_hal::entry]
 fn main() -> ! {
     let dp = arduino_hal::Peripherals::take().unwrap();
@@ -22,9 +30,10 @@ fn main() -> ! {
     pins.d10.into_output(); // claw
     let (carriage, claw) = configure_timer_one(&dp.TC1);
 
-    loop {
-        let position = encoder::ticks();
+    let switch = pins.d4.into_pull_up_input();
 
+    home(&carriage, &switch);
+    loop {
         let gain = rotations();
 
         carriage.set_speed(gain);
@@ -67,3 +76,12 @@ fn abs(val:f32) -> f32 {
         val
     }
 }
+
+fn home(carriage: &OCR1A, switch: &Pin<Input<PullUp>, PD4>) {
+    carriage.set_speed(-0.2);
+    while switch.is_high() { arduino_hal::delay_us(5) };
+    carriage.set_speed(0.1);
+    while switch.is_low() { arduino_hal::delay_us(5) };
+
+    interrupt::free(|cs| COUNTS.borrow(cs).set(0));
+}