diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 4ad22f0..430a633 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -1,7 +1,7 @@ use crate::{ filtering::kalman_filter::KalmanFilter, preprocessing::{ - accelerometer::AccelerometerPreprocessor, keyence::KeyenceAgrees, + accelerometer::AccelerometerPreprocessor, keyence::{KeyenceAgrees, SensorChecks}, optical::process_optical_data, }, }; @@ -17,9 +17,8 @@ pub struct Localizer { velocity: f64, acceleration: f64, kalman_filter: KalmanFilter, - current_optical_reading: Option, 2>>, - current_keyence_reading: Option>, - current_accelerometer_reading: Option>, + accelerometer_preprocessor: AccelerometerPreprocessor, + keyence_checker: KeyenceAgrees, } impl Localizer { @@ -65,6 +64,8 @@ impl Localizer { velocity: 0.0, acceleration: 0.0, kalman_filter, + accelerometer_preprocessor: AccelerometerPreprocessor::new(), + keyence_checker: KeyenceAgrees::new(), } } @@ -96,10 +97,26 @@ impl Localizer { self.acceleration } - //pub fn preproccesor( - // &mut self, - // optical_data: Vec, 2>, - // keyence_data: Vec, - // accelerometer_data: Vec, - //) + pub fn preprocessor( + &mut self, + optical_data: Vec, 2>, + keyence_data: Vec, + accelerometer_data: Vec, + ) { + + let processed_optical_data = process_optical_data(optical_data); + + let keyence_status = self.keyence_checker.check_keyence_agrees(keyence_data.clone()); + //if keyence_status == SensorChecks::Unacceptable { + //TODOLater: Change state + // return; + //} + + + + + + + } + }