From b59195d32ac57d411005fbe288df77075c86d76c Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 17 May 2024 13:52:32 -0500
Subject: [PATCH] demo path

---
 src/main.rs | 40 ++++++++++++++++++++++++++++------------
 1 file changed, 28 insertions(+), 12 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index 35aa186..3bfd2f8 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -3,7 +3,7 @@
 #![feature(abi_avr_interrupt, cell_update)]
 
 
-use arduino_hal::{hal::port::PD5, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
+use arduino_hal::{delay_ms, hal::port::PD5, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
 use avr_device::interrupt;
 use encoder::{rotations, setup_encoder, COUNTS};
 use panic_halt as _;
@@ -52,24 +52,40 @@ fn main() -> ! {
     //uwriteln!(serial, "gripped");
     move_to(&carriage, 0.125);
     uwriteln!(serial, "moved");
+    carriage.set_speed(0.);
+    delay_ms(500);
+
+    move_to(&carriage, 0.5);
+    uwriteln!(serial, "moved .5");
+    carriage.set_speed(0.);
+    delay_ms(500);
+
+    move_to(&carriage, 0.125);
+    uwriteln!(serial, "returnd");
+    carriage.set_speed(0.);
+    delay_ms(500);
+
     ////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));
+    //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);
+    //    carriage.set_speed(half_deadzone(gain, MIN_SPEED));
+    //    arduino_hal::delay_ms(20);
+    //}
+    loop {
+        
     }
 }
 
-const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
+const MAX_VELOCITY: f32 = 0.02; // 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 KP: f32 = 0.8;
+const KI: f32 = 0.4;
+const MIN_SPEED: f32 = 0.218; // 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];