final path

this works aside from shaking itself around

.cargo/config.toml

@@ -3,6 +3,8 @@ target = "avr-specs/avr-atmega328p.json"
 
 [target.'cfg(target_arch = "avr")']
 runner = "ravedude uno -cb 57600"
+# Workaround for: https://github.com/rust-lang/compiler-builtins/issues/420
+rustflags = ["-C", "link-arg=-Wl,--allow-multiple-definition"]
 
 [unstable]
 build-std = ["core"]

Cargo.toml

@@ -14,6 +14,8 @@ panic-halt = "0.2.0"
 ufmt = "0.2.0"
 nb = "1"
 embedded-hal = "1"
+avr-device-macros = "0"
+avr-device = "0"
 
 [dependencies.avr-hal-generic]
 git = "https://github.com/rahix/avr-hal"
@@ -39,4 +41,4 @@ panic = "abort"
 codegen-units = 1
 debug = true
 lto = true
-opt-level = "s"
+opt-level = "z"

src/encoder.rs

@@ -1,6 +1,6 @@
 use core::{cell::{Cell, OnceCell}, sync::atomic::AtomicU8};
 
-use arduino_hal::{hal::port::{PD1, PD2, PD3, PD4}, pac::{exint::eicra::{ISC0_A, ISC1_A}, EXINT}, port::{mode::{Floating, Input}, Pin}};
+use arduino_hal::{hal::port::{PD3, PD4}, pac::{EXINT}, port::{mode::{Floating, Input}, Pin}};
 use avr_device::interrupt::{self, CriticalSection, Mutex};
 pub fn setup_encoder(d3: Pin<Input<Floating>, PD3>, d4: Pin<Input<Floating>, PD4>, int: EXINT) {
     interrupt::free(|cs| {

src/main.rs

@@ -2,15 +2,15 @@
 #![no_main]
 #![feature(abi_avr_interrupt, cell_update)]
 
-use core::sync::atomic::Ordering;
 
-use arduino_hal::{default_serial, hal::port::PD4, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
+use arduino_hal::{delay_ms, hal::port::PD5, pac::tc1::{OCR1A, OCR1B}, port::{mode::{Input, PullUp}, Pin}};
 use avr_device::interrupt;
-use encoder::{rotations, setup_encoder, update_encoder, COUNTS, TICKS_PER_ROT};
+use encoder::{rotations, setup_encoder, COUNTS};
 use panic_halt as _;
 mod servo;
 use servo::{configure_timer_one, Servo};
 use ufmt::uwriteln;
+
 mod encoder;
 
 // d3: encoder
@@ -40,37 +40,103 @@ 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);
+    const CONVEYOR: f32 = 0.37;
+    const SPINNER: f32 = 0.75;
+    const LIDS: f32 = 0.05;
+    const OUT: f32 = 0.89;
 
-    //claw.set_speed(-0.4);
-    //move_to(&carriage, 1.);
-    //claw.set_speed(0.4);
+    piston.set_high();
+    uwriteln!(serial, "homing").unwrap();
+    home(carriage, &switch);
+    uwriteln!(serial, "homed").unwrap();
+    uwriteln!(serial, "start").unwrap();
 
     loop {
-        let gain = rotations();
+        // get jar
+        move_to(&carriage, CONVEYOR);
+        open(&claw);
+        piston.set_low();
+        delay_ms(1000);
+        close(&claw);
+        
+        piston.set_high(); // up 
+        delay_ms(1000); // rise
 
-        carriage.set_speed(gain);
-        arduino_hal::delay_ms(20);
+        // drop jar
+        move_to(&carriage, SPINNER);
+        piston.set_low(); // down 
+        delay_ms(1000);
+        open(&claw);
+        piston.set_high(); // up 
+        delay_ms(1000); // rise
+
+        // get cap
+        piston.set_high(); // up 
+        delay_ms(1000); // rise
+        move_to(&carriage, LIDS);
+        piston.set_low(); // down 
+        delay_ms(1000);
+        close(&claw);
+        piston.set_high(); // up 
+        delay_ms(1000); // rise
+
+        move_to(&carriage, SPINNER);
+        piston.set_low(); // down 
+        delay_ms(1_000); // TODO: spin caps
+
+        //piston.set_high(); // up 
+        //delay_ms(1000); // rise
+
+        //move_to(&carriage, OUT);
+        open(&claw);
+
+        piston.set_high(); // up 
+        delay_ms(10_000);
     }
 }
 
-const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
-const ACCEPTABLE_ERROR: f32 = 0.2; // rotations
-const KP: f32 = 0.2;
+const MAX_VELOCITY: f32 = 0.02; // rotations per 10ms
+const ACCEPTABLE_ERROR: f32 = 0.02; // rotations
+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];
+    let mut i_cursor = 0;
     loop {
         let current = rotations();
         let setpoint = approach(current, position, MAX_VELOCITY);
         let error = current - setpoint;
 
-        carriage.set_speed(error * KP);
+        const I_FIXED_POINT: f32 = 10.;
+        i_buf[i_cursor % I_BUF_LEN] = (error * I_FIXED_POINT) as i8;
+        i_cursor += 1;
+        let integral: i16 = i_buf.iter().map(|n| *n as i16).sum();
+        let integral = (integral as f32) / (I_BUF_LEN as f32 * I_FIXED_POINT);
+
+        let out = error * KP + integral * KI;
+        carriage.set_speed(-half_deadzone(out, MIN_SPEED));
         if abs(error) < ACCEPTABLE_ERROR && setpoint == position {
             break;
         }
         arduino_hal::delay_ms(10);
     }
+    carriage.set_speed(0.);
+}
+
+/// 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 {
@@ -91,11 +157,26 @@ 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));
 }
+
+fn close(claw: &OCR1B) {
+    claw.set_speed(-0.2); // close
+    delay_ms(1000);
+    claw.set_speed(0.0);
+    delay_ms(1000);
+}
+
+fn open(claw: &OCR1B) {
+    claw.set_speed(0.2); // close
+    delay_ms(1000);
+    claw.set_speed(0.0);
+    delay_ms(1000);
+}

src/servo.rs

@@ -2,8 +2,10 @@ use arduino_hal::pac::tc1::OCR1B;
 use arduino_hal::pac::tc1::OCR1A;
 use arduino_hal::pac::TC1;
 
-const MIN: u16 = 100; // *4us =  .4ms
-const MAX: u16 = 700; // *4us = 2.8ms
+const CENTER: u16 = 375;
+const RADIUS: u16 = 185;
+const MIN: u16 = CENTER-RADIUS; // *4us
+const MAX: u16 = CENTER+RADIUS; // *4us
 const RANGE: u16 = MAX-MIN;
 
 pub(crate) trait Servo {
@@ -17,7 +19,7 @@ pub(crate) trait Servo {
 
 #[inline(always)]
 /// -1.0 full reverse, 1.0 full forward
-fn calculate_duty(speed: f32) -> u16 {
+pub 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
@@ -37,7 +39,7 @@ impl Servo for OCR1B { // pin 10
 
 pub fn configure_timer_one(timer: &TC1) -> (&OCR1A, &OCR1B) {
     timer.icr1.write(|w| w.bits(4999)); // 250kHz/5000 = 50Hz
-    timer.tccr1a.write(|w| w.wgm1().bits(0b10).com1a().match_clear());
+    timer.tccr1a.write(|w| w.wgm1().bits(0b10).com1a().match_clear().com1b().match_clear());
     timer.tccr1b.write(|w| w.wgm1().bits(0b11).cs1().prescale_64());
 
     (&timer.ocr1a, &timer.ocr1b)