Clean up

Author: knoellle

crates/control/src/fall_state_detection.rs

@@ -1,4 +1,8 @@
-use std::{collections::VecDeque, time::Duration};
+use core::panic;
+use std::{
+    collections::VecDeque,
+    time::{Duration, SystemTime},
+};
 
 use color_eyre::Result;
 use coordinate_systems::{Field, Robot};
@@ -11,7 +15,10 @@ use context_attribute::context;
 use framework::{deserialize_not_implemented, MainOutput};
 use tflite::{ops::builtin::BuiltinOpResolver, FlatBufferModel, InterpreterBuilder};
 use types::{
-    cycle_time::CycleTime, fall_state::FallState, joints::Joints, sensor_data::SensorData,
+    cycle_time::CycleTime,
+    fall_state::{Direction, FallState, Kind, Side},
+    joints::Joints,
+    sensor_data::SensorData,
 };
 
 #[derive(Deserialize, Serialize)]
@@ -55,7 +62,7 @@ pub struct CycleContext {
 #[context]
 #[derive(Default)]
 pub struct MainOutputs {
-    pub fall_state: MainOutput<FallState>,
+    pub fall_state_tinyml: MainOutput<FallState>,
 }
 
 impl FallStateDetection {
@@ -122,7 +129,6 @@ impl FallStateDetection {
         interpreter.invoke()?;
 
         let output: &[f32] = interpreter.tensor_data(output_index)?;
-        dbg!(output);
 
         let guess = output
             .iter()
@@ -131,10 +137,20 @@ impl FallStateDetection {
             .unwrap()
             .0;
 
-        dbg!(guess);
+        let fall_state = match guess {
+            0 => FallState::Upright,
+            1 => FallState::Falling {
+                start_time: SystemTime::UNIX_EPOCH,
+                direction: Direction::Forward { side: Side::Left },
+            },
+            2 => FallState::Fallen {
+                kind: Kind::FacingUp,
+            },
+            _ => panic!(),
+        };
 
         Ok(MainOutputs {
-            fall_state: FallState::default().into(),
+            fall_state_tinyml: fall_state.into(),
         })
     }
 }