diff --git a/src/main.rs b/src/main.rs index f5dde94..c96a3b3 100644 --- a/src/main.rs +++ b/src/main.rs @@ -3,7 +3,7 @@ #![feature(abi_avr_interrupt, cell_update)] -use arduino_hal::{delay_ms, hal::port::PD5, 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, COUNTS}; use panic_halt as _; @@ -44,38 +44,58 @@ fn main() -> ! { 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; + + piston.set_high(); uwriteln!(serial, "homing").unwrap(); home(carriage, &switch); uwriteln!(serial, "homed").unwrap(); - move_to(&carriage, 0.125); uwriteln!(serial, "start").unwrap(); loop { - claw.set_speed(-0.2); // close - uwriteln!(serial, "close").unwrap(); - delay_ms(1000); - claw.set_speed(0.0); + // get jar + move_to(&carriage, CONVEYOR); + open(&claw); + piston.set_low(); delay_ms(1000); + close(&claw); piston.set_high(); // up - delay_ms(1000); - - move_to(&carriage, 0.5); // go + delay_ms(1000); // rise + // drop jar + move_to(&carriage, SPINNER); piston.set_low(); // down delay_ms(1000); + open(&claw); + piston.set_high(); // up + delay_ms(1000); // rise - claw.set_speed(0.2); // open + // get cap + piston.set_high(); // up + delay_ms(1000); // rise + move_to(&carriage, LIDS); + piston.set_low(); // down delay_ms(1000); - claw.set_speed(0.0); + 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(1000); - - move_to(&carriage, 0.125); // initial - - piston.set_low(); //down - delay_ms(1000); + delay_ms(10_000); } } @@ -146,3 +166,17 @@ fn home(carriage: &OCR1A, switch: &Pin, PD5>) { 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); +}