AFG/rotor-control
3
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Added slop to AZ and EL movement

Browse source
This commit is contained in:
Sebastian 2022-09-05 19:21:32 +02:00
parent 29e3d21996
commit 06f4ba549b
2 changed files with 19 additions and 14 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

29
src/movement.rs
View file

@ -17,13 +17,17 @@ const AZ_MIN_READING: f32 = 0.0;
const AZ_MAX_READING: f32 = 4096.0; const AZ_MAX_READING: f32 = 4096.0;
// Range of motion for azimuth (0 to AZ_RANGE) // Range of motion for azimuth (0 to AZ_RANGE)
const AZ_RANGE: f32 = 360.0; const AZ_RANGE: f32 = 360.0;
// Tolerance for the azimuth setpoint
const AZ_SLOP : i16 = 1;
// ADC reading for elevation 0° // ADC reading for elevation 0°
const EL_MIN_READING: f32 = 0.0; const EL_MIN_READING: f32 = 0.0;
// ADC reading for elevation 360° // ADC reading for elevation 360°
const EL_MAX_READING: f32 = 4096.0; const EL_MAX_READING: f32 = 4096.0;
// Range of motion for elevantion (0 to EL_RANGE) // Range of motion for elevantion (0 to EL_RANGE)
const EL_RANGE: f32 = 90.0; const EL_RANGE: f32 = 180.0;
// Tolerance for the elevation setpoint
const EL_SLOP : i16 = 1;
#[embassy_executor::task] #[embassy_executor::task]
pub async fn movement_task( pub async fn movement_task(
@ -105,13 +109,16 @@ pub async fn movement_task(
rotor_state.actual_pos.az = az_actual as u16; rotor_state.actual_pos.az = az_actual as u16;
rotor_state.actual_pos.el = el_actual as u16; rotor_state.actual_pos.el = el_actual as u16;
if !rotor_state.stopped && rotor_state.actual_pos.az < rotor_state.setpoint_pos.az { let delta_az =
rotor_state.setpoint_pos.az as i16 - rotor_state.actual_pos.az as i16;
let delta_el =
rotor_state.setpoint_pos.el as i16 - rotor_state.actual_pos.el as i16;
if !rotor_state.stopped && delta_az > AZ_SLOP {
// Azimuth needs to move clockwise // Azimuth needs to move clockwise
cw_pin.set_high(); cw_pin.set_high();
ccw_pin.set_low(); ccw_pin.set_low();
} else if !rotor_state.stopped } else if !rotor_state.stopped && delta_az < -AZ_SLOP {
&& rotor_state.actual_pos.az > rotor_state.setpoint_pos.az
{
// Azimuth needs to move counter clockwise // Azimuth needs to move counter clockwise
cw_pin.set_low(); cw_pin.set_low();
ccw_pin.set_high(); ccw_pin.set_high();
@ -121,13 +128,11 @@ pub async fn movement_task(
ccw_pin.set_low(); ccw_pin.set_low();
} }
if !rotor_state.stopped && rotor_state.actual_pos.el < rotor_state.setpoint_pos.el { if !rotor_state.stopped && delta_el > EL_SLOP {
// Elevation needs to move up // Elevation needs to move up
up_pin.set_high(); up_pin.set_high();
down_pin.set_low(); down_pin.set_low();
} else if !rotor_state.stopped } else if !rotor_state.stopped && delta_el < -EL_SLOP {
&& rotor_state.actual_pos.el > rotor_state.setpoint_pos.el
{
// Elevation needs to move down // Elevation needs to move down
up_pin.set_low(); up_pin.set_low();
down_pin.set_high(); down_pin.set_high();
@ -155,14 +160,14 @@ pub async fn movement_task(
// Simple sliding average filter // Simple sliding average filter
struct Average { struct Average {
pos: usize, pos: usize,
data: Vec<f32, 10>, data: Vec<f32, 5>,
} }
impl Average { impl Average {
// Create a new filter and prefill the state using an initial value // Create a new filter and prefill the state using an initial value
fn new(initial: f32) -> Average { fn new(initial: f32) -> Average {
let mut data: Vec<f32, 10> = Vec::new(); let mut data: Vec<f32, 5> = Vec::new();
data.resize(10, initial).unwrap(); data.resize(5, initial).unwrap();
Average { pos: 0, data } Average { pos: 0, data }
} }