From 8106dc15b12ed038caeeb50d8148e59fcecf52b1 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Tue, 14 May 2024 13:48:25 -0500
Subject: [PATCH] integral implementation (untested)

---
 src/main.rs | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index b0fc831..e1b9200 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -44,16 +44,16 @@ fn main() -> ! {
 
     let switch = pins.d5.into_pull_up_input();
 
-    //home(&carriage, &switch);
+    home(&carriage, &switch);
 
-    //uwriteln!(serial, "homed");
+    uwriteln!(serial, "homed");
 
     //claw.set_speed(-0.8);
     //delay_ms(500);
     //claw.set_speed(0.0);
     //uwriteln!(serial, "gripped");
-    //move_to(&carriage, 0.125);
-    //uwriteln!(serial, "moved");
+    move_to(&carriage, 0.125);
+    uwriteln!(serial, "moved");
     ////claw.set_speed(0.4);
 
     loop {
@@ -70,14 +70,24 @@ fn main() -> ! {
 const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
 const ACCEPTABLE_ERROR: f32 = 0.02; // rotations
 const KP: f32 = 2.2;
+const KI: f32 = 1.2;
 const MIN_SPEED: f32 = 0.128; // motor speed that overcomes the friction of the table
+const I_BUF_LEN: usize = 64;
 fn move_to(carriage: &OCR1A, position: f32) {
+    let mut i_buf = [0i8;I_BUF_LEN];
+    let mut i_cursor = 0;
     loop {
         let current = rotations();
         let setpoint = approach(current, position, MAX_VELOCITY);
         let error = current - setpoint;
 
-        let out = error * KP;
+        const I_FIXED_POINT: f32 = 10.;
+        i_buf[i_cursor % I_BUF_LEN] = (error * I_FIXED_POINT) as i8;
+        i_cursor += 1;
+        let integral: i16 = i_buf.iter().map(|n| *n as i16).sum();
+        let integral = (integral as f32) / (I_BUF_LEN as f32 * I_FIXED_POINT);
+
+        let out = error * KP + integral * KI;
         carriage.set_speed(-half_deadzone(out, MIN_SPEED));
         if abs(error) < ACCEPTABLE_ERROR && setpoint == position {
             break;