From 9fcadcde5452d29833cd9c4ed69f26e7ae9a1712 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@tutanota.com>
Date: Thu, 26 Sep 2024 23:15:08 +0200
Subject: [PATCH] leave other non-working examples in the code

---
 control_act.m | 21 ++++++++++++++++++++-
 tesi.m        |  4 ++--
 2 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/control_act.m b/control_act.m
index 1ff786b..2451471 100644
--- a/control_act.m
+++ b/control_act.m
@@ -32,9 +32,28 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         T_inv = decouple_matrix(q_act, sim_data);
         ut = utrack(t, q_act, sim_data);
         
-        % minimize v
+        % minimize v, unicycle and ddr
+        % actually minimize xdot^2 + ydot^2 = v
         H = 2*eye(2);
 
+        % minimize vcorr^2 + wcorr^2, unicycle and ddr
+        %H = T_inv
+        
+        if eq(sim_data.robot, 0)
+            % ex3: unicycle, minimize v
+            %H = T_inv' * [1 0; 0 0] * T_inv;
+            % ex4: unicycle, minimize w
+            %H = T_inv' * [0 0; 0 1] * T_inv;
+        else
+            % ex1: ddr, minimize v. det(H) = 0 H not symmetric
+            %R = [sim_data.r/2 sim_data.r/2]
+            %H = T_inv' * R' * R * T_inv;
+
+            % ex2: ddr, minimize w. det(H) = 0
+            %R = [sim_data.r/sim_data.d -sim_data.r/sim_data.d]
+            %H = T_inv' * R' * R * T_inv;
+        end
+
         f = zeros(2,1);
         A = [T_inv; -T_inv];        
         %A = [eye(2); -eye(2)];
diff --git a/tesi.m b/tesi.m
index 54c551b..af18749 100644
--- a/tesi.m
+++ b/tesi.m
@@ -3,7 +3,7 @@ clear all
 close all
 
 % options
-ROBOT = 'diffdrive'
+ROBOT = 'unicycle'
 %TESTS = ["straightline/chill", "straightline/chill_errortheta_pisixths", "straightline/toofast", "straightline/chill_errory", "circle/start_center", "figure8/chill", "figure8/toofast", "square"]
 TESTS = ["circle/start_center"]
 
@@ -36,7 +36,7 @@ for i = 1:length(TESTS)
     % 1: track only
     % 2: track + 1step
     % 3: track + multistep
-    spmd (3)
+    spmd (2)
         worker_index = spmdIndex;
         % load controller-specific options
         data = load(['tests/' num2str(worker_index) '.mat']);