Second switch, directional control, set endpoint, lerp

src/main.rs

@@ -68,39 +68,49 @@ fn main() -> ! {
 
     let mut servo = pins.d9.into_output().downgrade();
     let switch = pins.d10.into_pull_up_input();
+    let switch2 = pins.d11.into_pull_up_input();
     println!("hewwo :3");
 
     // Variables for loop-persistent data
-    let mut switchSmoothed: f32 = 0.0;
-    let mut switchPrev: f32 = 0.0;
     let mut switch_count = 0.0;
+    let mut endpoint = 75.0;
 
     loop {
         /* Get Input */
         // This debouncing sucks ass and isn't good - doesn't matter, just for testing
         switch_count = match switch.is_low() {
-            true => 1.0,
+            true => {
+                endpoint = 170.0;
+                1.0
+            },
             false => {
                 if switch_count > 0.0 {
-                    switch_count - 0.2
+                    switch_count - 0.1
+                } else {
+                    0.0
+                }
+            },
+        };
+        switch_count = match switch2.is_low() {
+            true => {
+                endpoint = 5.0;
+                1.0
+            },
+            false => {
+                if switch_count > 0.0 {
+                    switch_count - 0.1
                 } else {
                     0.0
                 }
             },
         };
 
-        let val;
-        if switch_count > 0.0 {
-            val = 1.0;
-        } else {
-            val = 0.0;
-        }
-
         /* Move Servos */
-        switchSmoothed = (switch_count * 0.05) + (switchPrev * 0.95);
-        switchPrev = switchSmoothed;
-        let deg = (190.0 * switchSmoothed) + 25.0;
-
+        let deg = lerp(75.0, endpoint, switch_count);
         set_servo_degree(&mut servo, deg);
     }
 }
+
+fn lerp(v0: f32, v1: f32, t: f32) -> f32 {
+    return (1.0 - t) * v0 + t * v1;
+}