Diff: DifferentialDriveKinematics.h

Revision:

3:7b00b653cbce

Parent:

2:baa00f631c7e

Child:

4:067fefe01204

--- a/DifferentialDriveKinematics.h	Tue Apr 14 19:30:37 2020 +0000
+++ b/DifferentialDriveKinematics.h	Sat Apr 18 00:57:09 2020 +0000
@@ -7,6 +7,7 @@
 #include "Matrix.h"
 #include "Motor.h"
 #include "HallEffectEncoder.h"
+#include <SDFileSystem.h>
 
 
 
@@ -23,9 +24,7 @@
     void updateStates(float speed_left, float speed_right);
     
     float getAngleFromTarget(float x_target, float y_target);
-    
-    float getSpeedFromTarget(float x_target, float y_target);
-    
+        
     float controlAngle(float angle, float target_angle);
     
     bool getWheelSpeedFromLinearAngularSpeed(float linear_speed, float angular_speed, float* wheel_speeds);
@@ -37,19 +36,22 @@
     //Logger
     void init_logger(RawSerial* pc_);
     void log();
-    std::string getLog();
+    std::string getLog(float angular_vel, float Vr, float Vl, float real_speed_left, float real_speed_right);
     
     // active functions, like follow path and stuff
-    bool goToAngle(float angle);
     
-    bool followPath(float path[100][2], int length_array, RawSerial* pc);
+    bool followPath(float path[100][2], int length_array, RawSerial* pc, FILE* fp);
     
     bool goToStart(float start_x, float start_y, float next_target_x, float next_target_y);
     
     void kalmanPredictUpdate(float speed_left, float speed_right); // the fuction will only rely the attributes of the class
     
-    float getPwmFromMotorSpeed(float speed_motor);
+    void goToAngle(float target_angle, FILE* fp);
+    
     
+    // Does not follow path smoothly
+    bool followPathV2(float path[100][2], int length_array, RawSerial* pc, FILE* fp);
+        
     
     float getX();
     float getY();
@@ -108,10 +110,10 @@
     
     
     // const for the motor profiles
-    const float A_L = 80.95987857;
-    const float B_L = -4.598798777003484;
-    const float A_R = 68.53560796;
-    const float B_R = -3.283267993031359;
+    const float A_L = 65.52336318;
+    const float B_L = -4.768421976470584;
+    const float A_R = 55.72711016;
+    const float B_R = -4.212873788235296;
     
 };