move_to (untested)

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())
 }
 

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
+    }
+}