Pivot

vanttec · Jan 15, 2024 · 9c53199 · 9c53199
1 parent f9b0cdb
commit 9c53199
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git a/src/control/ASMC.cpp b/src/control/ASMC.cpp
@@ -90,6 +90,16 @@ ASMCOutput ASMC::update(const ASMCState &s, const ASMCSetpoint &setpoint) {
   double Tport = (Tx / 2.0) + (Tz / B);
   double Tstbd = (Tx / (2 * c)) - (Tz / (B * c));
 
+  if(setpoint.pivot_enabled == 1){
+    if(Tport > Tstbd){
+      Tstbd = - Tport;
+      Tport/=2;
+    } else {
+      Tport = - Tstbd;
+      Tstbd/=2;
+    }
+  }
+
   Tport = std::clamp(Tport, -30.0, 36.0);
   Tstbd = std::clamp(Tstbd, -30.0, 36.0);
 

diff --git a/src/control/ASMC.h b/src/control/ASMC.h
@@ -5,13 +5,15 @@
 #ifndef USV_ROS2_ASMC_H
 #define USV_ROS2_ASMC_H
 
+#include <iostream>
+
 struct ASMCParams {
   double k_u, k_psi, kmin_u, kmin_psi, k2_u, k2_psi, mu_u, mu_psi, lambda_u,
       lambda_psi;
 };
 
 struct ASMCSetpoint {
-  double heading_setpoint, velocity_setpoint;
+  double heading_setpoint, velocity_setpoint, pivot_enabled;
 };
 
 // TODO Switch over to vanttec::ControllerState