.cargo/config.toml

@@ -3,8 +3,6 @@ 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,8 +14,6 @@ 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"
@@ -41,4 +39,4 @@ panic = "abort"
 codegen-units = 1
 debug = true
 lto = true
-opt-level = "z"
+opt-level = "s"

src/encoder.rs

@@ -1,6 +1,6 @@
 use core::{cell::{Cell, OnceCell}, sync::atomic::AtomicU8};
 
-use arduino_hal::{hal::port::{PD3, PD4}, pac::{EXINT}, port::{mode::{Floating, Input}, Pin}};
+use arduino_hal::{hal::port::{PD1, PD2, PD3, PD4}, pac::{exint::eicra::{ISC0_A, ISC1_A}, 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::{delay_ms, hal::port::PD5, pac::tc1::{OCR1A, OCR1B}, port::{mode::{Input, PullUp}, Pin}};
+use arduino_hal::{default_serial, hal::port::PD4, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
 use avr_device::interrupt;
-use encoder::{rotations, setup_encoder, COUNTS};
+use encoder::{rotations, setup_encoder, update_encoder, COUNTS, TICKS_PER_ROT};
 use panic_halt as _;
 mod servo;
 use servo::{configure_timer_one, Servo};
 use ufmt::uwriteln;
-
 mod encoder;
 
 // d3: encoder
@@ -40,103 +40,37 @@ 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();
 
-    const CONVEYOR: f32 = 0.37;
-    const SPINNER: f32 = 0.75;
-    const LIDS: f32 = 0.05;
-    const OUT: f32 = 0.89;
+    //home(&carriage, &switch);
 
-    piston.set_high();
-    uwriteln!(serial, "homing").unwrap();
-    home(carriage, &switch);
-    uwriteln!(serial, "homed").unwrap();
-    uwriteln!(serial, "start").unwrap();
+    //claw.set_speed(-0.4);
+    //move_to(&carriage, 1.);
+    //claw.set_speed(0.4);
 
     loop {
-        // get jar
-        move_to(&carriage, CONVEYOR);
-        open(&claw);
-        piston.set_low();
-        delay_ms(1000);
-        close(&claw);
-        
-        piston.set_high(); // up 
-        delay_ms(1000); // rise
+        let gain = rotations();
 
-        // 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);
+        carriage.set_speed(gain);
+        arduino_hal::delay_ms(20);
     }
 }
 
-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;
+const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
+const ACCEPTABLE_ERROR: f32 = 0.2; // rotations
+const KP: f32 = 0.2;
 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;
 
-        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));
+        carriage.set_speed(error * KP);
         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 {
@@ -157,26 +91,11 @@ fn abs(val:f32) -> f32 {
     }
 }
 
-fn home(carriage: &OCR1A, switch: &Pin<Input<PullUp>, PD5>) {
-    carriage.set_speed(-0.8);
+fn home(carriage: &OCR1A, switch: &Pin<Input<PullUp>, PD4>) {
+    carriage.set_speed(-0.2);
     while switch.is_high() { arduino_hal::delay_us(5) };
-    carriage.set_speed(0.7);
+    carriage.set_speed(0.1);
     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,10 +2,8 @@ use arduino_hal::pac::tc1::OCR1B;
 use arduino_hal::pac::tc1::OCR1A;
 use arduino_hal::pac::TC1;
 
-const CENTER: u16 = 375;
-const RADIUS: u16 = 185;
-const MIN: u16 = CENTER-RADIUS; // *4us
-const MAX: u16 = CENTER+RADIUS; // *4us
+const MIN: u16 = 100; // *4us =  .4ms
+const MAX: u16 = 700; // *4us = 2.8ms
 const RANGE: u16 = MAX-MIN;
 
 pub(crate) trait Servo {
@@ -19,7 +17,7 @@ pub(crate) trait Servo {
 
 #[inline(always)]
 /// -1.0 full reverse, 1.0 full forward
-pub fn calculate_duty(speed: f32) -> u16 {
+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
@@ -39,7 +37,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().com1b().match_clear());
+    timer.tccr1a.write(|w| w.wgm1().bits(0b10).com1a().match_clear());
     timer.tccr1b.write(|w| w.wgm1().bits(0b11).cs1().prescale_64());
 
     (&timer.ocr1a, &timer.ocr1b)