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

integral implementation (untested)

Browse source
This commit is contained in:
Andy Killorin 2024-05-14 13:48:25 -05:00
parent 5fcdc4cf14
commit 8106dc15b1
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 15 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
src/main.rs
View file

@ -44,16 +44,16 @@ fn main() -> ! {
let switch = pins.d5.into_pull_up_input();
//home(&carriage, &switch);
home(&carriage, &switch);
//uwriteln!(serial, "homed");
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");
move_to(&carriage, 0.125);
uwriteln!(serial, "moved");
////claw.set_speed(0.4);
loop {
@ -70,14 +70,24 @@ fn main() -> ! {
const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
const ACCEPTABLE_ERROR: f32 = 0.02; // rotations
const KP: f32 = 2.2;
const KI: f32 = 1.2;
const MIN_SPEED: f32 = 0.128; // motor speed that overcomes the friction of the table
const I_BUF_LEN: usize = 64;
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;
let out = error * KP;
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));
if abs(error) < ACCEPTABLE_ERROR && setpoint == position {
break;