From bc920907bd4b979359f175121ad6b5bf71c14b18 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 1 May 2024 15:58:00 -0500
Subject: [PATCH] servo control

---
 Cargo.toml   |  8 +++++---
 src/main.rs  | 27 ++++++++++++------------
 src/servo.rs | 58 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 77 insertions(+), 16 deletions(-)
 create mode 100644 src/servo.rs

diff --git a/Cargo.toml b/Cargo.toml
index 49aa2c1..ad1ee19 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -12,12 +12,14 @@ bench = false
 [dependencies]
 panic-halt = "0.2.0"
 ufmt = "0.2.0"
-nb = "0.1.2"
-embedded-hal = "0.2.3"
+nb = "1"
+embedded-hal = "1"
+
+[dependencies.avr-hal-generic]
+git = "https://github.com/rahix/avr-hal"
 
 [dependencies.arduino-hal]
 git = "https://github.com/rahix/avr-hal"
-rev = "3e362624547462928a219c40f9ea8e3a64f21e5f"
 features = ["arduino-uno"]
 
 # The latest releases of `proc-macro2` do not support the rust toolchain that
diff --git a/src/main.rs b/src/main.rs
index 7a6a7ba..b84da1f 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,27 +1,28 @@
-#![no_std]
+#![cfg_attr(not(test), no_std)]
 #![no_main]
 
 use panic_halt as _;
+mod servo;
+use servo::Servo;
 
 #[arduino_hal::entry]
 fn main() -> ! {
     let dp = arduino_hal::Peripherals::take().unwrap();
     let pins = arduino_hal::pins!(dp);
 
-    /*
-     * For examples (and inspiration), head to
-     *
-     *     https://github.com/Rahix/avr-hal/tree/main/examples
-     *
-     * NOTE: Not all examples were ported to all boards!  There is a good chance though, that code
-     * for a different board can be adapted for yours.  The Arduino Uno currently has the most
-     * examples available.
-     */
+    // configure timer for servos
+    pins.d9.into_output();
+    pins.d10.into_output();
+    let tc1 = dp.TC1; // 250kHz
+    tc1.icr1.write(|w| w.bits(4999)); // 250kHz/5000 = 50Hz
+    tc1.tccr1a.write(|w| w.wgm1().bits(0b10).com1a().match_clear());
+    tc1.tccr1b.write(|w| w.wgm1().bits(0b11).cs1().prescale_64());
 
-    let mut led = pins.d13.into_output();
+    let carriage = &tc1.ocr1a; // pin 9
+    let claw = &tc1.ocr1b; // pin 10
 
     loop {
-        led.toggle();
-        arduino_hal::delay_ms(1000);
+        carriage.set_speed(1.0);
+        arduino_hal::delay_ms(20);
     }
 }
diff --git a/src/servo.rs b/src/servo.rs
new file mode 100644
index 0000000..5e4c342
--- /dev/null
+++ b/src/servo.rs
@@ -0,0 +1,58 @@
+use arduino_hal::pac::tc1::OCR1B;
+use arduino_hal::pac::tc1::OCR1A;
+
+const MIN: u16 = 100; // *4us =  .4ms
+const MAX: u16 = 700; // *4us = 2.8ms
+const RANGE: u16 = MAX-MIN;
+
+pub(crate) trait Servo {
+    /// -1.0 full reverse, 1.0 full forward
+    fn set_speed(&self, speed: f32) {
+        self.set_duty(calculate_duty(speed));
+    }
+
+    fn set_duty(&self, duty: u16);
+}
+
+#[inline(always)]
+/// -1.0 full reverse, 1.0 full forward
+fn calculate_duty(speed: f32) -> u16 {
+    let speed = speed / 2.0 + 0.5;
+    let duty = speed * RANGE as f32 + MIN as f32;
+    duty as u16
+}
+
+impl Servo for OCR1A { // pin 9
+    fn set_duty(&self, duty: u16) {
+        self.write(|w| w.bits(duty));
+    }
+}
+
+impl Servo for OCR1B { // pin 10
+    fn set_duty(&self, duty: u16) {
+        self.write(|w| w.bits(duty));
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    // unit tests don't run on avr-hal-template projects, could be due to target rules, could be
+    // no_std. to troubleshoot later
+    extern crate std;
+    use crate::servo::calculate_duty;
+
+    #[test]
+    fn forward() {
+        assert_eq!(MAX, calculate_duty(1.0));
+    }
+
+    #[test]
+    fn reverse() {
+        assert_eq!(MIN, calculate_duty(-1.0));
+    }
+
+    #[test]
+    fn stopped() {
+        assert_eq!(MIN+RANGE/2, calculate_duty(0.0));
+    }
+}