Add limiting

industrial-robotics-lab · Mar 28, 2024 · 576bea0 · 576bea0
1 parent b14f911
commit 576bea0
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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
@@ -1 +1 @@
-#include "tests/carClassTest.h"
+#include "tests/finalSystem.h"
Original file line number	Diff line number	Diff line change
		@@ -1 +1 @@
		#include "tests/carClassTest.h"
		#include "tests/finalSystem.h"