From a20817d55f812965259ca2c504ef311d4d9fbe97 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 2 May 2024 11:25:17 -0500
Subject: [PATCH] move_to (untested)

---
 src/encoder.rs |  6 +++++-
 src/main.rs    | 43 ++++++++++++++++++++++++++++++++++++++++---
 2 files changed, 45 insertions(+), 4 deletions(-)

diff --git a/src/encoder.rs b/src/encoder.rs
index 673e2d8..36c0e6c 100644
--- a/src/encoder.rs
+++ b/src/encoder.rs
@@ -14,7 +14,11 @@ pub fn setup_encoder(d1: Pin<Input<Floating>, PD1>, d2: Pin<Input<Floating>, PD2
     int.eimsk.write(|w| w.int1().set_bit());
 }
 
-pub fn position() -> i64{
+pub fn rotations() -> f32 {
+    ticks() as f32 / TICKS_PER_ROT
+}
+
+pub fn ticks() -> i64{
     interrupt::free(|cs| COUNTS.borrow(cs).get())
 }
 
diff --git a/src/main.rs b/src/main.rs
index 5cfdd75..d5758e0 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -2,7 +2,8 @@
 #![no_main]
 #![feature(abi_avr_interrupt, cell_update)]
 
-use encoder::{setup_encoder, TICKS_PER_ROT};
+use arduino_hal::pac::tc1::OCR1A;
+use encoder::{rotations, setup_encoder, TICKS_PER_ROT};
 use panic_halt as _;
 mod servo;
 use servo::{configure_timer_one, Servo};
@@ -22,11 +23,47 @@ fn main() -> ! {
     let (carriage, claw) = configure_timer_one(&dp.TC1);
 
     loop {
-        let position = encoder::position();
+        let position = encoder::ticks();
 
-        let gain = position as f32 / TICKS_PER_ROT;
+        let gain = rotations();
 
         carriage.set_speed(gain);
         arduino_hal::delay_ms(20);
     }
 }
+
+
+const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
+const ACCEPTABLE_ERROR: f32 = 0.2; // rotations
+const KP: f32 = 0.2;
+fn move_to(carriage: &OCR1A, position: f32) {
+    loop {
+        let current = rotations();
+        let setpoint = approach(current, position, MAX_VELOCITY);
+        let error = current - setpoint;
+
+        carriage.set_speed(error * KP);
+        if abs(error) < ACCEPTABLE_ERROR && setpoint == position {
+            break;
+        }
+        arduino_hal::delay_ms(10);
+    }
+}
+
+fn approach(current: f32, goal: f32, max: f32) -> f32 {
+    if current > goal {
+        goal.max(current - max)
+    } else {
+        goal.min(current + max)
+    }
+}
+
+// not in core for whatever reason (probably nan or something)
+fn abs(val:f32) -> f32 {
+    // TODO: bit-twiddling
+    if val < 0. {
+        -val
+    } else {
+        val
+    }
+}