Diff: DifferentialDriveKinematics.h

Revision:

4:067fefe01204

Parent:

3:7b00b653cbce

diff -r 7b00b653cbce -r 067fefe01204 DifferentialDriveKinematics.h
--- a/DifferentialDriveKinematics.h	Sat Apr 18 00:57:09 2020 +0000
+++ b/DifferentialDriveKinematics.h	Thu Apr 30 00:02:07 2020 +0000
@@ -1,58 +1,117 @@
 #ifndef DIFFERENTIAL_DRIVE_KINEMATICS_H
 #define DIFFERENTIAL_DRIVE_KINEMATICS_H
 
+// STL
 #include<sstream>
 #include<string>
 
-#include "Matrix.h"
-#include "Motor.h"
+// EIGEN
+#include <Eigen/Dense.h>
+
+// MBED IO
+#include "mbed.h"
+#include "motordriver.h"
 #include "HallEffectEncoder.h"
+#include <Servo.h>
 #include <SDFileSystem.h>
+#include "LSM9DS1.h"
+
+//TYPEDEFS 
+typedef float Path[100][3] ;
+
+
+//WIP own class
+//LOW POSER SERVO VARIANT using mosfet to power up and down
+class ServoLP{
+public:
+    ServoLP(PinName pin_servo, PinName pin_mosfet):
+        servo(pin_servo),
+        mosfet(pin_mosfet)
+    {
+        
+    }    
+
+    void power_and_set (float percent){
+        mosfet = 1;
+        servo = percent;
+    }
+    
+    void shutdown (){
+        mosfet = 0;
+    }
+
+private:
+    Servo servo; 
+    DigitalOut mosfet;   
+    
+};
+
+
+
+
 
 
 
 class DifferentialDriveKinematics {
 public:
+
+    // CTOR
     DifferentialDriveKinematics(float x, float y, float theta, Motor left_motor, Motor right_motor, 
                                 // PinName pin_enc_l, PinName pin_enc_r, float encoder_speed_period ,
                                 HallEffectEncoder& enc_l_, HallEffectEncoder& enc_r_,
+                                ServoLP& penservo_,
+                                LSM9DS1& imu_,
                                 float Ts, float controller_precision);
 
     
     
-    // Passive functions, used to get speed, angle, and other stuff
-    void updateStates(float speed_left, float speed_right);
+    /* ===Passive functions=== */
+    void dynamicsWheels(float speed_left  , float speed_right  , float previous_states[3], float updated_states[3]);
+    void dynamicsIMU   (float linear_speed, float angular_speed, float previous_states[3], float updated_states[3]);
+    void updateStates  (float speed_left, float speed_right);
+    
+    
+    void updateAngleStateIMU(float angular_vel, float dt);
     
+    void updatePositionWheels(float speed_left, float speed_right, float dt);
+    
+    
+    //IMU
+    //float getLinearSpeedFromIMU();
+    //float getAngularSpeedFromIMU();
+    
+    // Helpers
+    float getDifferenceAngle(float angle1, float angle2);
     float getAngleFromTarget(float x_target, float y_target);
-        
-    float controlAngle(float angle, float target_angle);
+    bool  getWheelSpeedFromLinearAngularSpeed(float linear_speed, float angular_speed, float* wheel_speeds);
     
-    bool getWheelSpeedFromLinearAngularSpeed(float linear_speed, float angular_speed, float* wheel_speeds);
-    
+    //Speed Profiles
     float getPWMFromSpeedRight(float speed);
     float getPWMFromSpeedLeft(float speed);
     
+    float getPWMFromSpeedRightPos(float speed);
+    float getPWMFromSpeedLeftPos(float speed);
+    float getPWMFromSpeedRightNeg(float speed);
+    float getPWMFromSpeedLeftNeg(float speed);
+    
+
+    //KALMAN
+    void kalmanUpdate(float observed_states[3], float predicted_states[3], FILE* fp, bool imu );
+    void kalmanPredict(float speed_left, float speed_right, float predicted_states[3], bool imu );
     
     //Logger
-    void init_logger(RawSerial* pc_);
+    void init_logger(Serial* pc_);
     void log();
-    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 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
-    
-    void goToAngle(float target_angle, FILE* fp);
+    std::string getLog(float angular_vel, float Vr, float Vl, float real_speed_left, float real_speed_right, float diff_angle, float target_angle);
     
     
-    // Does not follow path smoothly
-    bool followPathV2(float path[100][2], int length_array, RawSerial* pc, FILE* fp);
+    //===Active functions===
+
+    bool followPathV4(Path path, int length_array, Serial* pc, FILE* fp);
+    
+    bool followPathV5(Path path, int length_array, Serial* pc, FILE* fp);
         
-    
+    //Accessors
     float getX();
     float getY();
     float getTheta();
@@ -62,59 +121,72 @@
 
 
 private:
-
-    //Logger
+    /* ===IO=== */
+    //Motors
+    Motor m_r;
+    Motor m_l;
+    
+    //Encoders
+    HallEffectEncoder& enc_l;
+    HallEffectEncoder& enc_r;
+    
+    /* PenServo */
+    ServoLP& penservo;
+    
+    /* IMU */
+    LSM9DS1 imu;
+    
+    /* Logger */
     Ticker log_ticker;
-    // RawSerial* pc;
 
-
+    //Kalman State
     float _x;
     float _y;
     float _theta;
     float Ts;
     
-    // the motors
-    Motor m_r;
-    Motor m_l;
+    //Body characteristics
+    float R;    // Radius of the wheel
+    float L;    // length between the wheels
     
-    // the wheel encoders
-    HallEffectEncoder& enc_l;
-    HallEffectEncoder& enc_r;
-    
-    float R; // Radius of the wheel
-    float L; // length between the wheels
-    
-    // gains used to control the angle of the robot
-    float Kp;
+    // Gains used to control the angle of the robot
+    float Kp;   
     float Ki;
     float Kd;
     
-    // errors used to control the angle of the robot
+    // Errors used to control the angle of the robot
     float previous_error;
     float steady_error;
     
-    // used for the controller
+    // uUed for the controller
     float precision;
+        
+
+    /* ===Kalman State=== */
+    // Var State mat
+    Eigen::Matrix3f Pk;
+    
+    //Transitions Matrices
+    Eigen::Matrix3f Fk;
+    Eigen::Matrix3f Hk; // Hk is I_3x3 for both measures
+    Eigen::Matrix3f Qk;
+    
+    Eigen::Matrix3f Rk_odo; //Odometry meaure uncertainty
+    Eigen::Matrix3f Rk_imu; //IMU      meaure uncertainty
     
     
-    // this is used to remove the accoup at the beginning
-    bool isItFirstExecControlAngle;
+    // CONSTANTS (WIP static since non-c++11 prohibit non static init) could be in a struct and not HARDCODED
+    static const float A_L_Pos = 66.19920199;
+    static const float B_L_Pos = -6.466423047830933;
+    static const float A_R_Pos = 57.79291202;
+    static const float B_R_Pos = -5.18994185761958;
     
-    // those are used for Kalman
-    Matrix Pk;
-    Matrix Fk;
-    Matrix Hk; // Hk is I33
-    Matrix Qk;
-    Matrix Rk;
+    static const float A_L_Neg = 65.71910777;
+    static const float B_L_Neg = 6.389965540983612;
+    static const float A_R_Neg = 59.69554934;
+    static const float B_R_Neg = 5.7779939016393485;
     
     
-    
-    // const for the motor profiles
-    const float A_L = 65.52336318;
-    const float B_L = -4.768421976470584;
-    const float A_R = 55.72711016;
-    const float B_R = -4.212873788235296;
-    
 };