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

incremental control theory changes (currently abysmal)

Browse source
This commit is contained in:
Andy Killorin 2024-05-14 13:37:58 -05:00
parent 022e4317f2
commit 5fcdc4cf14
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 34 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

44
src/main.rs
View file

@ -4,13 +4,15 @@
use core::sync::atomic::Ordering; use core::sync::atomic::Ordering;
use arduino_hal::{default_serial, hal::port::PD4, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}}; use arduino_hal::{default_serial, delay_ms, hal::port::{PD4, PD5}, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
use avr_device::interrupt; use avr_device::interrupt;
use encoder::{rotations, setup_encoder, update_encoder, COUNTS, TICKS_PER_ROT}; use encoder::{rotations, setup_encoder, update_encoder, COUNTS, TICKS_PER_ROT};
use panic_halt as _; use panic_halt as _;
mod servo; mod servo;
use servo::{configure_timer_one, Servo}; use servo::{configure_timer_one, Servo};
use ufmt::uwriteln; use ufmt::uwriteln;
use crate::servo::calculate_duty;
mod encoder; mod encoder;
// d3: encoder // d3: encoder
@ -44,12 +46,21 @@ fn main() -> ! {
//home(&carriage, &switch); //home(&carriage, &switch);
//claw.set_speed(-0.4); //uwriteln!(serial, "homed");
//move_to(&carriage, 1.);
//claw.set_speed(0.4); //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 { loop {
//uwriteln!(serial, "svith {}", switch.is_high());
let gain = rotations(); let gain = rotations();
uwriteln!(serial, "speed {}", (gain * 1000.) as i16);
//uwriteln!(serial, "speed {}", calculate_duty(gain));
carriage.set_speed(gain); carriage.set_speed(gain);
arduino_hal::delay_ms(20); arduino_hal::delay_ms(20);
@ -57,15 +68,17 @@ fn main() -> ! {
} }
const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
const ACCEPTABLE_ERROR: f32 = 0.2; // rotations const ACCEPTABLE_ERROR: f32 = 0.02; // rotations
const KP: f32 = 0.2; 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) { fn move_to(carriage: &OCR1A, position: f32) {
loop { loop {
let current = rotations(); let current = rotations();
let setpoint = approach(current, position, MAX_VELOCITY); let setpoint = approach(current, position, MAX_VELOCITY);
let error = current - setpoint; let error = current - setpoint;
carriage.set_speed(error * KP); let out = error * KP;
carriage.set_speed(-half_deadzone(out, MIN_SPEED));
if abs(error) < ACCEPTABLE_ERROR && setpoint == position { if abs(error) < ACCEPTABLE_ERROR && setpoint == position {
break; break;
} }
@ -73,6 +86,16 @@ fn move_to(carriage: &OCR1A, position: f32) {
} }
} }
/// 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 { fn approach(current: f32, goal: f32, max: f32) -> f32 {
if current > goal { if current > goal {
goal.max(current - max) goal.max(current - max)
@ -91,11 +114,12 @@ fn abs(val:f32) -> f32 {
} }
} }
fn home(carriage: &OCR1A, switch: &Pin<Input<PullUp>, PD4>) { fn home(carriage: &OCR1A, switch: &Pin<Input<PullUp>, PD5>) {
carriage.set_speed(-0.2); carriage.set_speed(-0.8);
while switch.is_high() { arduino_hal::delay_us(5) }; 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) }; while switch.is_low() { arduino_hal::delay_us(5) };
carriage.set_speed(0.0);
interrupt::free(|cs| COUNTS.borrow(cs).set(0)); interrupt::free(|cs| COUNTS.borrow(cs).set(0));
} }