From d6614af693b6150e8a4ab2a607474004f08eac6f Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Tue, 21 May 2024 08:11:19 -0500
Subject: [PATCH] test cycle

this works aside from shaking itself around
---
 src/main.rs | 63 ++++++++++++++++++++++++++---------------------------
 1 file changed, 31 insertions(+), 32 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index 3bfd2f8..f5dde94 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -40,44 +40,42 @@ fn main() -> ! {
     pins.d10.into_output(); // claw
     let (carriage, claw) = configure_timer_one(&dp.TC1);
 
+    let mut piston = pins.d6.into_output();
+
     let switch = pins.d5.into_pull_up_input();
 
-    home(&carriage, &switch);
-
-    uwriteln!(serial, "homed");
-
-    //claw.set_speed(-0.8);
-    //delay_ms(500);
-    //claw.set_speed(0.0);
-    //uwriteln!(serial, "gripped");
+    uwriteln!(serial, "homing").unwrap();
+    home(carriage, &switch);
+    uwriteln!(serial, "homed").unwrap();
     move_to(&carriage, 0.125);
-    uwriteln!(serial, "moved");
-    carriage.set_speed(0.);
-    delay_ms(500);
+    uwriteln!(serial, "start").unwrap();
 
-    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));
-
-    //    carriage.set_speed(half_deadzone(gain, MIN_SPEED));
-    //    arduino_hal::delay_ms(20);
-    //}
     loop {
+        claw.set_speed(-0.2); // close
+        uwriteln!(serial, "close").unwrap();
+        delay_ms(1000);
+        claw.set_speed(0.0);
+        delay_ms(1000);
         
+        piston.set_high(); // up 
+        delay_ms(1000);
+
+        move_to(&carriage, 0.5); // go
+
+        piston.set_low(); // down 
+        delay_ms(1000);
+
+        claw.set_speed(0.2); // open
+        delay_ms(1000);
+        claw.set_speed(0.0);
+
+        piston.set_high(); // up 
+        delay_ms(1000);
+
+        move_to(&carriage, 0.125); // initial
+
+        piston.set_low(); //down
+        delay_ms(1000);
     }
 }
 
@@ -108,6 +106,7 @@ fn move_to(carriage: &OCR1A, position: f32) {
         }
         arduino_hal::delay_ms(10);
     }
+    carriage.set_speed(0.);
 }
 
 /// If val is != 0, map it from 0..1 to min..1 (or negative)