125 lines
3.5 KiB
Rust
125 lines
3.5 KiB
Rust
#![cfg_attr(not(test), no_std)]
|
|
#![no_main]
|
|
#![feature(abi_avr_interrupt, cell_update)]
|
|
|
|
use core::sync::atomic::Ordering;
|
|
|
|
use arduino_hal::{default_serial, delay_ms, hal::port::{PD4, PD5}, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
|
|
use avr_device::interrupt;
|
|
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;
|
|
|
|
use crate::servo::calculate_duty;
|
|
mod encoder;
|
|
|
|
// d3: encoder
|
|
// d4: encoder
|
|
// d5: limit switch
|
|
// d6: lift piston
|
|
// d9: carriage
|
|
// d10: claw
|
|
|
|
#[arduino_hal::entry]
|
|
fn main() -> ! {
|
|
let dp = arduino_hal::Peripherals::take().unwrap();
|
|
let pins = arduino_hal::pins!(dp);
|
|
|
|
let mut serial = arduino_hal::Usart::new(
|
|
dp.USART0,
|
|
pins.d0,
|
|
pins.d1.into_output(),
|
|
arduino_hal::hal::usart::BaudrateArduinoExt::into_baudrate(57600),
|
|
);
|
|
|
|
// configure encoder
|
|
setup_encoder(pins.d3, pins.d4, dp.EXINT);
|
|
|
|
// configure timer for servos
|
|
pins.d9.into_output(); // carriage
|
|
pins.d10.into_output(); // claw
|
|
let (carriage, claw) = configure_timer_one(&dp.TC1);
|
|
|
|
let switch = pins.d5.into_pull_up_input();
|
|
|
|
//home(&carriage, &switch);
|
|
|
|
//uwriteln!(serial, "homed");
|
|
|
|
//claw.set_speed(-0.8);
|
|
//delay_ms(500);
|
|
//claw.set_speed(0.0);
|
|
//uwriteln!(serial, "gripped");
|
|
//move_to(&carriage, 0.125);
|
|
//uwriteln!(serial, "moved");
|
|
////claw.set_speed(0.4);
|
|
|
|
loop {
|
|
//uwriteln!(serial, "svith {}", switch.is_high());
|
|
let gain = rotations();
|
|
uwriteln!(serial, "speed {}", (gain * 1000.) as i16);
|
|
//uwriteln!(serial, "speed {}", calculate_duty(gain));
|
|
|
|
carriage.set_speed(gain);
|
|
arduino_hal::delay_ms(20);
|
|
}
|
|
}
|
|
|
|
const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
|
|
const ACCEPTABLE_ERROR: f32 = 0.02; // rotations
|
|
const KP: f32 = 2.2;
|
|
const MIN_SPEED: f32 = 0.128; // motor speed that overcomes the friction of the table
|
|
fn move_to(carriage: &OCR1A, position: f32) {
|
|
loop {
|
|
let current = rotations();
|
|
let setpoint = approach(current, position, MAX_VELOCITY);
|
|
let error = current - setpoint;
|
|
|
|
let out = error * KP;
|
|
carriage.set_speed(-half_deadzone(out, MIN_SPEED));
|
|
if abs(error) < ACCEPTABLE_ERROR && setpoint == position {
|
|
break;
|
|
}
|
|
arduino_hal::delay_ms(10);
|
|
}
|
|
}
|
|
|
|
/// 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 {
|
|
if current > goal {
|
|
goal.max(current - max)
|
|
} else {
|
|
goal.min(current + max)
|
|
}
|
|
}
|
|
|
|
// not in core for whatever reason (probably nan or something)
|
|
fn abs(val:f32) -> f32 {
|
|
// TODO: bit-twiddling
|
|
if val < 0. {
|
|
-val
|
|
} else {
|
|
val
|
|
}
|
|
}
|
|
|
|
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.7);
|
|
while switch.is_low() { arduino_hal::delay_us(5) };
|
|
carriage.set_speed(0.0);
|
|
|
|
interrupt::free(|cs| COUNTS.borrow(cs).set(0));
|
|
}
|