From e4e77aa3091e107f73d2ab064dc40ec37b6f0cc2 Mon Sep 17 00:00:00 2001
From: Fyor Klein Gunnewiek <fyorkg@gmail.com>
Date: Sun, 25 Jun 2023 17:34:30 +0200
Subject: [PATCH] Introduce `Calibration` state (#87)

Co-authored-by: Gijs de Jong <berichtaangijs@gmail.com>
---
 crates/control/src/behavior/calibrate.rs         | 11 +++++++++++
 crates/control/src/behavior/mod.rs               |  1 +
 crates/control/src/behavior/node.rs              |  3 +++
 crates/control/src/localization.rs               |  4 ++++
 crates/types/src/action.rs                       |  1 +
 etc/configuration/head.P0000074A03S83I00027.json | 10 ++++++++--
 etc/configuration/head.P0000074A04S8C700011.json | 10 ++++++++--
 7 files changed, 36 insertions(+), 4 deletions(-)
 create mode 100644 crates/control/src/behavior/calibrate.rs

diff --git a/crates/control/src/behavior/calibrate.rs b/crates/control/src/behavior/calibrate.rs
new file mode 100644
index 00000000..ac9de636
--- /dev/null
+++ b/crates/control/src/behavior/calibrate.rs
@@ -0,0 +1,11 @@
+use types::{MotionCommand, PrimaryState, WorldState};
+
+pub fn execute(world_state: &WorldState) -> Option<MotionCommand> {
+    match world_state.robot.primary_state {
+        PrimaryState::Calibration => Some(MotionCommand::Stand {
+            head: types::HeadMotion::ZeroAngles,
+            is_energy_saving: false,
+        }),
+        _ => None,
+    }
+}
diff --git a/crates/control/src/behavior/mod.rs b/crates/control/src/behavior/mod.rs
index bda01961..b467e410 100644
--- a/crates/control/src/behavior/mod.rs
+++ b/crates/control/src/behavior/mod.rs
@@ -1,3 +1,4 @@
+mod calibrate;
 mod defend;
 mod dribble;
 mod fall_safely;
diff --git a/crates/control/src/behavior/node.rs b/crates/control/src/behavior/node.rs
index e7c00eff..733d6be7 100644
--- a/crates/control/src/behavior/node.rs
+++ b/crates/control/src/behavior/node.rs
@@ -12,6 +12,7 @@ use types::{
 };
 
 use super::{
+    calibrate,
     defend::Defend,
     dribble, fall_safely,
     head::LookAction,
@@ -97,6 +98,7 @@ impl Behavior {
             Action::FallSafely,
             Action::StandUp,
             Action::Stand,
+            Action::Calibrate,
         ];
 
         if let Some(active_since) = self.active_since {
@@ -184,6 +186,7 @@ impl Behavior {
                     Action::StandUp => stand_up::execute(world_state),
                     Action::Stand => stand::execute(world_state, context.field_dimensions),
                     Action::LookAround => look_around::execute(world_state),
+                    Action::Calibrate => calibrate::execute(world_state),
                     Action::DefendGoal => defend.goal(&mut context.path_obstacles),
                     Action::DefendKickOff => defend.kick_off(&mut context.path_obstacles),
                     Action::DefendLeft => defend.left(&mut context.path_obstacles),
diff --git a/crates/control/src/localization.rs b/crates/control/src/localization.rs
index faccab84..27df31ff 100644
--- a/crates/control/src/localization.rs
+++ b/crates/control/src/localization.rs
@@ -529,6 +529,10 @@ impl Localization {
                 );
                 Some(*context.robot_to_field)
             }
+            (PrimaryState::Calibration, _) => {
+                *context.robot_to_field = Isometry2::identity();
+                Some(*context.robot_to_field)
+            }
             _ => None,
         };
         Ok(MainOutputs {
diff --git a/crates/types/src/action.rs b/crates/types/src/action.rs
index 334650fa..e01db783 100644
--- a/crates/types/src/action.rs
+++ b/crates/types/src/action.rs
@@ -11,6 +11,7 @@ pub enum Action {
     StandUp,
     Stand,
     LookAround,
+    Calibrate,
     Dribble,
     DefendGoal,
     DefendKickOff,
diff --git a/etc/configuration/head.P0000074A03S83I00027.json b/etc/configuration/head.P0000074A03S83I00027.json
index f4b1e83b..ea3d0502 100644
--- a/etc/configuration/head.P0000074A03S83I00027.json
+++ b/etc/configuration/head.P0000074A03S83I00027.json
@@ -1,4 +1,10 @@
 {
-  "disable_communication_acceptor": true,
-  "player_number": "Two"
+  "camera_matrix_parameters": {
+    "vision_top": {
+      "extrinsic_rotations": [
+        0.7900000214576721, -3.119999885559082, 0.7400000095367432
+      ]
+    }
+  },
+  "player_number": "One"
 }
diff --git a/etc/configuration/head.P0000074A04S8C700011.json b/etc/configuration/head.P0000074A04S8C700011.json
index 9c5f9e38..54005541 100644
--- a/etc/configuration/head.P0000074A04S8C700011.json
+++ b/etc/configuration/head.P0000074A04S8C700011.json
@@ -1,4 +1,10 @@
 {
-  "disable_communication_acceptor": true,
-  "player_number": "Three"
+  "camera_matrix_parameters": {
+    "vision_top": {
+      "extrinsic_rotations": [
+        0.03999999910593033, -3.009999990463257, 1.2999999523162842
+      ]
+    }
+  },
+  "player_number": "Two"
 }