From 5fcdc4cf14ff36e2e75cb0dc3a2023f91ee7976a Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Tue, 14 May 2024 13:37:58 -0500
Subject: [PATCH] incremental control theory changes (currently abysmal)

---
 src/main.rs | 44 ++++++++++++++++++++++++++++++++++----------
 1 file changed, 34 insertions(+), 10 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index c49fd22..b0fc831 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -4,13 +4,15 @@
 
 use core::sync::atomic::Ordering;
 
-use arduino_hal::{default_serial, hal::port::PD4, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
+use arduino_hal::{default_serial, delay_ms, hal::port::{PD4, PD5}, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
 use avr_device::interrupt;
 use encoder::{rotations, setup_encoder, update_encoder, COUNTS, TICKS_PER_ROT};
 use panic_halt as _;
 mod servo;
 use servo::{configure_timer_one, Servo};
 use ufmt::uwriteln;
+
+use crate::servo::calculate_duty;
 mod encoder;
 
 // d3: encoder
@@ -44,12 +46,21 @@ fn main() -> ! {
 
     //home(&carriage, &switch);
 
-    //claw.set_speed(-0.4);
-    //move_to(&carriage, 1.);
-    //claw.set_speed(0.4);
+    //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");
+    ////claw.set_speed(0.4);
 
     loop {
+        //uwriteln!(serial, "svith {}", switch.is_high());
         let gain = rotations();
+        uwriteln!(serial, "speed {}", (gain * 1000.) as i16);
+        //uwriteln!(serial, "speed {}", calculate_duty(gain));
 
         carriage.set_speed(gain);
         arduino_hal::delay_ms(20);
@@ -57,15 +68,17 @@ fn main() -> ! {
 }
 
 const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
-const ACCEPTABLE_ERROR: f32 = 0.2; // rotations
-const KP: f32 = 0.2;
+const ACCEPTABLE_ERROR: f32 = 0.02; // rotations
+const KP: f32 = 2.2;
+const MIN_SPEED: f32 = 0.128; // motor speed that overcomes the friction of the table
 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);
+        let out = error * KP;
+        carriage.set_speed(-half_deadzone(out, MIN_SPEED));
         if abs(error) < ACCEPTABLE_ERROR && setpoint == position {
             break;
         }
@@ -73,6 +86,16 @@ fn move_to(carriage: &OCR1A, position: f32) {
     }
 }
 
+/// If val is != 0, map it from 0..1 to min..1 (or negative)
+fn half_deadzone(val:f32, min:f32) -> f32 {
+    if val == 0. { return 0.; }
+    let neg = val < 0.;
+    let val = abs(val);
+
+    let val = (val * (1. - min)) + min;
+    if neg {-val} else {val}
+}
+
 fn approach(current: f32, goal: f32, max: f32) -> f32 {
     if current > goal {
         goal.max(current - max)
@@ -91,11 +114,12 @@ fn abs(val:f32) -> f32 {
     }
 }
 
-fn home(carriage: &OCR1A, switch: &Pin<Input<PullUp>, PD4>) {
-    carriage.set_speed(-0.2);
+fn home(carriage: &OCR1A, switch: &Pin<Input<PullUp>, PD5>) {
+    carriage.set_speed(-0.8);
     while switch.is_high() { arduino_hal::delay_us(5) };
-    carriage.set_speed(0.1);
+    carriage.set_speed(0.7);
     while switch.is_low() { arduino_hal::delay_us(5) };
+    carriage.set_speed(0.0);
 
     interrupt::free(|cs| COUNTS.borrow(cs).set(0));
 }