ank/pick-n-place
1
Fork 0
Code Issues Activity

extracted encoder to module (hopefully interrupts work)

Browse source
This commit is contained in:
Andy Killorin 2024-05-02 09:29:16 -05:00
parent a017b86e8c
commit df36230608
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 61 additions and 50 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

56
src/encoder.rs Normal file
View file

@ -0,0 +1,56 @@
use core::{cell::{Cell, OnceCell}, sync::atomic::AtomicU8};
use arduino_hal::{hal::port::{PD1, PD2}, pac::{exint::eicra::{ISC0_A, ISC1_A}, EXINT}, port::{mode::{Floating, Input}, Pin}};
use avr_device::interrupt::{self, CriticalSection, Mutex};
pub fn setup_encoder(d1: Pin<Input<Floating>, PD1>, d2: Pin<Input<Floating>, PD2>, int: EXINT) {
interrupt::free(|cs| {
let _ = ENCODER.borrow(cs).set((d1, d2));
});
//dp.EXINT.pcifr.write(0b11);
int.eicra.write(|w| w.isc0().bits(ISC0_A::VAL_0X01 as u8)); // rising and falling edge
int.eimsk.write(|w| w.int0().set_bit());
int.eicra.write(|w| w.isc1().bits(ISC1_A::VAL_0X01 as u8)); // rising and falling edge
int.eimsk.write(|w| w.int1().set_bit());
}
pub fn position() -> i64{
interrupt::free(|cs| COUNTS.borrow(cs).get())
}
static ENCODER: Mutex<OnceCell<(Pin<Input<Floating>, PD1>, Pin<Input<Floating>, PD2>)>> = Mutex::new(OnceCell::new());
static LAST: AtomicU8 = AtomicU8::new(0);
pub static COUNTS: Mutex<Cell<i64>> = Mutex::new(Cell::new(0));
pub const TICKS_PER_ROT: f32 = 90. * 4.;
#[avr_device_macros::interrupt(atmega328p)]
fn INT0() {
interrupt::free(|cs| update_encoder(cs))
}
#[avr_device_macros::interrupt(atmega328p)]
fn INT1() {
interrupt::free(|cs| update_encoder(cs))
}
fn update_encoder(cs: CriticalSection) {
let (d1,d2) = ENCODER.borrow(cs).get().unwrap();
let d1 = d1.is_high() as u8;
let d2 = d2.is_high() as u8;
let curr = d2<<1 +d1;
let last = LAST.load(core::sync::atomic::Ordering::SeqCst);
// could probably do this bitwise (subtract individual phases?)
let diff: i8 = match curr<<2 +last {
0b0001 => 1,
0b0111 => 1,
0b1110 => 1,
0b1000 => 1,
0b0010 => -1,
0b1011 => -1,
0b1101 => -1,
0b0100 => -1,
_ => 0,
};
COUNTS.borrow(cs).update(|v| v+diff as i64);
LAST.store(curr, core::sync::atomic::Ordering::SeqCst);
}

55
src/main.rs
View file

@ -2,27 +2,19 @@
#![no_main]
#![feature(abi_avr_interrupt, cell_update)]
use core::{cell::{Cell, OnceCell}, sync::atomic::AtomicU8};
use arduino_hal::{hal::port::{PD1, PD2}, pac::exint::eicra::{ISC0_A, ISC1_A}, port::{mode::{Floating, Input}, Pin}};
use avr_device::interrupt::{self, CriticalSection, Mutex};
use encoder::{setup_encoder, TICKS_PER_ROT};
use panic_halt as _;
mod servo;
use servo::{configure_timer_one, Servo};
mod encoder;
#[arduino_hal::entry]
fn main() -> ! {
let dp = arduino_hal::Peripherals::take().unwrap();
let pins = arduino_hal::pins!(dp);
interrupt::free(|cs| {
let _ = ENCODER.borrow(cs).set((pins.d1, pins.d2));
});
//dp.EXINT.pcifr.write(0b11);
dp.EXINT.eicra.write(|w| w.isc0().bits(ISC0_A::VAL_0X01 as u8)); // rising and falling edge
dp.EXINT.eimsk.write(|w| w.int0().set_bit());
dp.EXINT.eicra.write(|w| w.isc1().bits(ISC1_A::VAL_0X01 as u8)); // rising and falling edge
dp.EXINT.eimsk.write(|w| w.int1().set_bit());
// configure encoder
setup_encoder(pins.d1, pins.d2, dp.EXINT);
// configure timer for servos
pins.d9.into_output(); // carriage
@ -30,7 +22,7 @@ fn main() -> ! {
let (carriage, claw) = configure_timer_one(&dp.TC1);
loop {
let position = interrupt::free(|cs| COUNTS.borrow(cs).get());
let position = encoder::position();
let gain = position as f32 / TICKS_PER_ROT;
@ -38,40 +30,3 @@ fn main() -> ! {
arduino_hal::delay_ms(20);
}
}
static ENCODER: Mutex<OnceCell<(Pin<Input<Floating>, PD1>, Pin<Input<Floating>, PD2>)>> = Mutex::new(OnceCell::new());
static LAST: AtomicU8 = AtomicU8::new(0);
static COUNTS: Mutex<Cell<i64>> = Mutex::new(Cell::new(0));
const TICKS_PER_ROT: f32 = 90. * 4.;
#[avr_device_macros::interrupt(atmega328p)]
fn INT0() {
interrupt::free(|cs| update_encoder(cs))
}
#[avr_device_macros::interrupt(atmega328p)]
fn INT1() {
interrupt::free(|cs| update_encoder(cs))
}
fn update_encoder(cs: CriticalSection) {
let (d1,d2) = ENCODER.borrow(cs).get().unwrap();
let d1 = d1.is_high() as u8;
let d2 = d2.is_high() as u8;
let curr = d2<<1 +d1;
let last = LAST.load(core::sync::atomic::Ordering::SeqCst);
// could probably do this bitwise (subtract individual phases?)
let diff: i8 = match curr<<2 +last {
0b0001 => 1,
0b0111 => 1,
0b1110 => 1,
0b1000 => 1,
0b0010 => -1,
0b1011 => -1,
0b1101 => -1,
0b0100 => -1,
_ => 0,
};
COUNTS.borrow(cs).update(|v| v+diff as i64);
LAST.store(curr, core::sync::atomic::Ordering::SeqCst);
}