From 576bea0dbf46e54c403ada4e1d608ad960502039 Mon Sep 17 00:00:00 2001
From: TexnoMan <antipov.vl.a@yandex.ru>
Date: Thu, 28 Mar 2024 14:29:04 +0300
Subject: [PATCH] Add limiting

---
 lib/Car/Car.cpp                             | 10 +++++++++-
 lib/SerialTransceiver/SerialTransceiver.cpp |  3 ---
 lib/SerialTransceiver/SerialTransceiver.h   |  4 ----
 src/main.cpp                                |  2 +-
 4 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/lib/Car/Car.cpp b/lib/Car/Car.cpp
index 09b39cf..694db91 100644
--- a/lib/Car/Car.cpp
+++ b/lib/Car/Car.cpp
@@ -29,7 +29,7 @@ Car::Car(
     float ilw = 1.0 / lw;
     Jac = {-lw, 1, -1, lw, 1, 1, lw, 1, -1, -lw, 1, 1};
     Jac /= wheelRadius;
-    invJac = {ilw, -ilw, -ilw, ilw,-1, -1, -1, -1, 1, -1, 1, -1};
+    invJac = {-ilw, ilw, ilw, -ilw, 1, 1, 1, 1, -1, 1, -1, 1};
     invJac *= wheelRadius / 4;
     
     resetOdom();
@@ -69,6 +69,14 @@ void Car::setMotorsPWM(Matrix<WHEELS_COUNT, 1, float> &motorsPWM)
 
 void Car::reachCarVelocity(Matrix<3,1, float> &carVel)
 {
+    if(abs(carVel(0))>0.75){
+        carVel(0) = copysignf(0.5, carVel(0));
+    }
+    for(uint8_t i = 1; i<3; i++){
+        if(abs(carVel(i))>0.5){
+            carVel(i) = copysignf(0.5, carVel(i));
+        }
+    }
     *jointVelocities = Jac * carVel;
     reachWheelsAngularVelocity(*jointVelocities);
 }
diff --git a/lib/SerialTransceiver/SerialTransceiver.cpp b/lib/SerialTransceiver/SerialTransceiver.cpp
index 21dfa71..9317eed 100644
--- a/lib/SerialTransceiver/SerialTransceiver.cpp
+++ b/lib/SerialTransceiver/SerialTransceiver.cpp
@@ -3,9 +3,6 @@
 SerialTransceiver::SerialTransceiver(Matrix<3> *desiredVelocity, Matrix<4> *jointAngles, Matrix<4> *jointVelocities)
     :desiredVelocity(desiredVelocity), jointAngles(jointAngles), jointVelocities(jointVelocities)
 {
-    maxAngSpeed = 5;
-    maxLinSpeed = 0.85;
-    threshold = 0.0001;
 
     bufferOutSize = (jointVelocities->Rows+jointAngles->Rows)*sizeof(float)+2;
     stateVector = new float[jointVelocities->Rows+jointAngles->Rows]();
diff --git a/lib/SerialTransceiver/SerialTransceiver.h b/lib/SerialTransceiver/SerialTransceiver.h
index a8de510..1a72cf0 100644
--- a/lib/SerialTransceiver/SerialTransceiver.h
+++ b/lib/SerialTransceiver/SerialTransceiver.h
@@ -9,10 +9,6 @@ using namespace BLA;
 class SerialTransceiver
 {
 private:
-    float maxLinSpeed;
-    float maxAngSpeed;
-    float threshold;
-    float fi, x, y;
 
     Matrix<3> *desiredVelocity;
     Matrix<4> *jointAngles;
diff --git a/src/main.cpp b/src/main.cpp
index d003aae..122bd3b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1 +1 @@
-#include "tests/carClassTest.h"
\ No newline at end of file
+#include "tests/finalSystem.h"
\ No newline at end of file