Turned on pose estimation

README.md

@@ -11,7 +11,7 @@ Chosen configuration has only 4 free interrupt pins (other ones are used or unre
 # Structure
 The project structure consists of two blocks: car control (`Car`) and serial communication (`SerialTransceiver`).
 
-`Car` controls 4 wheels with tuned PID-controllers and calculates robot velocity by solving backward kinematics problem and robot position by solving forward kinematics problem (dedicated theory described in [Modern Robotics](http://hades.mech.northwestern.edu/images/7/7f/MR.pdf) book, p.519).
+`Car` controls 4 wheels with tuned PID-controllers and calculates robot velocity by solving inverse kinematics problem and robot position by solving forward kinematics problem (dedicated theory described in [Modern Robotics](http://hades.mech.northwestern.edu/images/7/7f/MR.pdf) book, p.519).
 
 `SerialTransceiver` receives control vector from Raspberry Pi (3 bytes) and transmits robot position in global coordinate system (12 bytes). It uses CRC8 algorithm to check message integrity.
 

lib/Car/Car.cpp

@@ -7,10 +7,9 @@ Car::Car(
     unsigned int wheelPeriod,
     Matrix<3> *desiredVelocity,
     Matrix<3> *feedbackPose)
-    : 
-    period(wheelPeriod), 
-    desiredCarVelocity(desiredVelocity),
-    feedbackCarPose(feedbackPose)
+    : period(wheelPeriod),
+      desiredCarVelocity(desiredVelocity),
+      feedbackCarPose(feedbackPose)
 {
     G = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
     w1 = new Wheel(1, 18, 19, false, wheelPeriod);
@@ -46,7 +45,6 @@ void Car::findCarPose()
     Matrix<3> vb = F * wheelsDisplacement;
     Matrix<6> twist = {0, 0, vb(0), vb(1), vb(2), 0};
     Matrix<4, 4> prevToCurrPose = vec6_to_SE3(twist);
-    // *feedbackCarPose = {atan2(prevToCurrPose(1, 0), prevToCurrPose(0, 0)), prevToCurrPose(0, 3), prevToCurrPose(1, 3)};
     G *= prevToCurrPose;
     *feedbackCarPose = {atan2(G(1, 0), G(0, 0)), G(0, 3), G(1, 3)};
 }
@@ -84,7 +82,7 @@ void Car::reachWheelsVelocity(Matrix<4> wheelsVel)
 
 void Car::update()
 {
-    // findCarPose();
+    findCarPose();
     reachCarVelocity(*desiredCarVelocity);
 }
 

lib/Car/Car.h

@@ -22,8 +22,8 @@ class Car
     Matrix<3, 4> F;
     Matrix<4> wheelsDisplacement;
 
-    void findCarPose();
-    void reachCarVelocity(Matrix<3> carVel);
+    void findCarPose(); // forward kinematics
+    void reachCarVelocity(Matrix<3> carVel); // inverse kinematics
     void reachWheelsVelocity(Matrix<4> wheelsVel);
 
 public:

lib/SerialTransceiver/SerialTransceiver.cpp

@@ -28,18 +28,9 @@ void SerialTransceiver::rx()
         calcChecksum = crc8((uint8_t *)controlVec, 3);
         if (readChecksum == calcChecksum)
         {
-            if (controlVec[0] == controlVec[1] == controlVec[2] == 127)
-            {
-                (*desiredVelocity)(0) = 0.0;
-                (*desiredVelocity)(1) = 0.0;
-                (*desiredVelocity)(2) = 0.0;
-            }
-            else
-            {
-                (*desiredVelocity)(0) = mapUint8ToFloat(controlVec[0], -maxAngSpeed, maxAngSpeed);
-                (*desiredVelocity)(1) = mapUint8ToFloat(controlVec[1], -maxLinSpeed, maxLinSpeed);
-                (*desiredVelocity)(2) = mapUint8ToFloat(controlVec[2], -maxLinSpeed, maxLinSpeed);
-            }
+            (*desiredVelocity)(0) = mapUint8ToFloat(controlVec[0], -maxAngSpeed, maxAngSpeed);
+            (*desiredVelocity)(1) = mapUint8ToFloat(controlVec[1], -maxLinSpeed, maxLinSpeed);
+            (*desiredVelocity)(2) = mapUint8ToFloat(controlVec[2], -maxLinSpeed, maxLinSpeed);
             // memcpy(desiredVelocity, vec, 12);
         }
     }